Component.cpp
Go to the documentation of this file.
1 /**
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package control::ArmarXObjects::controller_creator
17  * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
18  * @date 2023
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 
24 #include "Component.h"
25 
26 #include <Eigen/Core>
27 
28 #include <IceUtil/Time.h>
29 
30 #include <VirtualRobot/MathTools.h>
31 #include <VirtualRobot/Nodes/RobotNode.h>
32 #include <VirtualRobot/RobotNodeSet.h>
33 
34 // #include <SimoxUtility/color/Color.h>
35 #include "ArmarXCore/core/time/Metronome.h"
36 #include <ArmarXCore/core/logging/Logging.h>
37 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
38 #include <ArmarXCore/core/time/Clock.h>
39 #include <ArmarXCore/core/time/TimeUtil.h>
40 #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
41 // for kinesthetic teaching
42 #include <ArmarXCore/core/csv/CsvWriter.h>
43 
44 #include <ArmarXGui/interface/StaticPlotterInterface.h>
45 
46 #include <RobotAPI/interface/units/RobotUnit/NjointZeroTorqueController.h>
47 #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
48 #include <RobotAPI/libraries/armem_robot_state/types.h>
49 #include <RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h>
50 #include <RobotAPI/libraries/core/math/TimeSeriesUtils.h>
51 
52 #include <armarx/control/client/ControllerBuilder.h>
53 #include <armarx/control/common/aron_conversions.h>
54 #include <armarx/control/common/ft/FTSensorTrigger.h>
55 #include <armarx/control/common/utils.h>
56 #include <armarx/control/njoint_mp_controller/controller_descriptions.h>
57 
58 //#include <armarx/control/njoint_mp_controller/task_space/aron/aron_conversions.h>
59 #include <armarx/control/common/control_law/aron/CollisionAvoidanceControllerConfig.aron.generated.h>
60 
61 namespace armarx::control::components::controller_creator
62 {
63 
64  const std::string Component::defaultName = "controller_creator";
65 
66  Component::Component()
67  {
68  addPlugin(virtualRobotReader_);
69  }
70 
71  Component::~Component() = default;
72 
73  armarx::PropertyDefinitionsPtr
74  Component::createPropertyDefinitions()
75  {
76  armarx::PropertyDefinitionsPtr def =
77  new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
78 
79  def->optional(properties.robotName, "robotName");
80 
81  def->optional(properties.defaultRobotNodeSetLeft,
82  "default_robotNodeSet_left",
83  "default kinematic chain name of left arm.");
84  def->optional(properties.defaultRobotNodeSetRight,
85  "default_robotNodeSet_right",
86  "default kinematic chain name of right arm.");
87 
88  const std::string defaultName = "";
89  def->component(m_robot_unit, defaultName);
90  //def->component(m_force_torque_observer, properties.ftObserverName);
91  // Publish to a topic (passing the TopicListenerPrx).
92  // def->topic(myTopicListener);
93 
94  // Subscribe to a topic (passing the topic name).
95  // def->topic<PlatformUnitListener>("MyTopic");
96 
97  // Use (and depend on) another component (passing the ComponentInterfacePrx).
98  // def->component(myComponentProxy)
99 
100 
101  // Add a required property. (The component won't start without a value being set.)
102  // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
103 
104  // Add an optional property.
105  // def->optional(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
106  // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
107 
108  return def;
109  }
110 
111  void
112  Component::onInitComponent()
113  {
114  // Topics and properties defined above are automagically registered.
115 
116  // Keep debug observer data until calling `sendDebugObserverBatch()`.
117  // (Requires the armarx::DebugObserverComponentPluginUser.)
118  // setDebugObserverBatchModeEnabled(true);
119  }
120 
121  void
122  Component::onConnectComponent()
123  {
124  // Do things after connecting to topics and components.
125  ARMARX_CHECK_NOT_NULL(m_robot_unit);
126 
127  /* (Requires the armarx::DebugObserverComponentPluginUser.)
128  // Use the debug observer to log data over time.
129  // The data can be viewed in the ObserverView and the LivePlotter.
130  // (Before starting any threads, we don't need to lock mutexes.)
131  {
132  setDebugObserverDatafield("numBoxes", properties.numBoxes);
133  setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
134  sendDebugObserverBatch();
135  }
136  */
137 
138  /* (Requires the armarx::ArVizComponentPluginUser.)
139  // Draw boxes in ArViz.
140  // (Before starting any threads, we don't need to lock mutexes.)
141  drawBoxes(properties, arviz);
142  */
143 
144  /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
145  // Setup the remote GUI.
146  {
147  createRemoteGuiTab();
148  RemoteGui_startRunningTask();
149  }
150  */
151  ARMARX_INFO << properties.robotName;
152  // m_robot = m_virtualRobotReaderPlugin->get().getRobot(properties.robotName);
153  if (!m_robot)
154  {
155  // m_robot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::eStructure);
156  m_robot = addRobot("controller_creator_robot", VirtualRobot::RobotIO::eStructure);
157  }
158  ARMARX_CHECK_NOT_NULL(m_robot);
159  ARMARX_CHECK(synchronizeLocalClone(m_robot));
160 
161  m_rns_l = m_robot->getRobotNodeSet(properties.defaultRobotNodeSetLeft);
162  m_rns_r = m_robot->getRobotNodeSet(properties.defaultRobotNodeSetRight);
163 
164  //m_ft_l.reset(new armarx::control::common::ft::ForceTorqueUtility(m_force_torque_observer, properties.ftSensorNameLeft));
165  //m_ft_r.reset(new armarx::control::common::ft::ForceTorqueUtility(m_force_torque_observer, properties.ftSensorNameRight));
166 
167 
168  /* m_trigger =trigger::FTWristSensorTrigger leftTrigger(leftWristSensor);
169  // FT sensor handling
170  */
171  //trigger::FTWristSensorTrigger rightTrigger(rightWristSensor);
172 
173  // RobotUnitInterfacePrx robotUnit = getControlComponentPlugin().getRobotUnitPlugin().getRobotUnit();
174  // robotUnit->loadLibFromPackage("armarx_control", "libarmarx_control_njoint_controller");
175 
176  ARMARX_IMPORTANT << "controller creator is ready";
177 
178  // TODO add load all default config to memory
179  // armarx::control::common::ControllerTypeNames.to_name()
180  // getControlComponentPlugin().configMemoryWriter().storeDefaultConfig(controlTypeStr, AronDict, name);
181  }
182 
183  void
184  Component::onDisconnectComponent()
185  {
186  }
187 
188  void
189  Component::onExitComponent()
190  {
191  }
192 
193  /*
194  // std::string
195  // Component::createBiKAC(const std::string& namePrefix,
196  // const ::armarx::aron::data::dto::DictPtr& configDict,
197  // bool activate,
198  // bool allowReuse)
199  // {
200  // /// create controllers with controller builder
201  // auto builder = controllerBuilder<control::common::ControllerType::BiKAC>();
202  // builder.withConfig(configDict).allowReuse(allowReuse).daemonize();
203 
204  // std::ostringstream oss;
205  // for (const auto& pair : builder.config().cfg)
206  // {
207  // oss << pair.first;
208  // if (&pair != &(*builder.config().cfg.rbegin()))
209  // {
210  // oss << ",";
211  // }
212  // }
213  // std::string robotNodeSets = oss.str();
214 
215  // if (namePrefix.empty())
216  // {
217  // builder.withNamePrefix(robotNodeSets);
218  // }
219  // else
220  // {
221  // builder.allowReuse(true).withNamePrefix(namePrefix + "_" + robotNodeSets);
222  // }
223 
224  // ARMARX_TRACE;
225  // ARMARX_INFO << "creating controller";
226 
227  // auto ctrlWrapper = builder.create();
228 
229  // ARMARX_TRACE;
230  // ARMARX_CHECK(ctrlWrapper);
231 
232  // const std::string controllerName = builder.getControllerName();
233  // ARMARX_INFO << "using controller name " << controllerName;
234  // ARMARX_TRACE;
235 
236  // Clock::WaitFor(Duration::MilliSeconds(300));
237  // if (allowReuse)
238  // {
239  // ARMARX_INFO << "setting nullspace target: if the default is null use the cfg from the "
240  // "existing controller";
241  // auto ctrlConfig = ctrlWrapper->getConfig().cfg;
242  // for (const auto& pair : builder.config().cfg)
243  // {
244  // if (not pair.second.desiredNullspaceJointAngles.has_value())
245  // {
246  // ctrlWrapper->config.cfg.at(pair.first).desiredNullspaceJointAngles =
247  // ctrlConfig.at(pair.first).desiredNullspaceJointAngles;
248  // }
249  // ARMARX_INFO << "Activating controller ..."
250  // << ctrlWrapper->config.cfg.at(pair.first)
251  // .desiredNullspaceJointAngles.has_value();
252  // }
253  // }
254 
255  // ARMARX_TRACE;
256  // ctrlWrapper->updateConfig();
257  // if (activate)
258  // {
259  // ctrlWrapper->activate();
260  // ARMARX_TRACE;
261  // while (not ctrlWrapper->isActive())
262  // {
263  // Clock::WaitFor(Duration::MilliSeconds(10));
264  // }
265  // ARMARX_INFO << "Controller " << controllerName << " is active";
266  // }
267  // else
268  // {
269  // ARMARX_INFO << "User have to activate " << controllerName
270  // << " controller in your application";
271  // }
272  // return controllerName;
273  // }
274 
275  // void
276  // Component::createKVILImpedanceMPController(
277  // const std::string& controllerNamePrefix,
278  // const std::string& robotNodeSet,
279  // const std::string& configFilename,
280  // bool allowReuse
281  //
282  // // nlohmann::json& cfg,
283  // // const std::string& skill,
284  // // const std::string& kinematic_chain,
285  // // const std::string& control_config_file)
286  // {
287  // /// create controllers with controller builder
288  // auto builder = controllerBuilder<control::common::ControllerType::KVILImpedanceMP>();
289  // builder.withNodeSet(robotNodeSet).withNamePrefix(controllerNamePrefix);
290  //
291  // /// load predefined config from file
292  // {
293  // if (configFilename.empty())
294  // {
295  // const std::string controllerClassName =
296  // control::common::ControllerTypeNames.to_name(control::common::ControllerType::TSMPImp);
297  // const armarx::PackagePath configPath("armarx_control", "controller_config/" + controllerClassName + "/default.json");
298  // ARMARX_IMPORTANT << "Using config `" << configPath.toSystemPath() << "`.";
299  // ARMARX_CHECK(std::filesystem::exists(configPath.toSystemPath()));
300  //
301  // builder.withConfig(configPath.toSystemPath());
302  // }
303  // else
304  // {
305  // builder.withConfig(configFilename);
306  // }
307  //
308  //
309  // /// to be removed
310  // ARMARX_INFO << VAROUT(builder.config().kpImpedance);
311  // ARMARX_INFO << VAROUT(builder.config().kdImpedance);
312  // ARMARX_INFO << VAROUT(builder.config().kpNullspace);
313  // ARMARX_INFO << VAROUT(builder.config().kdNullspace);
314  // ARMARX_INFO << VAROUT(builder.config().desiredPose);
315  // ARMARX_INFO << VAROUT(builder.config().desiredTwist);
316  // if (builder.config().desiredNullspaceJointAngles.has_value())
317  // ARMARX_INFO << VAROUT(builder.config().desiredNullspaceJointAngles.value());
318  // ARMARX_INFO << VAROUT(builder.config().torqueLimit);
319  // ARMARX_INFO << VAROUT(builder.config().qvelFilter);
320  // }
321  //
322  // NJointControllerInterfacePrx controller = m_robot_unit->getNJointController(ctrl_name);
323  // bool createNewController = true;
324  // if (controller)
325  // {
326  // ARMARX_INFO << "controller exists, check config ...";
327  // m_kvil_imp_mp_ctrl = NJointKVILImpedanceMPControllerInterfacePrx::checkedCast(controller);
328  //
329  // if (m_kvil_imp_mp_ctrl->getKinematicChainName() == kinematic_chain)
330  // {
331  // ARMARX_INFO << "requested controller already exists, reuse the current one";
332  // createNewController = false;
333  // }
334  // else
335  // {
336  // if (controller->isControllerActive())
337  // {
338  // controller->deactivateController();
339  // while (controller->isControllerActive())
340  // {
341  // TimeUtil::SleepMS(10);
342  // }
343  // }
344  // controller->deleteController();
345  // TimeUtil::SleepMS(10);
346  // }
347  // }
348  // else
349  // {
350  // ARMARX_INFO << "requested controller doesn't exist, create a new one";
351  // }
352  //
353  //
354  // /// -------------------- acquire controller configurations from json file --------------------
355  // m_rns = m_robot->getRobotNodeSet(kinematic_chain);
356  // int n_joints = (int)m_rns->getJointValues().size();
357  // ARMARX_INFO << VAROUT(n_joints);
358  //
359  // /// -------------------- create new controller or get the existing controller and update the setting --------------------
360  // if (createNewController)
361  // {
362  // ARMARX_IMPORTANT << "Initialize ts admittance controllers for " << skill << " with kinematic chain " << kinematic_chain << " (dim = " << n_joints << ").";
363  // NJointTaskspaceControllerConfigPtr ctrl_cfg = new NJointTaskspaceControllerConfig;
364  // ARMARX_INFO << "start to init config";
365  // ctrl_cfg->nodeSetName = kinematic_chain;
366  // ctrl_cfg->controlParamJsonFile = control_config_file;
367  //
368  // m_kvil_imp_mp_ctrl = NJointKVILImpedanceMPControllerInterfacePrx::checkedCast(
369  // m_robot_unit->createNJointController(
370  // ctrl_class_name,
371  // ctrl_name,
372  // ctrl_cfg));
373  // }
374  // else
375  // {
376  // ARMARX_IMPORTANT << "Use existing controller " << ctrl_name << " for skill " << skill;
377  // m_kvil_imp_mp_ctrl->reconfigureController(control_config_file);
378  //
379  // }
380  // ARMARX_CHECK_NOT_NULL(m_kvil_imp_mp_ctrl);
381  //
382  // m_kvil_imp_mp_ctrl->learnFromCSV(std::vector<std::string>());
383  //
384  // /// -------------------- Activate RT Controller --------------------
385  // p2CStruct p2c_struct;
386  // p2c_struct.curve_kpt_position.setZero();
387  // curve_kpt_position_buffer.reinitAllBuffers(p2c_struct);
388  //
389  // curveInfoStruct curve_info_struct;
390  // curve_info_struct.curve_proj_point.setZero();
391  // curve_info_struct.curve_proj_value = 0.0f;
392  // curve_info_struct.curve_vec.setZero();
393  // curve_proj_info_buffer.reinitAllBuffers(curve_info_struct);
394  //
395  // m_kvil_imp_mp_ctrl->activateController();
396  // usleep(100000);
397  // m_kvil_imp_mp_ctrl->start("all", std::vector<double>(), std::vector<double>(), -1);
398  // while (true)
399  // {
400  // float current_canonical_value = m_kvil_imp_mp_ctrl->getCanVal("null");
401  // if (current_canonical_value < 0.999)
402  // {
403  // break;
404  // }
405  // }
406  // task_running.store(true);
407  //
408  //
409  // /// Wait after the controller has finished until either the goal has been reached or until a timeout is reached
410  // CycleUtil c(1);
411  // auto after_controller_startTime = TimeUtil::GetTime();
412  // // std::vector<float> kpt_position {0, 0, 0};
413  // // Eigen::Vector3f proj_point;
414  // // proj_point.setZero();
415  // // Eigen::Vector3f curve_vec;
416  // // curve_vec.setZero();
417  // ARMARX_INFO << "Starting loop";
418  // kpt_controller_running.store(true);
419  // while ((TimeUtil::GetTime() - after_controller_startTime).toSecondsDouble() < 20.0)
420  // {
421  // float current_canonical_value = static_cast<float>(m_kvil_imp_mp_ctrl->getCanVal("null"));
422  // ARMARX_INFO << VAROUT(current_canonical_value);
423  // curve_kpt_position_buffer.getWriteBuffer().curve_kpt_position = m_kvil_imp_mp_ctrl->getCurveKeypointPosition();
424  // curve_kpt_position_buffer.commitWrite();
425  //
426  // auto curve_info = curve_proj_info_buffer.getUpToDateReadBuffer();
427  // m_kvil_imp_mp_ctrl->setCurveProjection(curve_info.curve_proj_point, curve_info.curve_proj_value, curve_info.curve_vec);
428  //
429  // c.waitForCycleDuration();
430  // }
431  // kpt_controller_running.store(false);
432  // if (stop_with_mp)
433  // {
434  // enableVelocityMode();
435  // m_ts_adm_mp_ctrl->stop("default");
436  // ARMARX_INFO << "deativate controller";
437  // m_ts_adm_mp_ctrl->deactivateController();
438  // task_running.store(false);
439  // }
440  //
441  // // CycleUtil c(1);
442  // // while (!CALL_INTERUPTED)
443  // // {
444  // // c.waitForCycleDuration();
445  // // }
446  //
447  // // ARMARX_INFO << "interupted ... (deativate controller)";
448  // // enableVelocityMode();
449  // // m_kvil_imp_mp_ctrl->deactivateController();
450  // // task_running.store(false);
451  // ARMARX_IMPORTANT << "Finished Execution in MotionControl component";
452  // }
453  */
454 
455  template <control::common::ControllerType T>
456  std::string
457  Component::createComplianceController(const std::string& namePrefix,
458  const aron::data::dto::DictPtr& configDict,
459  const std::string& configFilename,
460  bool flagActivate,
461  bool flagAllowReuse,
462  bool flagFromMemory)
463  {
464  auto builder = controllerBuilder<T>();
465  auto ctrlWrapper = builder.createTSComplianceCtrl(namePrefix, configDict);
466  const std::string controllerName = builder.getControllerName();
467  return controllerName;
468  }
469 
470  std::string
471  Component::createController(const std::string& namePrefix,
472  const std::string& controllerType,
473  const ::armarx::aron::data::dto::DictPtr& configDict,
474  const std::string& configFilename,
475  bool activate,
476  bool allowReuse,
477  bool cfgFromMemory,
478  const Ice::Current&)
479  {
480  ARMARX_INFO << "creating controller of type " << controllerType;
481 
482  std::string controllerName;
483 
484  using T = control::common::ControllerType;
485 
486  const auto type = control::common::ControllerTypeShort.from_name(controllerType);
487 
488  switch (type)
489  {
490  /// ------------------------------------ controllers -----------------------------------
491  case T::TSAdm:
492  controllerName = createComplianceController<T::TSAdm>(
493  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
494  break;
495  case T::TSImp:
496  controllerName = createComplianceController<T::TSImp>(
497  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
498  break;
499  case T::TSImpCol:
500  controllerName = createComplianceController<T::TSImpCol>(
501  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
502  break;
503  case T::TSMixImpVelCol:
504  controllerName = createComplianceController<T::TSMixImpVelCol>(
505  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
506  break;
507  case T::TSMixImpVel:
508  controllerName = createComplianceController<T::TSMixImpVel>(
509  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
510  break;
511  case T::TSVel:
512  controllerName = createComplianceController<T::TSVel>(
513  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
514  break;
515  /// ----------------------------------- MP controllers ---------------------------------
516  case T::TSMPAdm:
517  controllerName = createComplianceController<T::TSMPAdm>(
518  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
519  break;
520  case T::TSMPImp:
521  controllerName = createComplianceController<T::TSMPImp>(
522  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
523  break;
524  case T::TSMPImpCol:
525  controllerName = createComplianceController<T::TSMPImpCol>(
526  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
527  break;
528  case T::TSMPMixImpVel:
529  controllerName = createComplianceController<T::TSMPMixImpVel>(
530  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
531  break;
532  case T::TSMPVel:
533  controllerName = createComplianceController<T::TSMPVel>(
534  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
535  break;
536  case T::TSMPMixImpVelCol:
537  controllerName = createComplianceController<T::TSMPMixImpVelCol>(
538  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
539  break;
540  case T::TSMPMixImpVelColWiping:
541  controllerName = createComplianceController<T::TSMPMixImpVelColWiping>(
542  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
543  break;
544  case T::TSMPVelWiping:
545  controllerName = createComplianceController<T::TSMPVelWiping>(
546  namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
547  break;
548  // case T::BiKAC:
549  // controllerName = createBiKAC(namePrefix, configDict, activate, allowReuse);
550  // break;
551 
552  default:
553  ARMARX_INFO << "controller type: " << controllerType << " is not supported yet";
554  }
555  return controllerName;
556  }
557 
558  /*
559  // std::string Component::bothArmToggleZeroTorque(const Ice::Current&)
560  // {
561  // /// ---------------------------------- creating zero torque controller ----------------------------------
562  // ARMARX_INFO << "Initialize zero torque controllers for both arms";
563  // const std::string ctrl_class_name = "NJointZeroTorqueController";
564  // const std::string ctrl_name_left = getDefaultName() + ctrl_class_name + "_left";
565  // const std::string ctrl_name_right = getDefaultName() + ctrl_class_name + "_right";
566  //
567  // NJointZeroTorqueControllerConfigPtr config(new NJointZeroTorqueControllerConfig());
568  // config->maxTorque = 0;
569  //
570  // // left
571  // {
572  // auto nodeSet = m_robot->getRobotNodeSet(kinematicChainName);
573  // for (auto& name : nodeSet->getNodeNames())
574  // {
575  // config->jointNames.push_back(name);
576  // }
577  // NJointZeroTorqueControllerInterfacePrx zeroTorqueController
578  // = NJointZeroTorqueControllerInterfacePrx::checkedCast(
579  // m_robot_unit->createNJointController(
580  // ctrl_class_name,
581  // ctrl_name,
582  // config));
583  // ARMARX_CHECK_NOT_NULL(zeroTorqueController);
584  // ARMARX_INFO << "zero torque controller created";
585  // }
586  //
587  //
588  // float motionThresholdRadian = 0.1f;
589  // float noMotionTimeoutMs = 2000.0f;
590  //
591  // VirtualRobot::RobotNodeSetPtr rns = m_robot->getRobotNodeSet(kinematicChainName);
592  // std::vector<std::string> jointNames = rns->getNodeNames();
593  //
594  // IceUtil::Time start = TimeUtil::GetTime();
595  // IceUtil::Time lastMotionTime = start;
596  // bool initialMotionDetected = false;
597  //
598  // int sampleMs = 10;
599  //
600  // float forceSpike = 0.0f;
601  // Eigen::Vector3f initForce;
602  // if (side == "right")
603  // {
604  // initForce = m_ft_r->getForce();
605  // }
606  // else
607  // {
608  // initForce = m_ft_l->getForce();
609  // }
610  // ARMARX_IMPORTANT << "=============== Waiting for force spike ===============";
611  // while (forceSpike < 10.0f)
612  // {
613  // Eigen::Vector3f force;
614  // if (side == "right")
615  // {
616  // force = m_ft_r->getForce();
617  // }
618  // else
619  // {
620  // force = m_ft_l->getForce();
621  // }
622  // forceSpike = (force - initForce).norm();
623  // }
624  // ARMARX_INFO << "Start the demonstration ...";
625  //
626  // zeroTorqueController->activateController();
627  // while (true) // stop run function if returning true
628  // {
629  // synchronizeLocalClone(m_robot);
630  //
631  // IceUtil::Time now = TimeUtil::GetTime();
632  //
633  // Eigen::VectorXf jve2 = rns->getJointValuesEigen();
634  // if ((jve2 - jve).norm() > motionThresholdRadian)
635  // {
636  // if (!initialMotionDetected)
637  // {
638  // ARMARX_IMPORTANT << "Start recording";
639  // motionStartIndex = 0;
640  // //start = now;
641  // }
642  // jve = jve2;
643  // initialMotionDetected = true;
644  // lastMotionTime = now;
645  // }
646  //
647  // double t = (now - start).toSecondsDouble();
648  //
649  // std::vector<float> jvs = rns->getJointValues();
650  // for (size_t i = 0; i < jvs.size(); i++)
651  // {
652  // jointData.at(i).push_back(jvs.at(i));
653  // }
654  //
655  // Eigen::Matrix4f tcpPose = rns->getTCP()->getPoseInRootFrame();
656  // cartData.at(0).push_back(tcpPose(0, 3));
657  // cartData.at(1).push_back(tcpPose(1, 3));
658  // cartData.at(2).push_back(tcpPose(2, 3));
659  // VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(tcpPose);
660  //
661  // // check flipped orientation quaternion
662  // double dotProduct = quat.w * oldquatE.w + quat.x * oldquatE.x + quat.y * oldquatE.y + quat.z * oldquatE.z;
663  // if (dotProduct < 0)
664  // {
665  // cartData.at(3).push_back(-quat.w);
666  // cartData.at(4).push_back(-quat.x);
667  // cartData.at(5).push_back(-quat.y);
668  // cartData.at(6).push_back(-quat.z);
669  // oldquatE.w = -quat.w;
670  // oldquatE.x = -quat.x;
671  // oldquatE.y = -quat.y;
672  // oldquatE.z = -quat.z;
673  //
674  // }
675  // else
676  // {
677  // cartData.at(3).push_back(quat.w);
678  // cartData.at(4).push_back(quat.x);
679  // cartData.at(5).push_back(quat.y);
680  // cartData.at(6).push_back(quat.z);
681  //
682  // oldquatE.w = quat.w;
683  // oldquatE.x = quat.x;
684  // oldquatE.y = quat.y;
685  // oldquatE.z = quat.z;
686  // }
687  //
688  // timestamps.push_back(t);
689  //
690  // if (initialMotionDetected && (now - lastMotionTime).toMilliSeconds() > noMotionTimeoutMs)
691  // {
692  // ARMARX_IMPORTANT << "Stop recording";
693  // motionEndIndex = timestamps.size();// - noMotionTimeoutMs / sampleMs;
694  // break;
695  // }
696  //
697  // TimeUtil::USleep(sampleMs * 1000);
698  // }
699  // ARMARX_IMPORTANT << "Demonstration done! Processing data ...";
700  //
701  // zeroTorqueController->deactivateController();
702  // while (zeroTorqueController->isControllerActive())
703  // {
704  // usleep(10000);
705  // }
706  // zeroTorqueController->deleteController();
707  // enableVelocityMode();
708  // }
709  */
710 
711 
712  std::string
713  Component::kinestheticTeaching(const std::string& kinematicChainName,
714  const std::string& side,
715  const std::string& fileName,
716  const std::string& nameSuffix,
717  const Ice::Current&)
718  {
719  /// ---------------------------------- creating zero torque controller ----------------------------------
720  ARMARX_INFO << "Kinesthetic teaching: Initialize zero torque controllers for "
721  << VAROUT(kinematicChainName);
722  const std::string ctrl_class_name = "NJointZeroTorqueController";
723  const std::string ctrl_name = "kinesthetic_teaching_" + ctrl_class_name + "_" + nameSuffix;
724 
725  armem::robot_state::RobotReader::Hand hand = armem::robot_state::RobotReader::Hand::Left;
726  ARMARX_IMPORTANT << "THIS kinematicChainName is " << kinematicChainName;
727  if (kinematicChainName == "RightArm" || kinematicChainName == "RightArmWithoutWrist")
728  {
729  hand = armem::robot_state::RobotReader::Hand::Right;
730  }
731 
732  auto& robotReader = virtualRobotReader_->get();
733  ARMARX_INFO << "robot reader is ready";
734  // ARMARX_CHECK_NOT_NULL(robotReader);
735  armarx::control::common::ft::FTSensorTrigger wristSensorTrigger(
736  robotReader, properties.robotName, hand);
737  /*armarx::kinesthetic_learning::kinesthetic_teaching::trigger::FTWristSensorTrigger m_trigger(
738  wristSensor);*/
739 
740  ARMARX_INFO << "wrist trigger has been created";
741 
742  NJointZeroTorqueControllerConfigPtr config(new NJointZeroTorqueControllerConfig());
743  config->maxTorque = 0;
744 
745  auto nodeSet = m_robot->getRobotNodeSet(kinematicChainName);
746  for (auto& name : nodeSet->getNodeNames())
747  {
748  config->jointNames.push_back(name);
749  }
750  NJointZeroTorqueControllerInterfacePrx zeroTorqueController =
751  NJointZeroTorqueControllerInterfacePrx::checkedCast(
752  m_robot_unit->createNJointController(ctrl_class_name, ctrl_name, config));
753  ARMARX_CHECK_NOT_NULL(zeroTorqueController);
754  ARMARX_INFO << "zero torque controller created";
755 
756  float motionThresholdRadian = 0.1f;
757  float noMotionTimeoutMs = 2000.0f;
758 
759  VirtualRobot::RobotNodeSetPtr rns = m_robot->getRobotNodeSet(kinematicChainName);
760  std::vector<std::string> jointNames = rns->getNodeNames();
761  IceUtil::Time start = TimeUtil::GetTime();
762  IceUtil::Time lastMotionTime = start;
763  bool initialMotionDetected = false;
764 
765  /// ---------------------------------- prepare data containers ----------------------------------
766  /// recording data
767  std::vector<std::vector<float>> jointData;
768  std::vector<std::vector<float>> cartData;
769  std::vector<float> timestamps;
770 
771  /// init data
772  Eigen::VectorXf jve = rns->getJointValuesEigen();
773  Eigen::Matrix4f initPose = rns->getTCP()->getPoseInRootFrame();
774  VirtualRobot::MathTools::Quaternion initQuat =
775  VirtualRobot::MathTools::eigen4f2quat(initPose);
776  VirtualRobot::MathTools::Quaternion oldquatE = initQuat;
777 
778  /// joint space
779  for (size_t i = 0; i < jointNames.size(); i++)
780  {
781  std::vector<float> cjoint{};
782  // cjoint.push_back(jve(i));
783  jointData.push_back(cjoint);
784  }
785 
786  /// task space
787  // float quatvec[] = {initPose(0, 3), initPose(1, 3), initPose(2, 3), initQuat.w, initQuat.x, initQuat.y, initQuat.z};
788  for (size_t i = 0; i < 7; ++i)
789  {
790  std::vector<float> cpos{};
791  // cpos.push_back(quatvec[i]);
792  cartData.push_back(cpos);
793  }
794 
795 
796  int motionStartIndex = 0;
797  int motionEndIndex = 0;
798 
799  int sigmaMs = 1;
800  int sampleMs = 10;
801 
802  /*float forceSpike = 0.0f;
803  Eigen::Vector3f initForce;
804  if (side == "right")
805  {
806  initForce = m_ft_r->getForce();
807  }
808  else
809  {
810  initForce = m_ft_l->getForce();
811  }
812  ARMARX_IMPORTANT << "=============== Waiting for force spike ===============";
813  while (forceSpike < 10.0f)
814  {
815  Eigen::Vector3f force;
816  if (side == "right")
817  {
818  force = m_ft_r->getForce();
819  }
820  else
821  {
822  force = m_ft_l->getForce();
823  }
824  forceSpike = (force - initForce).norm();
825  }*/
826  ARMARX_IMPORTANT << "before trigger";
827  //wristSensorTrigger.waitForForceSpike(armarx::control::common::ft::FTSensorTrigger::Edge::RISING);
828  wristSensorTrigger.waitForForceThreshold(20.);
829  ARMARX_INFO << "Start the demonstration ...";
830  ARMARX_IMPORTANT << "after trigger";
831 
832  zeroTorqueController->activateController();
833  while (true) // stop run function if returning true
834  {
835  synchronizeLocalClone(m_robot);
836 
837  IceUtil::Time now = TimeUtil::GetTime();
838 
839  Eigen::VectorXf jve2 = rns->getJointValuesEigen();
840  if ((jve2 - jve).norm() > motionThresholdRadian)
841  {
842  if (!initialMotionDetected)
843  {
844  ARMARX_IMPORTANT << "Start recording";
845  motionStartIndex = 0;
846  //start = now;
847  }
848  jve = jve2;
849  initialMotionDetected = true;
850  lastMotionTime = now;
851  }
852 
853  double t = (now - start).toSecondsDouble();
854 
855  std::vector<float> jvs = rns->getJointValues();
856  for (size_t i = 0; i < jvs.size(); i++)
857  {
858  jointData.at(i).push_back(jvs.at(i));
859  }
860 
861  Eigen::Matrix4f tcpPose = rns->getTCP()->getPoseInRootFrame();
862  cartData.at(0).push_back(tcpPose(0, 3));
863  cartData.at(1).push_back(tcpPose(1, 3));
864  cartData.at(2).push_back(tcpPose(2, 3));
865  VirtualRobot::MathTools::Quaternion quat =
866  VirtualRobot::MathTools::eigen4f2quat(tcpPose);
867 
868  // check flipped orientation quaternion
869  double dotProduct = quat.w * oldquatE.w + quat.x * oldquatE.x + quat.y * oldquatE.y +
870  quat.z * oldquatE.z;
871  if (dotProduct < 0)
872  {
873  cartData.at(3).push_back(-quat.w);
874  cartData.at(4).push_back(-quat.x);
875  cartData.at(5).push_back(-quat.y);
876  cartData.at(6).push_back(-quat.z);
877  oldquatE.w = -quat.w;
878  oldquatE.x = -quat.x;
879  oldquatE.y = -quat.y;
880  oldquatE.z = -quat.z;
881  }
882  else
883  {
884  cartData.at(3).push_back(quat.w);
885  cartData.at(4).push_back(quat.x);
886  cartData.at(5).push_back(quat.y);
887  cartData.at(6).push_back(quat.z);
888 
889  oldquatE.w = quat.w;
890  oldquatE.x = quat.x;
891  oldquatE.y = quat.y;
892  oldquatE.z = quat.z;
893  }
894 
895  timestamps.push_back(t);
896 
897  if (initialMotionDetected &&
898  (now - lastMotionTime).toMilliSeconds() > noMotionTimeoutMs)
899  {
900  ARMARX_IMPORTANT << "Stop recording";
901  motionEndIndex = timestamps.size(); // - noMotionTimeoutMs / sampleMs;
902  break;
903  }
904 
905  TimeUtil::USleep(sampleMs * 1000);
906  }
907  ARMARX_IMPORTANT << "Demonstration done! Processing data ...";
908 
909  zeroTorqueController->deactivateController();
910  while (zeroTorqueController->isControllerActive())
911  {
912  usleep(10000);
913  }
914  zeroTorqueController->deleteController();
915  ARMARX_INFO << "zerotorque controller for kinesthetic teaching is deleteded";
916  enableVelocityMode();
917 
918  motionStartIndex -= sigmaMs / sampleMs * 2;
919  if (motionStartIndex < 0)
920  {
921  motionStartIndex = 0;
922  }
923  //motionEndIndex += sigmaMs / sampleMs * 2;
924  //if (motionEndIndex >= data.size())
925  //{
926  // motionEndIndex = data.size() - 1;
927  //}
928 
929  ARMARX_IMPORTANT << VAROUT(motionStartIndex) << VAROUT(motionEndIndex);
930 
931  ARMARX_IMPORTANT << "copy timestamps";
932  timestamps = std::vector<float>(timestamps.begin() + motionStartIndex,
933  timestamps.begin() + motionEndIndex);
934  for (size_t n = 1; n < timestamps.size(); n++)
935  {
936  timestamps.at(n) = timestamps.at(n) - timestamps.at(0);
937  }
938  timestamps.at(0) = 0;
939 
940  ARMARX_IMPORTANT << VAROUT(timestamps);
941  std::vector<float> newT = math::TimeSeriesUtils::MakeTimestamps(
942  timestamps.at(0), timestamps.at(timestamps.size() - 1), timestamps.size());
943  ARMARX_IMPORTANT << VAROUT(newT);
944 
945  {
946  StringFloatSeqDict plotFiltered;
947  StringFloatSeqDict plotOrig;
948  StringFloatSeqDict plotResampled;
949  for (size_t i = 0; i < jointData.size(); i++)
950  {
951  jointData.at(i) = std::vector<float>(jointData.at(i).begin() + motionStartIndex,
952  jointData.at(i).begin() + motionEndIndex);
953  if (rns->getNode(i)->isLimitless())
954  {
955  math::LinearizeAngularTrajectory::LinearizeRef(jointData.at(i));
956  }
957  plotOrig[jointNames.at(i)] = jointData.at(i);
958  jointData.at(i) =
959  math::TimeSeriesUtils::Resample(timestamps, jointData.at(i), newT);
960  plotResampled[jointNames.at(i)] = jointData.at(i);
961  jointData.at(i) = math::TimeSeriesUtils::ApplyGaussianFilter(
962  jointData.at(i),
963  sigmaMs * 0.001f,
964  sampleMs * 0.001f,
965  math::TimeSeriesUtils::BorderMode::Nearest);
966  plotFiltered[jointNames.at(i)] = jointData.at(i);
967  }
968  // getStaticPlotter()->addPlotWithTimestampVector("original joint motion", timestamps, plotOrig);
969  // getStaticPlotter()->addPlotWithTimestampVector("resampled joint motion", newT, plotResampled);
970  // getStaticPlotter()->addPlotWithTimestampVector("filtered joint motion", newT, plotFiltered);
971  }
972 
973  std::vector<std::string> cartName = {"x", "y", "z", "qw", "qx", "qy", "qz"};
974 
975  timestamps = newT;
976 
977  std::string trajName;
978  {
979  IceUtil::Time now = IceUtil::Time::now();
980  time_t timer = now.toSeconds();
981  struct tm* ts;
982  char buffer[80];
983  ts = localtime(&timer);
984  strftime(buffer, 80, "%Y-%m-%d-%H-%M-%S", ts);
985  std::string str(buffer);
986  trajName = fileName + "-" + str;
987  }
988 
989 
990  double cutoutTime = (noMotionTimeoutMs / 1000.0f) * 0.8;
991 
992  std::string packageName = "armarx_control";
993 
994  armarx::CMakePackageFinder finder(packageName);
995  std::string dataDir = finder.getDataDir() + "/" + packageName + "/kinesthetic_teaching/";
996  std::string returnFileNamePattern = dataDir + trajName;
997 
998  /// write task space data to RobotControllers Data dir
999  ARMARX_IMPORTANT << "writing task space data!";
1000  std::string dmpForwardFile;
1001  std::string dmpBackwardFile;
1002  {
1003  std::vector<std::string> header;
1004  header.push_back("timestamp");
1005  for (size_t i = 0; i < cartName.size(); ++i)
1006  {
1007  header.push_back(cartName.at(i));
1008  }
1009 
1010  std::string ffname = trajName + "-ts-forward.csv";
1011  std::string bfname = trajName + "-ts-backward.csv";
1012  dmpForwardFile = dataDir + ffname;
1013  dmpBackwardFile = dataDir + bfname;
1014  std::vector<std::string> fflist;
1015  fflist.push_back(ffname);
1016 
1017  std::vector<std::string> bflist;
1018  bflist.push_back(bfname);
1019 
1020  CsvWriter fwriter(dmpForwardFile, header, false);
1021  CsvWriter bwriter(dmpBackwardFile, header, false);
1022 
1023  ARMARX_IMPORTANT << "writing taskspace trajectory as " << ffname << " and " << bfname;
1024 
1025  double endTime = timestamps.at(timestamps.size() - 1);
1026  for (size_t n = 0; n < timestamps.size(); n++)
1027  {
1028 
1029  std::vector<float> row;
1030  row.push_back(timestamps.at(n));
1031  for (size_t i = 0; i < cartData.size(); i++)
1032  {
1033  row.push_back(cartData.at(i).at(n));
1034  }
1035  fwriter.write(row);
1036 
1037  if (endTime - timestamps.at(n) < cutoutTime)
1038  {
1039  Eigen::Matrix4f endPose = VirtualRobot::MathTools::quat2eigen4f(
1040  row.at(5), row.at(6), row.at(7), row.at(4));
1041  endPose(0, 3) = row.at(1);
1042  endPose(1, 3) = row.at(2);
1043  endPose(2, 3) = row.at(3);
1044  break;
1045  }
1046  }
1047  fwriter.close();
1048  int nt = 0;
1049  for (int n = timestamps.size() - 1; n >= 0 && nt < static_cast<int>(timestamps.size());
1050  n--, nt++)
1051  {
1052  std::vector<float> row;
1053  row.push_back(timestamps.at(nt));
1054  for (size_t i = 0; i < cartData.size(); i++)
1055  {
1056  row.push_back(cartData.at(i).at(n));
1057  }
1058  bwriter.write(row);
1059 
1060  if (n == 0)
1061  {
1062  Eigen::Matrix4f endPose = VirtualRobot::MathTools::quat2eigen4f(
1063  row.at(5), row.at(6), row.at(7), row.at(4));
1064  endPose(0, 3) = row.at(1);
1065  endPose(1, 3) = row.at(2);
1066  endPose(2, 3) = row.at(3);
1067  break;
1068  }
1069  }
1070  bwriter.close();
1071  }
1072 
1073  /// write joint space data to RobotControllers Data dir
1074  ARMARX_IMPORTANT << "writing joint space data!";
1075  std::string dmpJSForwardFile;
1076  std::string dmpJSBackwardFile;
1077  {
1078  std::vector<std::string> header;
1079  header.push_back("timestamp");
1080  for (size_t i = 0; i < jointNames.size(); ++i)
1081  {
1082  header.push_back(jointNames.at(i));
1083  }
1084 
1085  std::string ffname = trajName + "-js-forward.csv";
1086  std::string bfname = trajName + "-js-backward.csv";
1087  dmpJSForwardFile = dataDir + ffname;
1088  dmpJSBackwardFile = dataDir + bfname;
1089  std::vector<std::string> fflist;
1090  fflist.push_back(ffname);
1091 
1092  std::vector<std::string> bflist;
1093  bflist.push_back(bfname);
1094 
1095  CsvWriter fwriter(dmpJSForwardFile, header, false);
1096  CsvWriter bwriter(dmpJSBackwardFile, header, false);
1097 
1098  ARMARX_IMPORTANT << "writing jointspace trajectory as " << ffname << " and " << bfname;
1099 
1100  double endTime = timestamps.at(timestamps.size() - 1);
1101  for (size_t n = 0; n < timestamps.size(); n++)
1102  {
1103 
1104  std::vector<float> row;
1105  row.push_back(timestamps.at(n));
1106  for (size_t i = 0; i < jointNames.size(); i++)
1107  {
1108  row.push_back(jointData.at(i).at(n));
1109  }
1110  fwriter.write(row);
1111 
1112  if (endTime - timestamps.at(n) < cutoutTime)
1113  {
1114  std::vector<float> endJointValue;
1115  for (size_t i = 0; i < jointNames.size(); i++)
1116  {
1117  endJointValue.push_back(jointData.at(i).at(n));
1118  }
1119  break;
1120  }
1121  }
1122  fwriter.close();
1123  ARMARX_INFO << "forward joint space data written to fine!";
1124  int nt = 0;
1125  for (int n = timestamps.size() - 1; n >= 0 && nt < static_cast<int>(timestamps.size());
1126  n--, nt++)
1127  {
1128  std::vector<float> row;
1129  row.push_back(timestamps.at(nt));
1130  for (size_t i = 0; i < jointNames.size(); i++)
1131  {
1132  row.push_back(jointData.at(i).at(n));
1133  }
1134  bwriter.write(row);
1135 
1136  if (n == 0)
1137  {
1138  std::vector<float> endJointValue;
1139  for (size_t i = 0; i < jointNames.size(); i++)
1140  {
1141  endJointValue.push_back(jointData.at(i).at(n));
1142  }
1143  break;
1144  }
1145  }
1146  bwriter.close();
1147  }
1148  return returnFileNamePattern;
1149  // return "";
1150  }
1151 
1152  void
1153  Component::enableVelocityMode(const Ice::Current&)
1154  {
1155  ARMARX_INFO << "enable velocity mode";
1156  // left arm
1157  {
1158  std::vector<std::string> joint_names = m_rns_l->getNodeNames();
1159  NameValueMap target_vels;
1160  NameControlModeMap control_modes;
1161  for (const std::string& joint_name : joint_names)
1162  {
1163  control_modes[joint_name] = eVelocityControl;
1164  target_vels[joint_name] = 0;
1165  }
1166  }
1167  // right arm
1168  {
1169  std::vector<std::string> joint_names = m_rns_r->getNodeNames();
1170  NameValueMap target_vels;
1171  NameControlModeMap control_modes;
1172  for (const std::string& joint_name : joint_names)
1173  {
1174  control_modes[joint_name] = eVelocityControl;
1175  target_vels[joint_name] = 0;
1176  }
1177  }
1178  // // platform
1179  // {
1180  // m_platform->move(0.0f, 0.0f, 0.0f);
1181  // }
1182  }
1183 
1184  Ice::FloatSeq
1185  Component::getPoseInRootFrame(const std::string& nodeName, const Ice::Current&)
1186  {
1187  ARMARX_CHECK(synchronizeLocalClone(m_robot));
1188  Eigen::Matrix4f pose = m_robot->getRobotNode(nodeName)->getPoseInRootFrame();
1189  ARMARX_INFO << VAROUT(pose);
1190  std::vector<Ice::Float> pose_seq;
1191  for (int i = 0; i < pose.rows(); i++)
1192  {
1193  for (int j = 0; j < pose.cols(); j++)
1194  {
1195  pose_seq.push_back(pose(i, j));
1196  }
1197  }
1198  return pose_seq;
1199  }
1200 
1201  Ice::FloatSeq
1202  Component::getPoseInGlobalFrame(const std::string& nodeName, const Ice::Current&)
1203  {
1204  ARMARX_CHECK(synchronizeLocalClone(m_robot));
1205  Eigen::Matrix4f pose = m_robot->getRobotNode(nodeName)->getGlobalPose();
1206  ARMARX_INFO << VAROUT(pose);
1207  std::vector<Ice::Float> pose_seq;
1208  for (int i = 0; i < pose.rows(); i++)
1209  {
1210  for (int j = 0; j < pose.cols(); j++)
1211  {
1212  pose_seq.push_back(pose(i, j));
1213  }
1214  }
1215  return pose_seq;
1216  }
1217 
1218  std::string
1219  Component::getDefaultName() const
1220  {
1221  return Component::defaultName;
1222  }
1223 
1224  std::string
1225  Component::GetDefaultName()
1226  {
1227  return Component::defaultName;
1228  }
1229 
1230  /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
1231  void
1232  Component::createRemoteGuiTab()
1233  {
1234  using namespace armarx::RemoteGui::Client;
1235 
1236  // Setup the widgets.
1237 
1238  tab.boxLayerName.setValue(properties.boxLayerName);
1239 
1240  tab.numBoxes.setValue(properties.numBoxes);
1241  tab.numBoxes.setRange(0, 100);
1242 
1243  tab.drawBoxes.setLabel("Draw Boxes");
1244 
1245  // Setup the layout.
1246 
1247  GridLayout grid;
1248  int row = 0;
1249  {
1250  grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
1251  ++row;
1252 
1253  grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
1254  ++row;
1255 
1256  grid.add(tab.drawBoxes, {row, 0}, {2, 1});
1257  ++row;
1258  }
1259 
1260  VBoxLayout root = {grid, VSpacer()};
1261  RemoteGui_createTab(getName(), root, &tab);
1262  }
1263 
1264 
1265  void
1266  Component::RemoteGui_update()
1267  {
1268  if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
1269  {
1270  std::scoped_lock lock(propertiesMutex);
1271  properties.boxLayerName = tab.boxLayerName.getValue();
1272  properties.numBoxes = tab.numBoxes.getValue();
1273 
1274  {
1275  setDebugObserverDatafield("numBoxes", properties.numBoxes);
1276  setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
1277  sendDebugObserverBatch();
1278  }
1279  }
1280  if (tab.drawBoxes.wasClicked())
1281  {
1282  // Lock shared variables in methods running in separate threads
1283  // and pass them to functions. This way, the called functions do
1284  // not need to think about locking.
1285  std::scoped_lock lock(propertiesMutex);
1286  drawBoxes(properties, arviz);
1287  }
1288  }
1289  */
1290 
1291 
1292  /* (Requires the armarx::ArVizComponentPluginUser.)
1293  void
1294  Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
1295  {
1296  // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
1297  // See the ArVizExample in RobotAPI for more examples.
1298 
1299  viz::Layer layer = arviz.layer(p.boxLayerName);
1300  for (int i = 0; i < p.numBoxes; ++i)
1301  {
1302  layer.add(viz::Box("box_" + std::to_string(i))
1303  .position(Eigen::Vector3f(i * 100, 0, 0))
1304  .size(20).color(simox::Color::blue()));
1305  }
1306  arviz.commit(layer);
1307  }
1308  */
1309 
1310 
1311  ARMARX_DECOUPLED_REGISTER_COMPONENT(Component);
1312 
1313 } // namespace armarx::control::components::controller_creator
str
std::string str(const T &t)
Definition: UserAssistedSegmenterGuiWidgetController.cpp:43
armarx::CsvWriter
Definition: CsvWriter.h:36
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
Component.h
LinearizeAngularTrajectory.h
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::math::TimeSeriesUtils::BorderMode::Nearest
@ Nearest
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::control::components::controller_creator::Component::onExitComponent
void onExitComponent() override
Definition: Component.cpp:189
armarx::armem::robot_state::RobotReader::Hand
Hand
Definition: RobotReader.h:91
armarx::armem::robot_state::RobotReader::Hand::Right
@ Right
armarx::control::components::controller_creator::Component
Definition: Component.h:57
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:52
armarx::control::common::ft::FTSensorTrigger::waitForForceThreshold
void waitForForceThreshold(float threshold) const
Definition: FTSensorTrigger.cpp:181
CsvWriter.h
armarx::ManagedIceObject::addPlugin
PluginT * addPlugin(const std::string prefix="", ParamsT &&... params)
Definition: ManagedIceObject.h:186
aron_conversions.h
armarx::control::components::controller_creator::Component::Component
Component()
Definition: Component.cpp:66
armarx::control::components::controller_creator::Component::createController
std::string createController(const std::string &namePrefix, const std::string &controllerType, const aron::data::dto::DictPtr &configDict=nullptr, const std::string &configFilename="", bool activate=false, bool allowReuse=true, bool cfgFromMemory=false, const Ice::Current &=Ice::emptyCurrent) override
Definition: Component.cpp:471
armarx::control::components::controller_creator::Component::createComplianceController
std::string createComplianceController(const std::string &namePrefix, const ::armarx::aron::data::dto::DictPtr &configDict=nullptr, const std::string &configFilename="", bool flagActivate=true, bool flagAllowReuse=true, bool flagFromMemory=false)
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::RobotStateComponentPluginUser::addRobot
VirtualRobot::RobotPtr addRobot(const std::string &id, const VirtualRobot::RobotPtr &robot, const VirtualRobot::RobotNodeSetPtr &rns={}, const VirtualRobot::RobotNodePtr &node={})
Definition: RobotStateComponentPlugin.cpp:405
armarx::RobotStateComponentPluginUser::synchronizeLocalClone
bool synchronizeLocalClone(const VirtualRobot::RobotPtr &robot) const
Definition: RobotStateComponentPlugin.cpp:470
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
Clock.h
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::math::TimeSeriesUtils::ApplyGaussianFilter
static std::vector< float > ApplyGaussianFilter(const std::vector< float > &data, float sigma, float sampleTime, BorderMode mode)
Definition: TimeSeriesUtils.cpp:105
armarx::CsvWriter::close
void close()
Definition: CsvWriter.cpp:83
armarx::armem::robot_state::RobotReader::Hand::Left
@ Left
armarx::TimeUtil::USleep
static int USleep(long usec)
like timed_wait on boost condition_variables, but with timeserver support
Definition: TimeUtil.cpp:159
FTSensorTrigger.h
armarx::control::components::controller_creator::Component::getPoseInRootFrame
Ice::FloatSeq getPoseInRootFrame(const std::string &nodeName, const Ice::Current &=Ice::emptyCurrent) override
Definition: Component.cpp:1185
armarx::math::TimeSeriesUtils::MakeTimestamps
static std::vector< float > MakeTimestamps(float start, float end, size_t count)
Definition: TimeSeriesUtils.cpp:128
armarx::CMakePackageFinder::getDataDir
std::string getDataDir() const
Definition: CMakePackageFinder.h:194
armarx::control::common::ControllerTypeShort
const simox::meta::EnumNames< ControllerType > ControllerTypeShort
Definition: type.h:85
aron_conversions.h
Metronome.h
armarx::control::components::controller_creator::Component::~Component
~Component() override
armarx::control::components::controller_creator::Component::kinestheticTeaching
std::string kinestheticTeaching(const std::string &kinematicChainName, const std::string &side, const std::string &fileName, const std::string &nameSuffix, const Ice::Current &=Ice::emptyCurrent) override
Definition: Component.cpp:713
ControllerBuilder.h
types.h
controller_descriptions.h
armarx::control::components::controller_creator::Component::onInitComponent
void onInitComponent() override
Definition: Component.cpp:112
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::components::controller_creator::Component::GetDefaultName
static std::string GetDefaultName()
Get the component's default name.
Definition: Component.cpp:1225
utils.h
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
armarx::CsvWriter::write
void write(const std::vector< std::string > &row)
Definition: CsvWriter.cpp:43
armarx::control::components::controller_creator::ARMARX_DECOUPLED_REGISTER_COMPONENT
ARMARX_DECOUPLED_REGISTER_COMPONENT(Component)
armarx::control::njoint_controller::platform::platform_follower_controller::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformFollowerController.h:88
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:79
CMakePackageFinder.h
Decoupled.h
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:69
TimeUtil.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::control::common::ft::FTSensorTrigger
Definition: FTSensorTrigger.h:41
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
armarx::control::common::ControllerType
ControllerType
Definition: type.h:29
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::control::components::controller_creator::Component::getDefaultName
std::string getDefaultName() const override
Definition: Component.cpp:1219
armarx::math::LinearizeAngularTrajectory::LinearizeRef
static void LinearizeRef(std::vector< float > &data)
Definition: LinearizeAngularTrajectory.cpp:66
armarx::math::TimeSeriesUtils::Resample
static std::vector< float > Resample(const std::vector< float > &timestamps, const std::vector< float > &data, const std::vector< float > &newTimestamps)
Definition: TimeSeriesUtils.cpp:37
TimeSeriesUtils.h
Logging.h
armarx::control::components::controller_creator::ComponentInterface::enableVelocityMode
void enableVelocityMode()
T
float T
Definition: UnscentedKalmanFilterTest.cpp:38
armarx::control::components::controller_creator::Component::onConnectComponent
void onConnectComponent() override
Definition: Component.cpp:122
armarx::control::components::controller_creator::Component::getPoseInGlobalFrame
Ice::FloatSeq getPoseInGlobalFrame(const std::string &nodeName, const Ice::Current &=Ice::emptyCurrent) override
Definition: Component.cpp:1202
armarx::control::components::controller_creator
Definition: Component.cpp:61
armarx::control::components::controller_creator::Component::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: Component.cpp:74
armarx::control::components::controller_creator::Component::onDisconnectComponent
void onDisconnectComponent() override
Definition: Component.cpp:184
norm
double norm(const Point &a)
Definition: point.hpp:102