MixedImpedanceVelocityController.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 ...
17  * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
18  * @date 2024
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
24 
25 #include <VirtualRobot/MathTools.h>
26 #include <VirtualRobot/Nodes/RobotNode.h>
27 #include <VirtualRobot/RobotNodeSet.h>
28 
30 #include <ArmarXCore/core/PackagePath.h> // for GUI
34 
40 
41 #include <armarx/control/common/aron_conversions.h> // for GUI
44 
46 {
47  NJointControllerRegistration<NJointTaskspaceMixedImpedanceVelocityController>
49  "NJointTaskspaceMixedImpedanceVelocityController");
50 
51  void
53  ArmPtr& arm,
54  Config& cfg,
55  VirtualRobot::RobotPtr& nonRtRobotPtr)
56  {
57  arm->kinematicChainName = nodeSetName;
58  VirtualRobot::RobotNodeSetPtr rtRns =
59  rtGetRobot()->getRobotNodeSet(arm->kinematicChainName);
60  VirtualRobot::RobotNodeSetPtr nonRtRns =
61  nonRtRobotPtr->getRobotNodeSet(arm->kinematicChainName);
62  ARMARX_CHECK_EXPRESSION(rtRns) << arm->kinematicChainName;
63  ARMARX_CHECK_EXPRESSION(nonRtRns) << arm->kinematicChainName;
64  ARMARX_IMPORTANT << "Creating Taskspace Impedance controller with kinematic chain: "
65  << arm->kinematicChainName << " with num of joints: (RT robot) "
66  << rtRns->getSize() << ", and (non-RT robot) " << nonRtRns->getSize();
67 
68  std::vector<size_t> jointIDTorqueMode;
69  std::vector<size_t> jointIDVelocityMode;
70  arm->jointNames.clear();
71  for (size_t i = 0; i < rtRns->getSize(); ++i)
72  {
73  std::string jointName = rtRns->getNode(i)->getName();
74  arm->jointNames.push_back(jointName);
75 
76  bool foundControlDevice = false;
77  auto it = std::find(
78  cfg.jointNameListTorque.begin(), cfg.jointNameListTorque.end(), jointName);
79  if (it != cfg.jointNameListTorque.end())
80  {
81  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
83  auto* casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>();
84  ARMARX_CHECK_EXPRESSION(casted_ct);
85  arm->targetsTorque.push_back(casted_ct);
86  jointIDTorqueMode.push_back(i);
87  foundControlDevice = true;
88  }
89 
90  it = std::find(
91  cfg.jointNameListVelocity.begin(), cfg.jointNameListVelocity.end(), jointName);
92  if (it != cfg.jointNameListVelocity.end())
93  {
94  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
96  auto* casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
97  ARMARX_CHECK_EXPRESSION(casted_ct);
98  arm->targetsVel.push_back(casted_ct);
99  jointIDVelocityMode.push_back(i);
100  foundControlDevice = true;
101  }
102  if (not foundControlDevice)
103  {
104  auto namesTorque = armarx::control::common::sVecToString(cfg.jointNameListTorque);
105  auto namesVelocity =
106  armarx::control::common::sVecToString(cfg.jointNameListVelocity);
107  ARMARX_ERROR << "Does not found valid control device for joint name: " << jointName
108  << "Please check torque controlled joints: [" << namesTorque
109  << "] and velocity controlled joints: [" << namesVelocity << "].";
110  }
111 
112  const SensorValueBase* sv = useSensorValue(jointName);
114  arm->sensorDevices.append(sv, jointName);
115  };
116  ARMARX_DEBUG << "Number of torque controlled joints " << jointIDTorqueMode.size();
117  ARMARX_DEBUG << "Number of velocity controlled joints " << jointIDVelocityMode.size();
118  ARMARX_CHECK_EQUAL(rtRns->getNodeNames().size(),
119  jointIDTorqueMode.size() + jointIDVelocityMode.size());
120 
121  arm->controller.initialize(nonRtRns, rtRns, jointIDTorqueMode, jointIDVelocityMode);
122  arm->controller.ftsensor.initialize(rtRns, robotUnit, cfg.ftConfig);
123 
124  validateConfigData(cfg, arm);
125  arm->rtConfig = cfg;
126  arm->nonRtConfig = cfg;
127  arm->rtStatus.reset(rtRns->getSize(), jointIDTorqueMode.size(), jointIDVelocityMode.size());
128  }
129 
132  const NJointControllerConfigPtr& config,
133  const VirtualRobot::RobotPtr&) :
134  robotUnit(robotUnit)
135  {
137 
138  ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
140  userConfig.fromAron(cfg->config);
141 
143  nonRtRobot = robotUnit->cloneRobot();
144  robotUnit->updateVirtualRobot(nonRtRobot);
145 
146  for (auto& pair : userConfig.limbs)
147  {
148  limb.emplace(pair.first, std::make_unique<ArmData>());
149  limbInit(pair.first, limb.at(pair.first), pair.second, nonRtRobot);
150  controllableNodeSets.emplace(pair.first, nonRtRobot->getRobotNodeSet(pair.first));
151  }
152 
153  if (not userConfig.hands.empty())
154  {
155  hands = std::make_shared<core::HandControlBase>(nonRtRobot, userConfig.hands);
156  for (auto& pair : userConfig.hands)
157  {
158  for (auto& jointName : hands->hands.at(pair.first)->jointNames)
159  {
160  hands->appendDevice(pair.first,
161  useSensorValue(jointName),
162  useControlTarget(jointName, ControlModes::Position1DoF),
163  jointName);
164  }
165  ARMARX_INFO << "construction of hand "
166  << hands->hands.at(pair.first)->kinematicChainName;
167  controllableNodeSets.emplace(pair.first, nonRtRobot->getRobotNodeSet(pair.first));
168  }
169  }
170  }
171 
172  std::string
174  {
175  return "NJointTaskspaceMixedImpedanceVelocityController";
176  }
177 
178  void
180  {
181  std::string taskName = getClassName() + "AdditionalTask";
182  runTask(taskName,
183  [&]
184  {
186  4); // please don't set this to 0 as it is the rtRun/control thread
189 
190  CycleUtil c(1);
191  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
192  ARMARX_IMPORTANT << "Create a new thread for " << getClassName();
193  while (getState() == eManagedIceObjectStarted)
194  {
195  if (isControllerActive())
196  {
197  additionalTask();
198  }
199  c.waitForCycleDuration();
200  }
201  });
202  }
203 
204  void
206  {
207  arm->bufferConfigNonRtToRt.getWriteBuffer() = arm->nonRtConfig;
208  arm->bufferConfigNonRtToRt.commitWrite();
209  }
210 
211  void
213  {
214  // bool rtSafe = additionalTaskUpdateStatus();
215  auto [rtTargetSafe, ftSafe] = additionalTaskUpdateStatus();
217  if (not rtTargetSafe)
218  {
220  }
221  }
222 
223  std::tuple<bool, bool>
225  {
226  robotUnit->updateVirtualRobot(nonRtRobot);
227  bool rtSafe = true;
228  bool rtTargetSafe = true;
229  bool forceTorqueSafe = true;
230  for (auto& pair : limb)
231  {
232  auto& arm = pair.second;
233  arm->nonRtConfig = arm->bufferConfigUserToNonRt.getUpToDateReadBuffer();
234  arm->rtStatusInNonRT = arm->bufferRtStatusToNonRt.getUpToDateReadBuffer();
235  rtSafe = rtSafe and pair.second->rtStatusInNonRT.rtSafe;
236  rtTargetSafe = rtTargetSafe and pair.second->rtStatusInNonRT.rtTargetSafe;
237  forceTorqueSafe = forceTorqueSafe and pair.second->rtStatusInNonRT.forceTorqueSafe;
238  }
239  if (hands)
240  {
241  hands->nonRTUpdateStatus();
242  }
243  return std::make_tuple(rtTargetSafe, forceTorqueSafe);
244  }
245 
246  void
248  {
249  for (auto& pair : limb)
250  {
251  limbNonRT(pair.second);
252  }
253  if (hands)
254  {
255  hands->nonRTSetTarget();
256  }
257  }
258 
259  void
261  {
262  for (auto& pair : limb)
263  {
264  if (not pair.second->rtReady)
265  continue;
266 
267  if (not pair.second->rtStatusInNonRT.rtTargetSafe)
268  {
269  // pair.second->rtStatusInNonRT =
270  // pair.second->bufferRtStatusToNonRt.getUpToDateReadBuffer();
272  pair.second->rtStatusInNonRT.currentPose,
273  pair.second->rtStatusInNonRT.desiredPoseUnsafe,
274  "current pose",
275  "desired pose");
276  }
277  }
278  }
279 
280  /// -------------------------------- Real time cotnrol -----------------------------------------
281  void
283  const double deltaT)
284  {
285  arm->rtConfig = arm->bufferConfigNonRtToRt.getUpToDateReadBuffer();
286  arm->rtStatus.deltaT = deltaT;
287  arm->rtStatus.accumulateTime += deltaT;
288 
289  arm->controller.ftsensor.updateStatus(arm->rtConfig.ftConfig,
290  arm->rtStatus.currentForceTorque,
291  arm->rtStatus.deltaT,
292  arm->rtFirstRun.load());
293  arm->rtStatus.safeFTGuardOffset.head<3>() =
294  arm->controller.ftsensor.getSafeGuardForceOffset();
295 
296  arm->sensorDevices.updateJointValues(
297  arm->rtStatus.jointPos, arm->rtStatus.jointVel, arm->rtStatus.jointTor);
298  // rtGetRobot()->getRobotNodeSet(arm->kinematicChainName)
299 
300 
301  arm->bufferRtStatusToNonRt.getWriteBuffer() = arm->rtStatus;
302  arm->bufferRtStatusToNonRt.commitWrite();
303  arm->bufferRtStatusToUser.getWriteBuffer() = arm->rtStatus;
304  arm->bufferRtStatusToUser.commitWrite();
305  }
306 
307  void
309  ArmPtr& arm,
310  const size_t nDoFTorque,
311  const size_t nDoFVelocity,
312  const Eigen::VectorXf& targetTorque,
313  const Eigen::VectorXf& targetVelocity)
314  {
315  for (size_t i = 0; i < nDoFTorque; i++)
316  {
317  auto jointIdx = arm->controller.jointIDTorqueMode[i];
318  arm->targetsTorque.at(i)->torque = targetTorque(jointIdx);
319  if (!arm->targetsTorque.at(i)->isValid())
320  {
321  arm->targetsTorque.at(i)->torque = 0;
322  }
323  }
324  for (size_t i = 0; i < nDoFVelocity; i++)
325  {
326  auto jointIdx = arm->controller.jointIDVelocityMode[i];
327  arm->targetsVel.at(i)->velocity = targetVelocity(jointIdx);
328  // if (!targets.at(i)->isValid() || fabs(targets.at(i)->velocity) > cfg->maxJointVelocity)
329  if (!arm->targetsVel.at(i)->isValid())
330  {
331  arm->targetsVel.at(i)->velocity = 0;
332  }
333  }
334  arm->bufferRtStatusToOnPublish.getWriteBuffer() = arm->rtStatus;
335  arm->bufferRtStatusToOnPublish.commitWrite();
336 
337  arm->bufferConfigRtToOnPublish.getWriteBuffer() = arm->rtConfig;
338  arm->bufferConfigRtToOnPublish.commitWrite();
339 
340  arm->bufferConfigRtToUser.getWriteBuffer() = arm->rtConfig;
341  arm->bufferConfigRtToUser.commitWrite();
342 
343  if (arm->rtFirstRun.load())
344  {
345  arm->rtFirstRun.store(false);
346  arm->rtReady.store(true);
347  }
348  }
349 
350  void
352  {
353  double time_measure = IceUtil::Time::now().toMicroSecondsDouble();
354  limbRTUpdateStatus(arm, deltaT);
355  double time_update_status = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
356 
357  arm->controller.run(arm->rtConfig, arm->rtStatus);
358  double time_run_rt = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
359 
360  limbRTSetTarget(arm,
361  arm->rtStatus.nDoFTorque,
362  arm->rtStatus.nDoFVelocity,
363  arm->rtStatus.desiredJointTorques,
364  arm->rtStatus.desiredJointVelocity);
365 
366  double time_set_target = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
367  time_measure = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
368 
369  if (time_measure > 200)
370  {
371  ARMARX_RT_LOGF_WARN("---- rt too slow: "
372  "time_update_status: %.2f\n"
373  "run_rt_limb: %.2f\n"
374  "set_target_limb: %.2f\n"
375  "time all: %.2f\n",
376  time_update_status, // 0-1 us
377  time_run_rt - time_update_status, //
378  time_set_target - time_run_rt, //
379  time_measure)
380  .deactivateSpam(1.0f); // 0-1 us
381  }
382  }
383 
384  void
386  const IceUtil::Time& /*sensorValuesTimestamp*/,
387  const IceUtil::Time& timeSinceLastIteration)
388  {
389  double deltaT = timeSinceLastIteration.toSecondsDouble();
390  for (auto& pair : limb)
391  {
392  limbRT(pair.second, deltaT);
393  }
394  if (hands)
395  {
396  hands->updateRTStatus(deltaT);
397  hands->setTargets();
398  }
399  }
400 
401  /// ------------------------------ update/get config -------------------------------------------
402  void
405  const Ice::Current& iceCurrent)
406  {
407  auto prevCfg = userConfig;
408  userConfig.fromAron(dto);
409 
410  for (auto& pair : userConfig.limbs)
411  {
412  validateConfigData(pair.second, limb.at(pair.first));
414  pair.second.desiredPose,
415  prevCfg.limbs.at(pair.first).desiredPose,
416  "new desired pose",
417  "previous desired pose",
418  pair.second.safeDistanceMMToGoal,
419  pair.second.safeRotAngleDegreeToGoal,
420  "updateConfig_" + pair.first))
421  {
422  ARMARX_INFO << "use the existing target pose";
423  pair.second.desiredPose = prevCfg.limbs.at(pair.first).desiredPose;
424  }
425  limb.at(pair.first)->bufferConfigUserToNonRt.getWriteBuffer() = pair.second;
426  limb.at(pair.first)->bufferConfigUserToNonRt.commitWrite();
427  }
428  if (hands)
429  {
430  hands->updateConfig(userConfig.hands);
431  }
432  }
433 
436  {
437  for (auto& pair : limb)
438  {
439  userConfig.limbs.at(pair.first) =
440  pair.second->bufferConfigRtToUser.getUpToDateReadBuffer();
441  }
442  if (hands)
443  {
444  hands->getConfig(userConfig.hands);
445  }
446  return userConfig.toAronDTO();
447  }
448 
451  {
452  for (auto& pair : limb)
453  {
454  userConfig.limbs.at(pair.first) =
455  pair.second->bufferConfigRtToUser.getUpToDateReadBuffer();
456  }
457  if (hands)
458  {
459  hands->getConfig(userConfig.hands);
460  }
461  return userConfig.toAronDTO();
462  }
463 
464  void
466  const std::string& nodeSetName,
467  const bool forceGuard,
468  const bool torqueGuard,
469  const Ice::Current& iceCurrent)
470  {
471  auto it = userConfig.limbs.find(nodeSetName);
472  if (it != userConfig.limbs.end())
473  {
474  it->second.ftConfig.enableSafeGuardForce = forceGuard;
475  it->second.ftConfig.enableSafeGuardTorque = torqueGuard;
476  limb.at(nodeSetName)->controller.ftsensor.enableSafeGuard(forceGuard, torqueGuard);
478  << "reset safe guard with force torque sensors: safe? "
479  << limb.at(nodeSetName)->controller.ftsensor.isSafe(it->second.ftConfig);
480  limb.at(nodeSetName)->bufferConfigUserToNonRt.getWriteBuffer() = it->second;
481  limb.at(nodeSetName)->bufferConfigUserToNonRt.commitWrite();
482  }
483  else
484  {
485  ARMARX_WARNING << "no robot node set with name " << nodeSetName
486  << " found in the controllers.";
487  }
488  }
489 
490  /// -------------------------------- Other interaces -------------------------------------------
491  bool
493  const std::string& nodeSetName,
494  const Ice::Current& iceCurrent)
495  {
496  auto it = userConfig.limbs.find(nodeSetName);
497  if (it != userConfig.limbs.end())
498  {
499  return limb.at(nodeSetName)->controller.ftsensor.ftSafe.load();
500  }
501 
502  ARMARX_WARNING << "no robot node set with name " << nodeSetName
503  << " found in the controllers.";
504  return false;
505  }
506 
507  Ice::FloatSeq
509  const Ice::Current& iceCurrent)
510  {
511  std::vector<float> tcpVel;
512  auto& arm = limb.at(rns);
513  auto s = arm->bufferRtStatusToUser.getUpToDateReadBuffer();
514  for (int i = 0; i < s.currentTwist.size(); i++)
515  {
516  tcpVel.push_back(s.currentTwist[i]);
517  }
518  return tcpVel;
519  }
520 
521  Ice::DoubleSeq
523  const std::string& nodeSetName,
524  const Ice::Current& iceCurrent)
525  {
526  auto search = limb.find(nodeSetName);
527  if (search == limb.end())
528  {
529  ARMARX_WARNING << "requested node set " << nodeSetName
530  << "does not exist in the controller";
531  return std::vector<double>{};
532  }
533 
534  auto& arm = limb.at(nodeSetName);
535  auto currentPose = arm->bufferRtStatusToOnPublish.getUpToDateReadBuffer().currentPose;
536  ARMARX_DEBUG << "returning current pose" << VAROUT(currentPose);
537  return common::mat4ToDVec(currentPose);
538  }
539 
540  bool
542  const TargetPoseMap& targetPoseMap,
543  const TargetNullspaceMap& targetNullspaceMap,
544  const Ice::Current& iceCurrent)
545  {
546  for (auto& pair : userConfig.limbs)
547  {
548  for (size_t i = 0; i < 4; ++i)
549  {
550  for (int j = 0; j < 4; ++j)
551  {
552  pair.second.desiredPose(i, j) = targetPoseMap.at(pair.first)[i][j];
553  }
554  }
555  if (targetNullspaceMap.at(pair.first).size() > 0)
556  {
557  const size_t nDoF = pair.second.desiredNullspaceJointAngles.value().size();
558  ARMARX_CHECK_EQUAL(targetNullspaceMap.at(pair.first).size(), nDoF)
559  << "the joint numbers does not match";
560  for (size_t i = 0; i < nDoF; ++i)
561  {
562  pair.second.desiredNullspaceJointAngles.value()(i) =
563  targetNullspaceMap.at(pair.first)[i];
564  }
565  }
566  limb.at(pair.first)->bufferConfigUserToNonRt.getWriteBuffer() = pair.second;
567  limb.at(pair.first)->bufferConfigUserToNonRt.commitWrite();
568  }
569  return true;
570  }
571 
572  void
574  ArmPtr& arm)
575  {
576  const auto nDoF = arm->controller.numOfJoints;
577 
578  const auto checkSize = [nDoF](const auto& v) { ARMARX_CHECK_EQUAL(v.rows(), nDoF); };
579  const auto checkNonNegative = [](const auto& v) { ARMARX_CHECK((v.array() >= 0).all()); };
580 
581  if (!configData.desiredNullspaceJointAngles.has_value())
582  {
583  if (!isControllerActive())
584  {
585  ARMARX_INFO << "No user defined nullspace target, reset to zero with "
586  << VAROUT(nDoF);
587  configData.desiredNullspaceJointAngles = Eigen::VectorXf::Zero(nDoF);
588  arm->reInitPreActivate.store(true);
589  }
590  else
591  {
592  auto currentValue =
593  arm->bufferConfigRtToUser.getUpToDateReadBuffer().desiredNullspaceJointAngles;
595  << "Controller is active, but no user defined nullspace target. \n"
596  "You should first get up-to-date config of this controller and then update "
597  "it:\n"
598  " auto cfg = ctrl->getConfig(); \n"
599  " cfg.desiredPose = xxx;\n"
600  " ctrl->updateConfig(cfg);\n"
601  "Now, I decide by myself to use the existing values of nullspace target\n"
602  << currentValue.value();
603  configData.desiredNullspaceJointAngles = currentValue;
604  }
605  }
606  ARMARX_CHECK_GREATER(configData.safeDistanceMMToGoal, 100.0f);
607  checkSize(configData.desiredNullspaceJointAngles.value());
608  checkSize(configData.kdNullspaceTorque);
609  checkSize(configData.kpNullspaceTorque);
610  checkSize(configData.kdNullspaceVel);
611  checkSize(configData.kpNullspaceVel);
612 
613  checkNonNegative(configData.kdNullspaceTorque);
614  checkNonNegative(configData.kpNullspaceTorque);
615  checkNonNegative(configData.kpNullspaceVel);
616  checkNonNegative(configData.kdNullspaceVel);
617  checkNonNegative(configData.kdImpedance);
618  checkNonNegative(configData.kpImpedance);
619  }
620 
621  void
623  ArmPtr& arm,
624  const DebugObserverInterfacePrx& debugObs)
625  {
626  StringVariantBaseMap datafields;
627  auto rtData = arm->bufferConfigRtToOnPublish.getUpToDateReadBuffer();
628  auto rtStatus = arm->bufferRtStatusToOnPublish.getUpToDateReadBuffer();
629 
630  common::debugEigenVec(datafields, "kpImpedance", rtData.kpImpedance);
631  common::debugEigenVec(datafields, "kdImpedance", rtData.kdImpedance);
632  if (rtData.desiredNullspaceJointAngles.has_value())
633  {
634  common::debugEigenVec(datafields,
635  "desiredNullspaceJointAngles",
636  rtData.desiredNullspaceJointAngles.value());
637  }
638 
639  common::debugEigenVec(datafields, "jointPos", rtStatus.jointPos);
640  common::debugEigenVec(datafields, "jointVel", rtStatus.jointVel);
641  common::debugEigenVec(datafields, "qvelFiltered", rtStatus.qvelFiltered);
642 
643  common::debugEigenPose(datafields, "current_pose", rtStatus.currentPose);
644  common::debugEigenPose(datafields, "desired_pose", rtStatus.desiredPose);
645  float positionError =
646  (common::getPos(rtStatus.currentPose) - common::getPos(rtStatus.desiredPose)).norm();
647  float angleError =
648  common::getDeltaAngleBetweenPose(rtStatus.currentPose, rtStatus.desiredPose);
649  datafields["poseError_position"] = new Variant(positionError);
650  datafields["poseError_angle"] = new Variant(angleError);
651  datafields["poseError_threshold_position"] = new Variant(rtData.safeDistanceMMToGoal);
652  datafields["poseError_threshold_angle"] = new Variant(rtData.safeRotAngleDegreeToGoal);
653 
654  common::debugEigenVec(datafields, "current_twist", rtStatus.currentTwist);
655  common::debugEigenVec(datafields, "poseErrorImp", rtStatus.poseErrorImp);
656 
657  common::debugEigenVec(datafields, "currentForceTorque", rtStatus.currentForceTorque);
658  common::debugEigenVec(datafields, "safeFTGuardOffset", rtStatus.safeFTGuardOffset);
659 
660  float currentForceNorm = rtStatus.currentForceTorque.head<3>().norm();
661  float currentTorqueNorm = rtStatus.currentForceTorque.tail<3>().norm();
662  float safeForceGuardOffsetNorm = rtStatus.safeFTGuardOffset.head<3>().norm();
663  float safeTorqueGuardOffsetNorm = rtStatus.safeFTGuardOffset.tail<3>().norm();
664  datafields["currentForceNorm"] = new Variant(currentForceNorm);
665  datafields["currentTorqueNorm"] = new Variant(currentTorqueNorm);
666  datafields["safeForceGuardOffsetNorm"] = new Variant(safeForceGuardOffsetNorm);
667  datafields["safeTorqueGuardOffsetNorm"] = new Variant(safeTorqueGuardOffsetNorm);
668  datafields["safeForceGuardDifference"] =
669  new Variant(currentForceNorm - safeForceGuardOffsetNorm);
670  datafields["safeTorqueGuardDifference"] =
671  new Variant(currentTorqueNorm - safeTorqueGuardOffsetNorm);
672  datafields["safeForceGuardThreshold"] =
673  new Variant(rtData.ftConfig.safeGuardForceThreshold);
674  datafields["safeTorqueGuardThreshold"] =
675  new Variant(rtData.ftConfig.safeGuardTorqueThreshold);
676 
677  datafields["rtSafe"] = new Variant(rtStatus.rtSafe * 1.0);
678  datafields["rtTargetSafe"] = new Variant(rtStatus.rtTargetSafe * 1.0);
679  bool forceTrigger = (not rtStatus.rtSafe) and rtStatus.rtTargetSafe;
680  datafields["forceTrigger"] = new Variant(forceTrigger * 1.0);
681  datafields["forceTorqueSafe"] = new Variant(rtStatus.forceTorqueSafe * 1.0);
682 
683  common::debugEigenVec(datafields, "forceImpedance", rtStatus.forceImpedance);
684  common::debugEigenVec(datafields, "cartesianVelTarget", rtStatus.cartesianVelTarget);
685 
686  common::debugEigenVec(datafields, "desiredJointTorques", rtStatus.desiredJointTorques);
687  common::debugEigenVec(datafields, "desiredJointVelocity", rtStatus.desiredJointVelocity);
688  common::debugEigenVec(datafields, "nullspaceTorque", rtStatus.nullspaceTorque);
689  common::debugEigenVec(datafields, "nullspaceVelocity", rtStatus.nullspaceVelocity);
690  debugObs->setDebugChannel(getClassName() + "_" + arm->kinematicChainName, datafields);
691 
692  viz::Layer layer = arviz.layer(getClassName() + "_" + arm->kinematicChainName);
693  layer.add(viz::Arrow("tsImpForce")
694  .fromTo(common::getPos(rtStatus.currentPose),
695  common::getPos(rtStatus.currentPose) +
696  rtStatus.forceImpedance.head<3>() * 5.0)
698  arviz.commit(layer);
699  }
700 
701  void
703  const SensorAndControl&,
705  const DebugObserverInterfacePrx& debugObs)
706  {
707  for (auto& pair : limb)
708  {
709  if (not pair.second->rtReady.load())
710  continue;
711  limbPublish(pair.second, debugObs);
712  }
713  }
714 
715  void
717  {
718  for (auto& pair : limb)
719  {
720  pair.second->rtReady.store(false);
721  }
722  }
723 
724  void
726  {
727  VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(arm->kinematicChainName);
728  Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
729  auto quat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
730  ARMARX_RT_LOGF_INFO("rt preactivate controller with target pose\n\n "
731  "[ %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f ].",
732  currentPose(0, 3),
733  currentPose(1, 3),
734  currentPose(2, 3),
735  quat.w,
736  quat.x,
737  quat.y,
738  quat.z);
739 
740  if (arm->reInitPreActivate.load())
741  {
742  arm->rtConfig.desiredNullspaceJointAngles.value() = rns->getJointValuesEigen();
743  arm->rtConfig.desiredPose = currentPose;
744 
745  arm->bufferConfigUserToNonRt.reinitAllBuffers(arm->rtConfig);
746  arm->reInitPreActivate.store(false);
747  }
748  {
749  arm->nonRtConfig = arm->rtConfig;
750  arm->bufferConfigNonRtToRt.reinitAllBuffers(arm->rtConfig);
751  arm->bufferConfigRtToOnPublish.reinitAllBuffers(arm->rtConfig);
752  arm->bufferConfigRtToUser.reinitAllBuffers(arm->rtConfig);
753  }
754  {
755  arm->sensorDevices.updateJointValues(
756  arm->rtStatus.jointPos, arm->rtStatus.jointVel, arm->rtStatus.jointTor);
757  arm->rtStatus.rtPreActivate(currentPose);
758 
759  arm->rtStatusInNonRT = arm->rtStatus;
760  arm->nonRTDeltaT = 0.0;
761  arm->nonRTAccumulateTime = 0.0;
762  arm->bufferRtStatusToOnPublish.reinitAllBuffers(arm->rtStatus);
763  arm->bufferRtStatusToNonRt.reinitAllBuffers(arm->rtStatus);
764  arm->bufferRtStatusToUser.reinitAllBuffers(arm->rtStatus);
765  }
766 
767  arm->controller.preactivateInit(rns);
768  }
769 
770  void
772  {
773  for (auto& pair : limb)
774  {
775  ARMARX_RT_LOGF_INFO("rtPreActivateController for %s", pair.first.c_str());
776  limbReInit(pair.second);
777  userConfig.limbs.at(pair.first) = pair.second->rtConfig;
778  }
779  if (hands)
780  {
781  hands->rtPreActivate();
782  }
783  }
784 
785  void
787  {
788  // for (auto& pair : limb)
789  // {
790  // pair.second->controller.isInitialized.store(false);
791  // }
792  }
793 
794  /// ----------------------------------- GUI Widget ---------------------------------------------
797  const VirtualRobot::RobotPtr& robot,
798  const std::map<std::string, ConstControlDevicePtr>& controlDevices,
799  const std::map<std::string, ConstSensorDevicePtr>&)
800  {
801  using namespace armarx::WidgetDescription;
802  HBoxLayoutPtr layout = new HBoxLayout;
803 
804 
805  ::armarx::WidgetDescription::WidgetSeq widgets;
806 
807  /// select default config
808  LabelPtr label = new Label;
809  label->text = "select a controller config";
810 
811  StringComboBoxPtr cfgBox = new StringComboBox;
812  cfgBox->name = "config_box";
813  cfgBox->defaultIndex = 0;
814  cfgBox->multiSelect = false;
815 
816  cfgBox->options = std::vector<std::string>{"default", "default_right", "default_right_a7"};
817  ARMARX_TRACE;
818 
819  layout->children.emplace_back(label);
820  layout->children.emplace_back(cfgBox);
821  ARMARX_TRACE;
822 
823  layout->children.insert(layout->children.end(), widgets.begin(), widgets.end());
824  ARMARX_INFO_S << "Layout done";
825  return layout;
826  }
827 
831  {
832  auto cfgName = values.at("config_box")->getString();
833  const armarx::PackagePath configPath(
834  "armarx_control",
835  "controller_config/NJointTaskspaceMixedImpedanceVelocityController/" + cfgName +
836  ".json");
837  ARMARX_INFO_S << "Loading config from " << configPath.toSystemPath();
838  ARMARX_CHECK(std::filesystem::exists(configPath.toSystemPath()));
839 
840  auto cfgDTO = armarx::readFromJson<ConfigDict>(configPath.toSystemPath());
841 
842  ARMARX_TRACE;
843  return new ConfigurableNJointControllerConfig{cfgDTO.toAronDTO()};
844  }
845 
846 } // namespace armarx::control::njoint_controller::task_space
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::userConfig
ConfigDict userConfig
Definition: MixedImpedanceVelocityController.h:182
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: MixedImpedanceVelocityController.cpp:771
armarx::control::njoint_controller::task_space::registrationControllerNJointTaskspaceMixedImpedanceVelocityController
NJointControllerRegistration< NJointTaskspaceMixedImpedanceVelocityController > registrationControllerNJointTaskspaceMixedImpedanceVelocityController("NJointTaskspaceMixedImpedanceVelocityController")
armarx::ControlTargetBase::asA
const T * asA() const
Definition: ControlTargetBase.h:76
ArVizComponentPlugin.h
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:223
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition: Client.cpp:89
ARMARX_RT_LOGF_INFO
#define ARMARX_RT_LOGF_INFO(...)
Definition: ControlThreadOutputBuffer.h:344
SingleTypeVariantList.h
armarx::control::common::debugEigenPose
void debugEigenPose(StringVariantBaseMap &datafields, const std::string &name, Eigen::Matrix4f pose)
Definition: utils.cpp:113
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::getClassName
std::string getClassName(const Ice::Current &=Ice::emptyCurrent) const override
Definition: MixedImpedanceVelocityController.cpp:173
armarx::control::ethercat::RTUtility::RT_THREAD_PRIORITY
static constexpr int RT_THREAD_PRIORITY
The thread priority for rt-threads.
Definition: RTUtility.h:24
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: MixedImpedanceVelocityController.cpp:385
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::onPublish
void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition: MixedImpedanceVelocityController.cpp:702
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::limb
std::map< std::string, ArmPtr > limb
Definition: MixedImpedanceVelocityController.h:179
armarx::SensorValueBase
The SensorValueBase class.
Definition: SensorValueBase.h:40
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition: ControlTargetBase.h:47
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::ConfigPtrT
ConfigurableNJointControllerConfigPtr ConfigPtrT
Definition: MixedImpedanceVelocityController.h:56
RobotUnit.h
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::handleRTNotSafeInNonRT
void handleRTNotSafeInNonRT()
Definition: MixedImpedanceVelocityController.cpp:260
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::controllableNodeSets
std::map< std::string, VirtualRobot::RobotNodeSetPtr > controllableNodeSets
Definition: MixedImpedanceVelocityController.h:184
armarx::NJointControllerBase::useControlTarget
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
Definition: NJointController.cpp:410
armarx::control::common::detectAndReportPoseDeviationWarning
bool detectAndReportPoseDeviationWarning(const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2, const std::string &namePose1, const std::string &namePose2, float positionThrMM, float angleThrDeg, const std::string &who)
Definition: utils.cpp:646
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::updateTargetPose
bool updateTargetPose(const TargetPoseMap &targetPoseMap, const TargetNullspaceMap &targetNullspaceMap, const Ice::Current &=Ice::emptyCurrent) override
Definition: MixedImpedanceVelocityController.cpp:541
armarx::control::njoint_controller::task_space
Definition: AdmittanceController.cpp:43
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::additionalTask
virtual void additionalTask()
Definition: MixedImpedanceVelocityController.cpp:212
armarx::control::ethercat::RTUtility::pinThreadToCPU
static bool pinThreadToCPU(unsigned int cpu)
Pins the calling thread to the CPU with the given id.
Definition: RTUtility.cpp:52
ARMARX_CHECK_GREATER
#define ARMARX_CHECK_GREATER(lhs, rhs)
This macro evaluates whether lhs is greater (>) than rhs and if it turns out to be false it will thro...
Definition: ExpressionException.h:116
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition: CycleUtil.h:40
armarx::ManagedIceObject::getState
int getState() const
Retrieve current state of the ManagedIceObject.
Definition: ManagedIceObject.cpp:769
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::viz::Arrow
Definition: Elements.h:196
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::getCurrentTCPPose
Ice::DoubleSeq getCurrentTCPPose(const std::string &nodeSetName, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: MixedImpedanceVelocityController.cpp:522
armarx::NJointControllerBase::ConfigPtrT
NJointControllerConfigPtr ConfigPtrT
Definition: NJointControllerBase.h:587
aron_conversions.h
MixedImpedanceVelocityController.h
armarx::viz::Layer::add
void add(ElementT const &element)
Definition: Layer.h:31
armarx::control::common::reportPoseDeviationWarning
void reportPoseDeviationWarning(const std::string &who, const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2, const std::string &namePose1, const std::string &namePose2)
Definition: utils.cpp:623
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::limbInit
void limbInit(const std::string nodeSetName, ArmPtr &arm, Config &cfg, VirtualRobot::RobotPtr &nonRtRobotPtr)
Definition: MixedImpedanceVelocityController.cpp:52
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::validateConfigData
void validateConfigData(Config &config, ArmPtr &arm)
Definition: MixedImpedanceVelocityController.cpp:573
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::getConfig
::armarx::aron::data::dto::DictPtr getConfig(const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: MixedImpedanceVelocityController.cpp:435
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:846
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
visionx::voxelgrid::Label
uint32_t Label
Type of an object label.
Definition: types.h:6
armarx::control::TargetNullspaceMap
dictionary< string, Ice::FloatSeq > TargetNullspaceMap
Definition: ControllerInterface.ice:40
armarx::WidgetDescription
Definition: DefaultWidgetDescriptions.cpp:27
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:77
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::limbNonRT
void limbNonRT(ArmPtr &arm)
Definition: MixedImpedanceVelocityController.cpp:205
armarx::PackagePath::toSystemPath
static std::filesystem::path toSystemPath(const data::PackagePath &pp)
Definition: PackagePath.cpp:47
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition: NJointControllerBase.h:778
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::enableSafeGuardForceTorque
void enableSafeGuardForceTorque(const std::string &nodeSetName, const bool forceGuard, const bool torqueGuard, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: MixedImpedanceVelocityController.cpp:465
armarx::NJointControllerBase::runTask
void runTask(const std::string &taskName, Task &&task)
Executes a given task in a separate thread from the Application ThreadPool.
Definition: NJointControllerBase.h:754
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
armarx::control::common::getPos
Eigen::Block< Eigen::Matrix4f, 3, 1 > getPos(Eigen::Matrix4f &matrix)
Definition: utils.cpp:212
armarx::control::NJointTaskspaceMixedImpedanceVelocityControllerInterface::getRTStatus
armarx::aron::data::dto::Dict getRTStatus()
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::additionalTaskUpdateStatus
std::tuple< bool, bool > additionalTaskUpdateStatus()
Definition: MixedImpedanceVelocityController.cpp:224
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::NJointTaskspaceMixedImpedanceVelocityController
NJointTaskspaceMixedImpedanceVelocityController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: MixedImpedanceVelocityController.cpp:131
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::limbRTUpdateStatus
void limbRTUpdateStatus(ArmPtr &arm, const double deltaT)
-----------------------------— Real time cotnrol --------------------------------------—
Definition: MixedImpedanceVelocityController.cpp:282
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::isSafeForceTorque
bool isSafeForceTorque(const std::string &nodeSetName, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
-----------------------------— Other interaces ----------------------------------------—
Definition: MixedImpedanceVelocityController.cpp:492
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::limbReInit
void limbReInit(ArmPtr &arm)
Definition: MixedImpedanceVelocityController.cpp:725
armarx::control::common::getDeltaAngleBetweenPose
float getDeltaAngleBetweenPose(const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2)
Definition: utils.cpp:605
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::getTCPVel
Ice::FloatSeq getTCPVel(const std::string &rns, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: MixedImpedanceVelocityController.cpp:508
ArmarXObjectScheduler.h
ControlTarget1DoFActuator.h
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::control::ethercat::RTUtility::elevateThreadPriority
static bool elevateThreadPriority(int priority)
Elevate the thread priority of the calling thread to the given priority.
Definition: RTUtility.cpp:17
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::nonRtRobot
VirtualRobot::RobotPtr nonRtRobot
Definition: MixedImpedanceVelocityController.h:180
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::limbRT
void limbRT(ArmPtr &arm, const double deltaT)
Definition: MixedImpedanceVelocityController.cpp:351
armarx::control::common::debugEigenVec
void debugEigenVec(StringVariantBaseMap &datafields, const std::string &name, Eigen::VectorXf vec)
Definition: utils.cpp:125
utils.h
CycleUtil.h
armarx::RemoteGui::Client::HBoxLayout
Definition: Widgets.h:160
armarx::control::NJointTaskspaceMixedImpedanceVelocityControllerInterface::calibrateFTSensor
void calibrateFTSensor()
ft sensor
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::onInitNJointController
void onInitNJointController() override
NJointControllerBase interface.
Definition: MixedImpedanceVelocityController.cpp:179
NJointController.h
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::rtPostDeactivateController
void rtPostDeactivateController() override
This function is called after the controller is deactivated.
Definition: MixedImpedanceVelocityController.cpp:786
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::limbRTSetTarget
void limbRTSetTarget(ArmPtr &arm, const size_t nDoFTorque, const size_t nDoFVelocity, const Eigen::VectorXf &targetTorque, const Eigen::VectorXf &targetVelocity)
Definition: MixedImpedanceVelocityController.cpp:308
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::updateConfig
void updateConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
NJointController interface.
Definition: MixedImpedanceVelocityController.cpp:403
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
TimeUtil.h
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::GenerateConfigDescription
static WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr &, const std::map< std::string, ConstControlDevicePtr > &, const std::map< std::string, ConstSensorDevicePtr > &)
--------------------------------— GUI Widget ------------------------------------------—
Definition: MixedImpedanceVelocityController.cpp:796
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::ArVizComponentPluginUser::arviz
armarx::viz::Client arviz
Definition: ArVizComponentPlugin.h:42
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
magic_enum::detail::find
constexpr std::size_t find(string_view str, char_type c) noexcept
Definition: magic_enum.hpp:309
IceUtil::Handle< class RobotUnit >
armarx::viz::ElementOps::color
DerivedT & color(Color color)
Definition: ElementOps.h:218
IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface >
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::hands
core::HandControlPtr hands
Definition: MixedImpedanceVelocityController.h:183
armarx::control::TargetPoseMap
dictionary< string, FloatSeqSeq > TargetPoseMap
Definition: ControllerInterface.ice:39
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::GenerateConfigFromVariants
static ConfigPtrT GenerateConfigFromVariants(const StringVariantBaseMap &values)
Definition: MixedImpedanceVelocityController.cpp:829
armarx::control::common::mat4ToDVec
mplib::core::DVec mat4ToDVec(const Eigen::Matrix4f &mat)
convert Eigen:Matrix4f to 7D double vector.
Definition: utils.cpp:252
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::Config
law::TaskspaceMixedImpedanceVelocityController::Config Config
Definition: MixedImpedanceVelocityController.h:57
ARMARX_INFO_S
#define ARMARX_INFO_S
Definition: Logging.h:202
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::limbPublish
void limbPublish(ArmPtr &arm, const DebugObserverInterfacePrx &debugObs)
Definition: MixedImpedanceVelocityController.cpp:622
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::ArmPtr
std::unique_ptr< ArmData > ArmPtr
Definition: MixedImpedanceVelocityController.h:106
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::additionalTaskSetTarget
void additionalTaskSetTarget()
Definition: MixedImpedanceVelocityController.cpp:247
armarx::control::common::sVecToString
std::string sVecToString(const std::vector< std::string > &vec, const std::string &delimiter)
Definition: utils.cpp:318
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_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::viz::Client::layer
Layer layer(std::string const &name) const
Definition: Client.cpp:80
ARMARX_RT_LOGF_WARN
#define ARMARX_RT_LOGF_WARN(...)
Definition: ControlThreadOutputBuffer.h:350
armarx::PackagePath
Definition: PackagePath.h:52
armarx::viz::Layer
Definition: Layer.h:12
armarx::control::njoint_controller::task_space::NJointTaskspaceMixedImpedanceVelocityController::robotUnit
RobotUnitPtr robotUnit
Definition: MixedImpedanceVelocityController.h:181
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
RTUtility.h
NJointControllerRegistry.h
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:776
armarx::NJointControllerBase::useSensorValue
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
Definition: NJointController.cpp:383
armarx::green
QColor green()
Definition: StyleSheets.h:72
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
PackagePath.h
norm
double norm(const Point &a)
Definition: point.hpp:102