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