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 #include <Eigen/Core>
26 // #include <SimoxUtility/color/Color.h>
27 #include "ArmarXCore/core/time/Metronome.h"
28 #include <ArmarXCore/core/logging/Logging.h>
29 #include <ArmarXCore/core/time/Clock.h>
30 #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
31 // for kinesthetic teaching
32 #include <ArmarXCore/core/csv/CsvWriter.h>
33 #include <ArmarXGui/interface/StaticPlotterInterface.h>
34 #include <RobotAPI/libraries/core/math/TimeSeriesUtils.h>
35 #include <RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h>
36 #include <RobotAPI/interface/units/RobotUnit/NjointZeroTorqueController.h>
37 #include <armarx/control/common/utils.h>
38 
39 
40 namespace armarx::control::components::controller_creator
41 {
42 
43  const std::string
44  Component::defaultName = "controller_creator";
45 
46  Component::Component()
47  {
48  addPlugin(m_virtualRobotReaderPlugin);
49  }
50 
51  Component::~Component() = default;
52 
53 
54 
55  armarx::PropertyDefinitionsPtr
56  Component::createPropertyDefinitions()
57  {
58  armarx::PropertyDefinitionsPtr def = new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
59 
60  def->optional(properties.robotName, "robotName");
61  def->optional(properties.robotUnitName, "robotUnitName");
62 
63  def->optional(properties.defaultRobotNodeSetLeft, "default_robotNodeSet_left", "default kinematic chain name of left arm.");
64  def->optional(properties.defaultRobotNodeSetRight, "default_robotNodeSet_right", "default kinematic chain name of right arm.");
65 
66  def->component(m_robot_unit, properties.robotUnitName);
67  def->component(m_force_torque_observer, properties.ftObserverName);
68  // Publish to a topic (passing the TopicListenerPrx).
69  // def->topic(myTopicListener);
70 
71  // Subscribe to a topic (passing the topic name).
72  // def->topic<PlatformUnitListener>("MyTopic");
73 
74  // Use (and depend on) another component (passing the ComponentInterfacePrx).
75  // def->component(myComponentProxy)
76 
77 
78  // Add a required property. (The component won't start without a value being set.)
79  // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
80 
81  // Add an optional property.
82  // def->optional(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
83  // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
84 
85  return def;
86  }
87 
88 
89  void
90  Component::onInitComponent()
91  {
92  // Topics and properties defined above are automagically registered.
93 
94  // Keep debug observer data until calling `sendDebugObserverBatch()`.
95  // (Requires the armarx::DebugObserverComponentPluginUser.)
96  // setDebugObserverBatchModeEnabled(true);
97 
98  }
99 
100 
101  void
102  Component::onConnectComponent()
103  {
104  // Do things after connecting to topics and components.
105  ARMARX_CHECK_NOT_NULL(m_robot_unit);
106 
107  /* (Requires the armarx::DebugObserverComponentPluginUser.)
108  // Use the debug observer to log data over time.
109  // The data can be viewed in the ObserverView and the LivePlotter.
110  // (Before starting any threads, we don't need to lock mutexes.)
111  {
112  setDebugObserverDatafield("numBoxes", properties.numBoxes);
113  setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
114  sendDebugObserverBatch();
115  }
116  */
117 
118  /* (Requires the armarx::ArVizComponentPluginUser.)
119  // Draw boxes in ArViz.
120  // (Before starting any threads, we don't need to lock mutexes.)
121  drawBoxes(properties, arviz);
122  */
123 
124  /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
125  // Setup the remote GUI.
126  {
127  createRemoteGuiTab();
128  RemoteGui_startRunningTask();
129  }
130  */
131  ARMARX_INFO << properties.robotName;
132  // m_robot = m_virtualRobotReaderPlugin->get().getRobot(properties.robotName);
133  if (!m_robot)
134  {
135  // m_robot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::eStructure);
136  m_robot = addRobot("controller_creator_robot", VirtualRobot::RobotIO::eStructure);
137  }
138  ARMARX_CHECK_NOT_NULL(m_robot);
139  ARMARX_CHECK(synchronizeLocalClone(m_robot));
140 
141  m_rns_l = m_robot->getRobotNodeSet(properties.defaultRobotNodeSetLeft);
142  m_rns_r = m_robot->getRobotNodeSet(properties.defaultRobotNodeSetRight);
143 
144  m_ft_l.reset(new armarx::control::common::ft::ForceTorqueUtility(m_force_torque_observer, properties.ftSensorNameLeft));
145  m_ft_r.reset(new armarx::control::common::ft::ForceTorqueUtility(m_force_torque_observer, properties.ftSensorNameRight));
146 
147  ARMARX_IMPORTANT << "controller creator is ready";
148 
149  }
150 
151 
152  void
153  Component::onDisconnectComponent()
154  {
155  }
156 
157 
158  void
159  Component::onExitComponent()
160  {
161  }
162 
163  std::string
164  Component::createImpedanceController(
165  const std::string& controllerNamePrefix,
166  const std::string& robotNodeSet,
167  const std::string& configFilename)
168  {
169  /// create controllers with controller builder
170  auto builder = controllerBuilder<control::common::ControllerType::TSImpedance>();
171  builder.withNodeSet(robotNodeSet).withNamePrefix(controllerNamePrefix);
172 
173  /// load predefined config from file
174  {
175  if (configFilename.empty())
176  {
177  const std::string controllerClassName =
178  control::common::ControllerTypeNames.to_name(control::common::ControllerType::TSImpedance);
179  const armarx::PackagePath configPath("armarx_control", "controller_config/" + controllerClassName + "/default.json");
180  ARMARX_IMPORTANT << "Using config `" << configPath.toSystemPath() << "`.";
181  ARMARX_CHECK(std::filesystem::exists(configPath.toSystemPath()));
182 
183  builder.withConfig(configPath.toSystemPath());
184  }
185  else
186  {
187  builder.withConfig(configFilename);
188  }
189 
190 
191  /// to be removed
192  ARMARX_INFO << VAROUT(builder.config().kpImpedance);
193  ARMARX_INFO << VAROUT(builder.config().kdImpedance);
194  ARMARX_INFO << VAROUT(builder.config().kpNullspace);
195  ARMARX_INFO << VAROUT(builder.config().kdNullspace);
196  ARMARX_INFO << VAROUT(builder.config().desiredPose);
197  ARMARX_INFO << VAROUT(builder.config().desiredTwist);
198  if (builder.config().desiredNullspaceJointAngles.has_value())
199  ARMARX_INFO << VAROUT(builder.config().desiredNullspaceJointAngles.value());
200  ARMARX_INFO << VAROUT(builder.config().torqueLimit);
201  ARMARX_INFO << VAROUT(builder.config().qvelFilter);
202  }
203 
204  ARMARX_CHECK_NOT_NULL(m_robot);
205  ARMARX_CHECK(synchronizeLocalClone(m_robot));
206  const auto rns = m_robot->getRobotNodeSet(robotNodeSet);
207 
208 
209  builder.config().desiredPose = rns->getTCP()->getPoseInRootFrame();
210  ARMARX_IMPORTANT << VAROUT(builder.config().desiredPose);
211 
212  ARMARX_TRACE;
213  ARMARX_INFO << "creating controller";
214 
215  auto ctrlWrapper = builder.create();
216  const std::string controllerName = builder.getControllerName();
217  m_robotNodeSets[controllerName] = rns;
218  m_controllerJointNumbers[controllerName] = rns->getJointValues().size();
219 
220  // The controller is scoped by default, which means that if the ControllerWrapper goes out of scope, it will deactivate
221  // and delete the NJointController. To prevent this, you can use the daemonize() method if needed.
222  ctrlWrapper->daemonize(true);
223 
224  ARMARX_TRACE;
225  ARMARX_CHECK(ctrlWrapper.has_value());
226  m_ctrlTSImpedance[controllerName] = std::nullopt;
227  m_ctrlTSImpedance[controllerName].emplace(std::move(ctrlWrapper.value()));
228 
229  Clock::WaitFor(Duration::MilliSeconds(300));
230  ARMARX_INFO << "Activating controller";
231  m_ctrlTSImpedance[controllerName]->updateConfig();
232  m_ctrlTSImpedance[controllerName]->activate();
233 
234  m_prevTargetPose[controllerName] = rns->getTCP()->getPoseInRootFrame();
235  m_prevNullspaceJointTarget[controllerName] = rns->getJointValuesEigen();
236  return controllerName;
237  }
238 
239  std::string
240  Component::createImpedanceMPController(
241  const std::string& controllerNamePrefix,
242  const std::string& robotNodeSet,
243  const std::string& configFilename)
244  {
245  /// create controllers with controller builder
246  auto builder = controllerBuilder<control::common::ControllerType::TSImpedanceMP>();
247  builder.allowReuse(true).withNodeSet(robotNodeSet).withNamePrefix(controllerNamePrefix);
248 
249  /// load predefined config from file
250  {
251  if (configFilename.empty())
252  {
253  const std::string controllerClassName =
254  control::common::ControllerTypeNames.to_name(control::common::ControllerType::TSImpedanceMP);
255  const armarx::PackagePath configPath("armarx_control", "controller_config/" + controllerClassName + "/default.json");
256  ARMARX_IMPORTANT << "Using config `" << configPath.toSystemPath() << "`.";
257  ARMARX_CHECK(std::filesystem::exists(configPath.toSystemPath()));
258 
259  builder.withConfig(configPath.toSystemPath());
260  }
261  else
262  {
263  builder.withConfig(configFilename);
264  }
265 
266 
267  /// to be removed
268  ARMARX_INFO << VAROUT(builder.config().kpImpedance);
269  ARMARX_INFO << VAROUT(builder.config().kdImpedance);
270  ARMARX_INFO << VAROUT(builder.config().kpNullspace);
271  ARMARX_INFO << VAROUT(builder.config().kdNullspace);
272  ARMARX_INFO << VAROUT(builder.config().desiredPose);
273  ARMARX_INFO << VAROUT(builder.config().desiredTwist);
274  if (builder.config().desiredNullspaceJointAngles.has_value())
275  ARMARX_INFO << VAROUT(builder.config().desiredNullspaceJointAngles.value());
276  ARMARX_INFO << VAROUT(builder.config().torqueLimit);
277  ARMARX_INFO << VAROUT(builder.config().qvelFilter);
278  }
279 
280  ARMARX_CHECK_NOT_NULL(m_robot);
281  ARMARX_CHECK(synchronizeLocalClone(m_robot));
282  const auto rns = m_robot->getRobotNodeSet(robotNodeSet);
283 
284 
285  builder.config().desiredPose = rns->getTCP()->getPoseInRootFrame();
286  ARMARX_IMPORTANT << VAROUT(builder.config().desiredPose);
287 
288  ARMARX_TRACE;
289  ARMARX_INFO << "creating controller";
290 
291  auto ctrlWrapper = builder.create();
292  const std::string controllerName = builder.getControllerName();
293  m_robotNodeSets[controllerName] = rns;
294  m_controllerJointNumbers[controllerName] = rns->getJointValues().size();
295 
296  // The controller is scoped by default, which means that if the ControllerWrapper goes out of scope, it will deactivate
297  // and delete the NJointController. To prevent this, you can use the daemonize() method if needed.
298  ctrlWrapper->daemonize(true);
299 
300  ARMARX_TRACE;
301  ARMARX_CHECK(ctrlWrapper.has_value());
302  m_ctrlTSImpedanceMP[controllerName] = std::nullopt;
303  m_ctrlTSImpedanceMP[controllerName].emplace(std::move(ctrlWrapper.value()));
304 
305  Clock::WaitFor(Duration::MilliSeconds(300));
306  ARMARX_INFO << "Activating controller";
307  m_ctrlTSImpedanceMP[controllerName]->updateConfig();
308 
309  /// only for test
310  // auto ctrl = NJointTSImpedanceMPControllerInterfacePrx::checkedCast(m_ctrlTSImpedanceMP[controllerName]->ctrl());
311  // ctrl->learnFromCSV(std::vector<std::string>());
312 
313  m_ctrlTSImpedanceMP[controllerName]->activate();
314 
315  m_prevTargetPose[controllerName] = rns->getTCP()->getPoseInRootFrame();
316  m_prevNullspaceJointTarget[controllerName] = rns->getJointValuesEigen();
317  // /// only for test
318  // auto currentPose = m_prevTargetPose[controllerName];
319  // auto targetPose = currentPose;
320  // targetPose(2, 3) += 300.0;
321  // ctrl->start(
322  // "default",
323  // armarx::control::common::mat4ToDVec(currentPose),
324  // armarx::control::common::mat4ToDVec(targetPose),
325  // -1);
326 
327  return controllerName;
328  }
329 
330  std::string
331  Component::createAdmittanceController(
332  const std::string &controllerNamePrefix,
333  const std::string &robotNodeSet,
334  const std::string& configFilename)
335  {
336  ARMARX_INFO << "creating ts admittance controller";
337  /// create controllers with controller builder
338  auto builder = controllerBuilder<control::common::ControllerType::TSAdmittance>();
339  builder.withNodeSet(robotNodeSet).withNamePrefix(controllerNamePrefix);
340 
341  /// load predefined config from file
342  {
343  if (configFilename.empty())
344  {
345  const std::string controllerClassName =
346  control::common::ControllerTypeNames.to_name(control::common::ControllerType::TSAdmittance);
347  const armarx::PackagePath configPath("armarx_control", "controller_config/" + controllerClassName + "/default.json");
348  ARMARX_IMPORTANT << "Using config `" << configPath.toSystemPath() << "`.";
349  ARMARX_CHECK(std::filesystem::exists(configPath.toSystemPath()));
350 
351  builder.withConfig(configPath.toSystemPath());
352  }
353  else
354  {
355  builder.withConfig(configFilename);
356  }
357  }
358 
359  ARMARX_CHECK_NOT_NULL(m_robot);
360  ARMARX_CHECK(synchronizeLocalClone(m_robot));
361 
362  const auto rns = m_robot->getRobotNodeSet(robotNodeSet);
363 
364  builder.config().desiredPose = rns->getTCP()->getPoseInRootFrame();
365  ARMARX_IMPORTANT << VAROUT(builder.config().desiredPose);
366 
367  ARMARX_TRACE;
368  ARMARX_INFO << "creating controller";
369 
370  auto ctrlWrapper = builder.create();
371  const std::string controllerName = builder.getControllerName();
372  m_robotNodeSets[controllerName] = rns;
373  m_controllerJointNumbers[controllerName] = rns->getJointValues().size();
374 
375  // The controller is scoped by default, which means that if the ControllerWrapper goes out of scope, it will deactivate
376  // and delete the NJointController. To prevent this, you can use the daemonize() method if needed.
377  ctrlWrapper->daemonize(true);
378 
379  ARMARX_TRACE;
380  ARMARX_CHECK(ctrlWrapper.has_value());
381  m_ctrlTSAdmittance[controllerName] = std::nullopt;
382  m_ctrlTSAdmittance[controllerName].emplace(std::move(ctrlWrapper.value()));
383 
384  Clock::WaitFor(Duration::MilliSeconds(300));
385  ARMARX_INFO << "Activating controller";
386  m_ctrlTSAdmittance[controllerName]->updateConfig();
387  m_ctrlTSAdmittance[controllerName]->activate();
388 
389  m_prevTargetPose[controllerName] = rns->getTCP()->getPoseInRootFrame();
390  m_prevNullspaceJointTarget[controllerName] = rns->getJointValuesEigen();
391  return controllerName;
392  }
393 
394 
395  std::string
396  Component::createController(
397  const std::string& controllerNamePrefix,
398  const std::string& robotNodeSet,
399  const std::string& controllerType,
400  const std::string& configFilename,
401  const Ice::Current&)
402  {
403  ARMARX_INFO << "creating controller of type " << controllerType << " with robot node set " << robotNodeSet;
404  ARMARX_INFO << VAROUT(robotNodeSet) << VAROUT(controllerType) << VAROUT(configFilename);
405  RobotUnitInterfacePrx robotUnit = getControlComponentPlugin().getRobotUnitPlugin().getRobotUnit();
406  robotUnit->loadLibFromPackage("armarx_control", "libarmarx_control_njoint_controller");
407 
408  std::string controllerName;
409 
410  auto type = control::common::ControllerTypeShort.from_name(controllerType);
411  switch (type) {
412  case control::common::ControllerType::TSImpedance:
413  controllerName = createImpedanceController(controllerNamePrefix, robotNodeSet, configFilename);
414  break;
415  case control::common::ControllerType::TSAdmittance:
416  controllerName = createAdmittanceController(controllerNamePrefix, robotNodeSet, configFilename);
417  break;
418  case control::common::ControllerType::TSImpedanceMP:
419  controllerName = createImpedanceMPController(controllerNamePrefix, robotNodeSet, configFilename);
420  break;
421 
422  default:
423  ARMARX_INFO << "controller type: " << controllerType << " is not supported yet";
424  }
425  return controllerName;
426  }
427 
428  bool
429  Component::updateTargetPose(
430  const std::string &controllerName,
431  const std::string& controllerType,
432  const Ice::FloatSeq& targetPoseVector,
433  const Ice::Current&)
434  {
435  auto names = control::common::ControllerTypeShort.names();
436  const bool is_in = names.find(controllerType) != names.end();
437  ARMARX_CHECK_EQUAL(7, targetPoseVector.size());
438  if (!is_in)
439  {
440  ARMARX_ERROR << "controller type not supported";
441  return false;
442  }
443  if (targetPoseVector.size() != 7)
444  {
445  ARMARX_ERROR << "target pose vector " << targetPoseVector << " should follow (x, y, z, w, rx, ry, rz) convention";
446  return false;
447  }
448 
449  m_prevTargetPose[controllerName] = VirtualRobot::MathTools::quat2eigen4f(targetPoseVector.at(4), targetPoseVector.at(5), targetPoseVector.at(6), targetPoseVector.at(3));
450  m_prevTargetPose[controllerName](0, 3) = targetPoseVector.at(0);
451  m_prevTargetPose[controllerName](1, 3) = targetPoseVector.at(1);
452  m_prevTargetPose[controllerName](2, 3) = targetPoseVector.at(2);
453 
454  auto type = control::common::ControllerTypeShort.from_name(controllerType);
455  switch (type) {
456  case control::common::ControllerType::TSImpedance:
457  ARMARX_CHECK(m_ctrlTSImpedance[controllerName]);
458  m_ctrlTSImpedance[controllerName]->config.desiredPose = m_prevTargetPose[controllerName];
459  m_ctrlTSImpedance[controllerName]->config.desiredNullspaceJointAngles = m_prevNullspaceJointTarget[controllerName];
460  m_ctrlTSImpedance[controllerName]->updateConfig();
461  break;
462  case control::common::ControllerType::TSAdmittance:
463  ARMARX_CHECK(m_ctrlTSAdmittance[controllerName]);
464  m_ctrlTSAdmittance[controllerName]->config.desiredPose = m_prevTargetPose[controllerName];
465  m_ctrlTSAdmittance[controllerName]->config.desiredNullspaceJointAngles = m_prevNullspaceJointTarget[controllerName];
466  m_ctrlTSAdmittance[controllerName]->updateConfig();
467  break;
468 
469  default:
470  ARMARX_INFO << "controller type: " << controllerType << " is not supported yet";
471  return false;
472  }
473  return true;
474  }
475 
476  bool
477  Component::updateTargetNullspaceJointAngles(
478  const std::string &controllerName,
479  const std::string &controllerType,
480  const Ice::FloatSeq& targetNullspaceJointAngle,
481  const Ice::Current &)
482  {
483  auto names = control::common::ControllerTypeShort.names();
484  const bool is_in = names.find(controllerType) != names.end();
485  ARMARX_CHECK_EQUAL(m_controllerJointNumbers[controllerName], targetNullspaceJointAngle.size());
486  if (!is_in)
487  {
488  ARMARX_ERROR << "controller type not supported";
489  return false;
490  }
491 
492  Eigen::VectorXf targetJointAngles = Eigen::VectorXf::Zero(m_controllerJointNumbers[controllerName]);
493  for (size_t i = 0; i < m_controllerJointNumbers[controllerName]; i++)
494  {
495  targetJointAngles(i) = targetNullspaceJointAngle[i];
496  }
497 
498  auto type = control::common::ControllerTypeShort.from_name(controllerType);
499  switch (type) {
500  case control::common::ControllerType::TSImpedance:
501  ARMARX_CHECK(m_ctrlTSImpedance[controllerName]);
502  m_ctrlTSImpedance[controllerName]->config.desiredNullspaceJointAngles = targetJointAngles;
503  m_ctrlTSImpedance[controllerName]->updateConfig();
504  break;
505  case control::common::ControllerType::TSAdmittance:
506  ARMARX_CHECK(m_ctrlTSAdmittance[controllerName]);
507  m_ctrlTSAdmittance[controllerName]->config.desiredNullspaceJointAngles = targetJointAngles;
508  m_ctrlTSAdmittance[controllerName]->updateConfig();
509  break;
510 
511  default:
512  ARMARX_INFO << "controller type: " << controllerType << " is not supported yet";
513  return false;
514  }
515  return true;
516  }
517 
518  bool
519  Component::updateTarget(
520  const std::string &controllerName,
521  const std::string &controllerType,
522  const Ice::FloatSeq& targetPoseVector,
523  const Ice::FloatSeq& targetNullspaceJointAngle,
524  const Ice::Current &)
525  {
526  auto names = control::common::ControllerTypeShort.names();
527  const bool is_in = names.find(controllerType) != names.end();
528  if (!is_in)
529  {
530  ARMARX_ERROR << "controller type not supported";
531  return false;
532  }
533 
534  bool updataPose = false;
535  if (targetPoseVector.size() > 0)
536  {
537  if (targetPoseVector.size() != 7)
538  {
539  ARMARX_ERROR << "target pose vector " << targetPoseVector << " should follow (x, y, z, w, rx, ry, rz) convention";
540  return false;
541  }
542  updataPose = true;
543  m_prevTargetPose[controllerName] = VirtualRobot::MathTools::quat2eigen4f(targetPoseVector.at(4), targetPoseVector.at(5), targetPoseVector.at(6), targetPoseVector.at(3));
544  m_prevTargetPose[controllerName](0, 3) = targetPoseVector.at(0);
545  m_prevTargetPose[controllerName](1, 3) = targetPoseVector.at(1);
546  m_prevTargetPose[controllerName](2, 3) = targetPoseVector.at(2);
547  }
548 
549  bool updateNullspace = false;
550  if (targetNullspaceJointAngle.size() > 0)
551  {
552  for (size_t i = 0; i < m_controllerJointNumbers[controllerName]; i++)
553  {
554  m_prevNullspaceJointTarget[controllerName](i) = targetNullspaceJointAngle[i];
555  }
556  }
557 
558 
559  auto type = control::common::ControllerTypeShort.from_name(controllerType);
560  switch (type) {
561  case control::common::ControllerType::TSImpedance:
562  ARMARX_CHECK(m_ctrlTSImpedance[controllerName]);
563  if (updataPose)
564  {
565  m_ctrlTSImpedance[controllerName]->config.desiredPose = m_prevTargetPose[controllerName];
566  }
567  if (updateNullspace)
568  {
569  m_ctrlTSImpedance[controllerName]->config.desiredNullspaceJointAngles = m_prevNullspaceJointTarget[controllerName];
570  }
571  m_ctrlTSImpedance[controllerName]->updateConfig();
572  break;
573  case control::common::ControllerType::TSAdmittance:
574  ARMARX_CHECK(m_ctrlTSAdmittance[controllerName]);
575  if (updataPose)
576  {
577  m_ctrlTSAdmittance[controllerName]->config.desiredPose = m_prevTargetPose[controllerName];
578  }
579  if (updateNullspace)
580  {
581  m_ctrlTSAdmittance[controllerName]->config.desiredNullspaceJointAngles = m_prevNullspaceJointTarget[controllerName];
582  }
583  m_ctrlTSAdmittance[controllerName]->updateConfig();
584  break;
585 
586  default:
587  ARMARX_INFO << "controller type: " << controllerType << " is not supported yet";
588  return false;
589  }
590  return true;
591  }
592 
593 
594  std::string Component::kinestheticTeaching(
595  const std::string& kinematicChainName,
596  const std::string& side,
597  const std::string& fileName,
598  const Ice::Current&)
599  {
600  /// ---------------------------------- creating zero torque controller ----------------------------------
601  ARMARX_INFO << "Kinesthetic teaching: Initialize zero torque controllers for " << VAROUT(kinematicChainName);
602  const std::string ctrl_class_name = "NJointZeroTorqueController";
603  const std::string ctrl_name = getDefaultName() + "_kinesthetic_teaching_" + ctrl_class_name;
604 
605  NJointZeroTorqueControllerConfigPtr config(new NJointZeroTorqueControllerConfig());
606  config->maxTorque = 0;
607 
608  auto nodeSet = m_robot->getRobotNodeSet(kinematicChainName);
609  for (auto& name : nodeSet->getNodeNames())
610  {
611  config->jointNames.push_back(name);
612  }
613  NJointZeroTorqueControllerInterfacePrx zeroTorqueController
614  = NJointZeroTorqueControllerInterfacePrx::checkedCast(
615  m_robot_unit->createNJointController(
616  ctrl_class_name,
617  ctrl_name,
618  config));
619  ARMARX_CHECK_NOT_NULL(zeroTorqueController);
620  ARMARX_INFO << "zero torque controller created";
621 
622  float motionThresholdRadian = 0.1f;
623  float noMotionTimeoutMs = 2000.0f;
624 
625  VirtualRobot::RobotNodeSetPtr rns = m_robot->getRobotNodeSet(kinematicChainName);
626  std::vector<std::string> jointNames = rns->getNodeNames();
627  IceUtil::Time start = TimeUtil::GetTime();
628  IceUtil::Time lastMotionTime = start;
629  bool initialMotionDetected = false;
630 
631  /// ---------------------------------- prepare data containers ----------------------------------
632  /// recording data
633  std::vector<std::vector<float>> jointData;
634  std::vector<std::vector<float>> cartData;
635  std::vector<float> timestamps;
636 
637  /// init data
638  Eigen::VectorXf jve = rns->getJointValuesEigen();
639  Eigen::Matrix4f initPose = rns->getTCP()->getPoseInRootFrame();
640  VirtualRobot::MathTools::Quaternion initQuat = VirtualRobot::MathTools::eigen4f2quat(initPose);
641  VirtualRobot::MathTools::Quaternion oldquatE = initQuat;
642 
643  /// joint space
644  for (size_t i = 0; i < jointNames.size(); i++)
645  {
646  std::vector<float> cjoint {};
647  // cjoint.push_back(jve(i));
648  jointData.push_back(cjoint);
649  }
650 
651  /// task space
652  float quatvec[] = {initPose(0, 3), initPose(1, 3), initPose(2, 3), initQuat.w, initQuat.x, initQuat.y, initQuat.z};
653  for (size_t i = 0; i < 7; ++i)
654  {
655  std::vector<float> cpos {};
656  // cpos.push_back(quatvec[i]);
657  cartData.push_back(cpos);
658  }
659 
660 
661  int motionStartIndex = 0;
662  int motionEndIndex = 0;
663 
664  int sigmaMs = 1;
665  int sampleMs = 10;
666 
667  float forceSpike = 0.0f;
668  Eigen::Vector3f initForce;
669  if (side == "right")
670  {
671  initForce = m_ft_r->getForce();
672  }
673  else
674  {
675  initForce = m_ft_l->getForce();
676  }
677  ARMARX_IMPORTANT << "=============== Waiting for force spike ===============";
678  while (forceSpike < 10.0f)
679  {
680  Eigen::Vector3f force;
681  if (side == "right")
682  {
683  force = m_ft_r->getForce();
684  }
685  else
686  {
687  force = m_ft_l->getForce();
688  }
689  forceSpike = (force - initForce).norm();
690  }
691  ARMARX_INFO << "Start the demonstration ...";
692 
693  zeroTorqueController->activateController();
694  while (true) // stop run function if returning true
695  {
696  synchronizeLocalClone(m_robot);
697 
698  IceUtil::Time now = TimeUtil::GetTime();
699 
700  Eigen::VectorXf jve2 = rns->getJointValuesEigen();
701  if ((jve2 - jve).norm() > motionThresholdRadian)
702  {
703  if (!initialMotionDetected)
704  {
705  ARMARX_IMPORTANT << "Start recording";
706  motionStartIndex = 0;
707  //start = now;
708  }
709  jve = jve2;
710  initialMotionDetected = true;
711  lastMotionTime = now;
712  }
713 
714  double t = (now - start).toSecondsDouble();
715 
716  std::vector<float> jvs = rns->getJointValues();
717  for (size_t i = 0; i < jvs.size(); i++)
718  {
719  jointData.at(i).push_back(jvs.at(i));
720  }
721 
722  Eigen::Matrix4f tcpPose = rns->getTCP()->getPoseInRootFrame();
723  cartData.at(0).push_back(tcpPose(0, 3));
724  cartData.at(1).push_back(tcpPose(1, 3));
725  cartData.at(2).push_back(tcpPose(2, 3));
726  VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(tcpPose);
727 
728  // check flipped orientation quaternion
729  double dotProduct = quat.w * oldquatE.w + quat.x * oldquatE.x + quat.y * oldquatE.y + quat.z * oldquatE.z;
730  if (dotProduct < 0)
731  {
732  cartData.at(3).push_back(-quat.w);
733  cartData.at(4).push_back(-quat.x);
734  cartData.at(5).push_back(-quat.y);
735  cartData.at(6).push_back(-quat.z);
736  oldquatE.w = -quat.w;
737  oldquatE.x = -quat.x;
738  oldquatE.y = -quat.y;
739  oldquatE.z = -quat.z;
740 
741  }
742  else
743  {
744  cartData.at(3).push_back(quat.w);
745  cartData.at(4).push_back(quat.x);
746  cartData.at(5).push_back(quat.y);
747  cartData.at(6).push_back(quat.z);
748 
749  oldquatE.w = quat.w;
750  oldquatE.x = quat.x;
751  oldquatE.y = quat.y;
752  oldquatE.z = quat.z;
753  }
754 
755  timestamps.push_back(t);
756 
757  if (initialMotionDetected && (now - lastMotionTime).toMilliSeconds() > noMotionTimeoutMs)
758  {
759  ARMARX_IMPORTANT << "Stop recording";
760  motionEndIndex = timestamps.size();// - noMotionTimeoutMs / sampleMs;
761  break;
762  }
763 
764  TimeUtil::USleep(sampleMs * 1000);
765  }
766  ARMARX_IMPORTANT << "Demonstration done! Processing data ...";
767 
768  zeroTorqueController->deactivateController();
769  while (zeroTorqueController->isControllerActive())
770  {
771  usleep(10000);
772  }
773  zeroTorqueController->deleteController();
774  enableVelocityMode();
775 
776  motionStartIndex -= sigmaMs / sampleMs * 2;
777  if (motionStartIndex < 0)
778  {
779  motionStartIndex = 0;
780  }
781  //motionEndIndex += sigmaMs / sampleMs * 2;
782  //if (motionEndIndex >= data.size())
783  //{
784  // motionEndIndex = data.size() - 1;
785  //}
786 
787  ARMARX_IMPORTANT << VAROUT(motionStartIndex) << VAROUT(motionEndIndex);
788 
789  ARMARX_IMPORTANT << "copy timestamps";
790  timestamps = std::vector<float>(timestamps.begin() + motionStartIndex, timestamps.begin() + motionEndIndex);
791  for (size_t n = 1; n < timestamps.size(); n++)
792  {
793  timestamps.at(n) = timestamps.at(n) - timestamps.at(0);
794  }
795  timestamps.at(0) = 0;
796 
797  ARMARX_IMPORTANT << VAROUT(timestamps);
798  std::vector<float> newT = math::TimeSeriesUtils::MakeTimestamps(timestamps.at(0), timestamps.at(timestamps.size() - 1), timestamps.size());
799  ARMARX_IMPORTANT << VAROUT(newT);
800 
801  {
802  StringFloatSeqDict plotFiltered;
803  StringFloatSeqDict plotOrig;
804  StringFloatSeqDict plotResampled;
805  for (size_t i = 0; i < jointData.size(); i++)
806  {
807  jointData.at(i) = std::vector<float>(jointData.at(i).begin() + motionStartIndex, jointData.at(i).begin() + motionEndIndex);
808  if (rns->getNode(i)->isLimitless())
809  {
810  math::LinearizeAngularTrajectory::LinearizeRef(jointData.at(i));
811  }
812  plotOrig[jointNames.at(i)] = jointData.at(i);
813  jointData.at(i) = math::TimeSeriesUtils::Resample(timestamps, jointData.at(i), newT);
814  plotResampled[jointNames.at(i)] = jointData.at(i);
815  jointData.at(i) = math::TimeSeriesUtils::ApplyGaussianFilter(jointData.at(i), sigmaMs * 0.001f, sampleMs * 0.001f, math::TimeSeriesUtils::BorderMode::Nearest);
816  plotFiltered[jointNames.at(i)] = jointData.at(i);
817  }
818  // getStaticPlotter()->addPlotWithTimestampVector("original joint motion", timestamps, plotOrig);
819  // getStaticPlotter()->addPlotWithTimestampVector("resampled joint motion", newT, plotResampled);
820  // getStaticPlotter()->addPlotWithTimestampVector("filtered joint motion", newT, plotFiltered);
821  }
822 
823  std::vector<std::string> cartName = {"x", "y", "z", "qw", "qx", "qy", "qz"};
824 
825  timestamps = newT;
826 
827  std::string trajName;
828  {
829  IceUtil::Time now = armarx::rtNow();
830  time_t timer = now.toSeconds();
831  struct tm* ts;
832  char buffer[80];
833  ts = localtime(&timer);
834  strftime(buffer, 80, "%Y-%m-%d-%H-%M-%S", ts);
835  std::string str(buffer);
836  trajName = fileName + "-" + str;
837  }
838 
839 
840  double cutoutTime = (noMotionTimeoutMs / 1000.0f) * 0.8;
841 
842  std::string packageName = "armarx_control";
843 
844  armarx::CMakePackageFinder finder(packageName);
845  std::string dataDir = finder.getDataDir() + "/" + packageName + "/kinesthetic_teaching/";
846  std::string returnFileNamePattern = dataDir + trajName;
847 
848  /// write task space data to RobotControllers Data dir
849  ARMARX_IMPORTANT << "writing task space data!";
850  std::string dmpForwardFile;
851  std::string dmpBackwardFile;
852  {
853  std::vector<std::string> header;
854  header.push_back("timestamp");
855  for (size_t i = 0; i < cartName.size(); ++i)
856  {
857  header.push_back(cartName.at(i));
858  }
859 
860  std::string ffname = trajName + "-ts-forward.csv";
861  std::string bfname = trajName + "-ts-backward.csv";
862  dmpForwardFile = dataDir + ffname;
863  dmpBackwardFile = dataDir + bfname;
864  std::vector<std::string> fflist;
865  fflist.push_back(ffname);
866 
867  std::vector<std::string> bflist;
868  bflist.push_back(bfname);
869 
870  CsvWriter fwriter(dmpForwardFile, header, false);
871  CsvWriter bwriter(dmpBackwardFile, header, false);
872 
873  ARMARX_IMPORTANT << "writing taskspace trajectory as " << ffname << " and " << bfname;
874 
875  double endTime = timestamps.at(timestamps.size() - 1);
876  for (size_t n = 0; n < timestamps.size(); n++)
877  {
878 
879  std::vector<float> row;
880  row.push_back(timestamps.at(n));
881  for (size_t i = 0; i < cartData.size(); i++)
882  {
883  row.push_back(cartData.at(i).at(n));
884  }
885  fwriter.write(row);
886 
887  if (endTime - timestamps.at(n) < cutoutTime)
888  {
889  Eigen::Matrix4f endPose = VirtualRobot::MathTools::quat2eigen4f(row.at(5), row.at(6), row.at(7), row.at(4));
890  endPose(0, 3) = row.at(1);
891  endPose(1, 3) = row.at(2);
892  endPose(2, 3) = row.at(3);
893  break;
894  }
895  }
896  fwriter.close();
897  int nt = 0;
898  for (int n = timestamps.size() - 1; n >= 0 && nt < static_cast<int>(timestamps.size()); n--, nt++)
899  {
900  std::vector<float> row;
901  row.push_back(timestamps.at(nt));
902  for (size_t i = 0; i < cartData.size(); i++)
903  {
904  row.push_back(cartData.at(i).at(n));
905  }
906  bwriter.write(row);
907 
908  if (n == 0)
909  {
910  Eigen::Matrix4f endPose = VirtualRobot::MathTools::quat2eigen4f(row.at(5), row.at(6), row.at(7), row.at(4));
911  endPose(0, 3) = row.at(1);
912  endPose(1, 3) = row.at(2);
913  endPose(2, 3) = row.at(3);
914  break;
915  }
916  }
917  bwriter.close();
918  }
919 
920  /// write joint space data to RobotControllers Data dir
921  ARMARX_IMPORTANT << "writing joint space data!";
922  std::string dmpJSForwardFile;
923  std::string dmpJSBackwardFile;
924  {
925  std::vector<std::string> header;
926  header.push_back("timestamp");
927  for (size_t i = 0; i < jointNames.size(); ++i)
928  {
929  header.push_back(jointNames.at(i));
930  }
931 
932  std::string ffname = trajName + "-js-forward.csv";
933  std::string bfname = trajName + "-js-backward.csv";
934  dmpJSForwardFile = dataDir + ffname;
935  dmpJSBackwardFile = dataDir + bfname;
936  std::vector<std::string> fflist;
937  fflist.push_back(ffname);
938 
939  std::vector<std::string> bflist;
940  bflist.push_back(bfname);
941 
942  CsvWriter fwriter(dmpJSForwardFile, header, false);
943  CsvWriter bwriter(dmpJSBackwardFile, header, false);
944 
945  ARMARX_IMPORTANT << "writing jointspace trajectory as " << ffname << " and " << bfname;
946 
947  double endTime = timestamps.at(timestamps.size() - 1);
948  for (size_t n = 0; n < timestamps.size(); n++)
949  {
950 
951  std::vector<float> row;
952  row.push_back(timestamps.at(n));
953  for (size_t i = 0; i < jointNames.size(); i++)
954  {
955  row.push_back(jointData.at(i).at(n));
956  }
957  fwriter.write(row);
958 
959  if (endTime - timestamps.at(n) < cutoutTime)
960  {
961  std::vector<float> endJointValue;
962  for (size_t i = 0; i < jointNames.size(); i++)
963  {
964  endJointValue.push_back(jointData.at(i).at(n));
965  }
966  break;
967  }
968  }
969  fwriter.close();
970  ARMARX_INFO << "forward joint space data written to fine!";
971  int nt = 0;
972  for (int n = timestamps.size() - 1; n >= 0 && nt < static_cast<int>(timestamps.size()); n--, nt++)
973  {
974  std::vector<float> row;
975  row.push_back(timestamps.at(nt));
976  for (size_t i = 0; i < jointNames.size(); i++)
977  {
978  row.push_back(jointData.at(i).at(n));
979  }
980  bwriter.write(row);
981 
982  if (n == 0)
983  {
984  std::vector<float> endJointValue;
985  for (size_t i = 0; i < jointNames.size(); i++)
986  {
987  endJointValue.push_back(jointData.at(i).at(n));
988  }
989  break;
990  }
991  }
992  bwriter.close();
993  }
994  return returnFileNamePattern;
995 // return "";
996  }
997 
998  void
999  Component::enableVelocityMode(const Ice::Current&)
1000  {
1001  ARMARX_INFO << "enable velocity mode";
1002  // left arm
1003  {
1004  std::vector<std::string> joint_names = m_rns_l->getNodeNames();
1005  NameValueMap target_vels;
1006  NameControlModeMap control_modes;
1007  for (const std::string& joint_name : joint_names)
1008  {
1009  control_modes[joint_name] = eVelocityControl;
1010  target_vels[joint_name] = 0;
1011  }
1012  }
1013  // right arm
1014  {
1015  std::vector<std::string> joint_names = m_rns_r->getNodeNames();
1016  NameValueMap target_vels;
1017  NameControlModeMap control_modes;
1018  for (const std::string& joint_name : joint_names)
1019  {
1020  control_modes[joint_name] = eVelocityControl;
1021  target_vels[joint_name] = 0;
1022  }
1023  }
1024 // // platform
1025 // {
1026 // m_platform->move(0.0f, 0.0f, 0.0f);
1027 // }
1028  }
1029 
1030 
1031  Ice::FloatSeq
1032  Component::getPoseInRootFrame(const std::string& nodeName, const Ice::Current&)
1033  {
1034  ARMARX_CHECK(synchronizeLocalClone(m_robot));
1035  Eigen::Matrix4f pose = m_robot->getRobotNode(nodeName)->getPoseInRootFrame();
1036  ARMARX_INFO << VAROUT(pose);
1037  std::vector<Ice::Float> pose_seq;
1038  for (int i = 0; i < pose.rows(); i++){
1039  for (int j = 0; j < pose.cols(); j++){
1040  pose_seq.push_back(pose(i, j));
1041  }
1042  }
1043  return pose_seq;
1044  }
1045 
1046  Ice::FloatSeq
1047  Component::getPrevTargetPose(
1048  const std::string &controllerName,
1049  const Ice::Current&)
1050  {
1051  std::vector<Ice::Float> pose_seq;
1052  for (int i = 0; i < m_prevTargetPose[controllerName].rows(); i++){
1053  for (int j = 0; j < m_prevTargetPose[controllerName].cols(); j++){
1054  pose_seq.push_back(m_prevTargetPose[controllerName](i, j));
1055  }
1056  }
1057  return pose_seq;
1058  }
1059 
1060  Ice::FloatSeq
1061  Component::getPrevNullspaceTargetAngle(
1062  const std::string &controllerName,
1063  const Ice::Current&)
1064  {
1065  std::vector<Ice::Float> angle_seq;
1066  for (int i = 0; i < m_prevNullspaceJointTarget[controllerName].rows(); i++){
1067  angle_seq.push_back(m_prevNullspaceJointTarget[controllerName](i));
1068  }
1069  return angle_seq;
1070  }
1071 
1072 
1073  std::string
1074  Component::getDefaultName() const
1075  {
1076  return Component::defaultName;
1077  }
1078 
1079 
1080  std::string
1081  Component::GetDefaultName()
1082  {
1083  return Component::defaultName;
1084  }
1085 
1086 
1087  /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
1088  void
1089  Component::createRemoteGuiTab()
1090  {
1091  using namespace armarx::RemoteGui::Client;
1092 
1093  // Setup the widgets.
1094 
1095  tab.boxLayerName.setValue(properties.boxLayerName);
1096 
1097  tab.numBoxes.setValue(properties.numBoxes);
1098  tab.numBoxes.setRange(0, 100);
1099 
1100  tab.drawBoxes.setLabel("Draw Boxes");
1101 
1102  // Setup the layout.
1103 
1104  GridLayout grid;
1105  int row = 0;
1106  {
1107  grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
1108  ++row;
1109 
1110  grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
1111  ++row;
1112 
1113  grid.add(tab.drawBoxes, {row, 0}, {2, 1});
1114  ++row;
1115  }
1116 
1117  VBoxLayout root = {grid, VSpacer()};
1118  RemoteGui_createTab(getName(), root, &tab);
1119  }
1120 
1121 
1122  void
1123  Component::RemoteGui_update()
1124  {
1125  if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
1126  {
1127  std::scoped_lock lock(propertiesMutex);
1128  properties.boxLayerName = tab.boxLayerName.getValue();
1129  properties.numBoxes = tab.numBoxes.getValue();
1130 
1131  {
1132  setDebugObserverDatafield("numBoxes", properties.numBoxes);
1133  setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
1134  sendDebugObserverBatch();
1135  }
1136  }
1137  if (tab.drawBoxes.wasClicked())
1138  {
1139  // Lock shared variables in methods running in separate threads
1140  // and pass them to functions. This way, the called functions do
1141  // not need to think about locking.
1142  std::scoped_lock lock(propertiesMutex);
1143  drawBoxes(properties, arviz);
1144  }
1145  }
1146  */
1147 
1148 
1149  /* (Requires the armarx::ArVizComponentPluginUser.)
1150  void
1151  Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
1152  {
1153  // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
1154  // See the ArVizExample in RobotAPI for more examples.
1155 
1156  viz::Layer layer = arviz.layer(p.boxLayerName);
1157  for (int i = 0; i < p.numBoxes; ++i)
1158  {
1159  layer.add(viz::Box("box_" + std::to_string(i))
1160  .position(Eigen::Vector3f(i * 100, 0, 0))
1161  .size(20).color(simox::Color::blue()));
1162  }
1163  arviz.commit(layer);
1164  }
1165  */
1166 
1167 
1168  ARMARX_DECOUPLED_REGISTER_COMPONENT(Component);
1169 
1170 } /// namespace armarx::control::components::controller_creator
armarx::control::components::controller_creator::Component::updateTargetNullspaceJointAngles
bool updateTargetNullspaceJointAngles(const std::string &controllerName, const std::string &controllerType, const Ice::FloatSeq &targetNullspaceJointAngle, const Ice::Current &=Ice::emptyCurrent) override
Definition: Component.cpp:477
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::core::time::Clock::WaitFor
static void WaitFor(const Duration &duration)
Wait for a certain duration on the virtual clock.
Definition: Clock.cpp:104
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:159
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:594
armarx::control::components::controller_creator::Component
Definition: Component.h:54
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:53
CsvWriter.h
armarx::ManagedIceObject::addPlugin
PluginT * addPlugin(const std::string prefix="", ParamsT &&...params)
Definition: ManagedIceObject.h:182
armarx::navigation::platform_controller::platform_global_trajectory::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformGlobalTrajectoryController.h:93
armarx::control::components::controller_creator::Component::Component
Component()
Definition: Component.cpp:46
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::control::components::controller_creator::Component::getPrevNullspaceTargetAngle
Ice::FloatSeq getPrevNullspaceTargetAngle(const std::string &controllerName, const Ice::Current &=Ice::emptyCurrent) override
Definition: Component.cpp:1061
armarx::control::common::ControllerTypeNames
const simox::meta::EnumNames< ControllerType > ControllerTypeNames
Definition: type.h:58
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::control::common::ControllerType::TSImpedanceMP
@ TSImpedanceMP
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::CsvWriter::close
void close()
Definition: CsvWriter.cpp:77
armarx::TimeUtil::USleep
static int USleep(long usec)
like timed_wait on boost condition_variables, but with timeserver support
Definition: TimeUtil.cpp:148
armarx::control::components::controller_creator::Component::getPoseInRootFrame
Ice::FloatSeq getPoseInRootFrame(const std::string &nodeName, const Ice::Current &=Ice::emptyCurrent) override
Definition: Component.cpp:1032
armarx::math::TimeSeriesUtils::MakeTimestamps
static std::vector< float > MakeTimestamps(float start, float end, size_t count)
Definition: TimeSeriesUtils.cpp:114
armarx::control::components::controller_creator::Component::getPrevTargetPose
Ice::FloatSeq getPrevTargetPose(const std::string &controllerName, const Ice::Current &=Ice::emptyCurrent) override
Definition: Component.cpp:1047
armarx::CMakePackageFinder::getDataDir
std::string getDataDir() const
Definition: CMakePackageFinder.h:176
armarx::control::client::ComponentPlugin::getRobotUnitPlugin
armarx::plugins::RobotUnitComponentPlugin & getRobotUnitPlugin()
Definition: ComponentPlugin.h:106
armarx::control::common::ControllerTypeShort
const simox::meta::EnumNames< ControllerType > ControllerTypeShort
Definition: type.h:44
Metronome.h
armarx::control::components::controller_creator::Component::~Component
~Component() override
armarx::control::client::ComponentPluginUser::getControlComponentPlugin
ComponentPlugin & getControlComponentPlugin()
Definition: ComponentPlugin.cpp:70
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::control::common::ControllerType::TSImpedance
@ TSImpedance
armarx::control::components::controller_creator::Component::onInitComponent
void onInitComponent() override
Definition: Component.cpp:90
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:1081
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::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
Decoupled.h
armarx::control::components::controller_creator::Component::createController
std::string createController(const std::string &controllerNamePrefix, const std::string &robotNodeSet, const std::string &controllerType, const std::string &configFilename, const Ice::Current &=Ice::emptyCurrent) override
Definition: Component.cpp:396
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
armarx::viz::data::ElementFlags::names
const simox::meta::IntEnumNames names
Definition: json_elements.cpp:14
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::control::components::controller_creator::Component::getDefaultName
std::string getDefaultName() const override
Definition: Component.cpp:1074
IceInternal::ProxyHandle<::IceProxy::armarx::RobotUnitInterface >
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
armarx::control::components::controller_creator::Component::updateTargetPose
bool updateTargetPose(const std::string &controllerName, const std::string &controllerType, const Ice::FloatSeq &targetPoseVector, const Ice::Current &=Ice::emptyCurrent) override
Definition: Component.cpp:429
Logging.h
armarx::control::components::controller_creator::ComponentInterface::enableVelocityMode
void enableVelocityMode()
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::control::components::controller_creator::Component::onConnectComponent
void onConnectComponent() override
Definition: Component.cpp:102
armarx::PackagePath
Definition: PackagePath.h:55
armarx::control::components::controller_creator
Definition: Component.cpp:40
armarx::control::components::controller_creator::Component::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: Component.cpp:56
armarx::control::common::ft::ForceTorqueUtility
Definition: ForceTorqueUtility.h:39
armarx::control::components::controller_creator::Component::onDisconnectComponent
void onDisconnectComponent() override
Definition: Component.cpp:153
armarx::core::time::Duration::MilliSeconds
static Duration MilliSeconds(std::int64_t milliSeconds)
Constructs a duration in milliseconds.
Definition: Duration.cpp:55
armarx::control::components::controller_creator::Component::updateTarget
bool updateTarget(const std::string &controllerName, const std::string &controllerType, const Ice::FloatSeq &targetPoseVector, const Ice::FloatSeq &targetNullspaceJointAngle, const Ice::Current &=Ice::emptyCurrent) override
Definition: Component.cpp:519
armarx::plugins::RobotUnitComponentPlugin::getRobotUnit
RobotUnitInterfacePrx getRobotUnit() const
Definition: RobotUnitComponentPlugin.cpp:112
armarx::control::common::ControllerType::TSAdmittance
@ TSAdmittance
norm
double norm(const Point &a)
Definition: point.hpp:94
armarx::rtNow
IceUtil::Time rtNow()
Definition: RtTiming.h:40