SafetyImpedanceController.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 2021
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "SafetyImpedanceController.h"
24 
25 #include <VirtualRobot/MathTools.h>
26 
27 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
28 #include <ArmarXCore/core/time/CycleUtil.h>
29 #include <ArmarXCore/core/time/TimeUtil.h>
30 
31 #include <armarx/control/common/utils.h>
32 #include <armarx/control/ethercat/RTUtility.h>
33 
34 #include <simox/control/geodesics/util.h>
35 #include <simox/control/robot/NodeInterface.h>
36 
37 namespace armarx::control::njoint_controller::task_space
38 {
39  NJointControllerRegistration<NJointTaskspaceSafetyImpedanceController>
40  registrationControllerNJointTaskspaceSafetyImpedanceController(
41  "NJointTaskspaceSafetyImpedanceController");
42 
43  void
44  NJointTaskspaceSafetyImpedanceController::limbInit(
45  const std::string nodeSetName,
46  ArmPtr& arm,
47  Config& cfg,
48  VirtualRobot::RobotPtr& nonRtRobotPtr,
49  std::shared_ptr<std::vector<::simox::control::environment::DistanceResult>> collisionPairs)
50  {
51  arm->kinematicChainName = nodeSetName;
52  VirtualRobot::RobotNodeSetPtr rtRns =
53  rtGetRobot()->getRobotNodeSet(arm->kinematicChainName);
54  arm->tcpName = rtRns->getTCP()->getName();
55  VirtualRobot::RobotNodeSetPtr nonRtRns =
56  nonRtRobotPtr->getRobotNodeSet(arm->kinematicChainName);
57  ARMARX_CHECK_EXPRESSION(rtRns) << arm->kinematicChainName;
58  ARMARX_CHECK_EXPRESSION(nonRtRns) << arm->kinematicChainName;
59  ARMARX_IMPORTANT << "Creating Taskspace Impedance controller with kinematic chain: "
60  << arm->kinematicChainName << " with num of joints: (RT robot) "
61  << rtRns->getSize() << ", and (non-RT robot) " << nonRtRns->getSize();
62 
63  arm->controller.initialize(nonRtRns, rtRns, simoxReducedRobotPtr);
64  arm->controller.ftsensor.initialize(rtRns, robotUnit, cfg.ftConfig);
65  arm->jointNames.clear();
66  for (size_t i = 0; i < rtRns->getSize(); ++i)
67  {
68  std::string jointName = rtRns->getNode(i)->getName();
69  arm->jointNames.push_back(jointName);
70 
71  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
72  ARMARX_CHECK_EXPRESSION(ct);
73  const SensorValueBase* sv = useSensorValue(jointName);
74  ARMARX_CHECK_EXPRESSION(sv);
75  auto* casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>();
76  ARMARX_CHECK_EXPRESSION(casted_ct);
77  arm->targets.push_back(casted_ct);
78 
79  const auto* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
80  const auto* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
81  const auto* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
82 
83  if (torqueSensor == nullptr)
84  {
85  ARMARX_WARNING << "No Torque sensor available for " << jointName;
86  }
87  if (velocitySensor == nullptr)
88  {
89  ARMARX_WARNING << "No velocity sensor available for " << jointName;
90  }
91  if (positionSensor == nullptr)
92  {
93  ARMARX_WARNING << "No position sensor available for " << jointName;
94  }
95 
96  arm->sensorDevices.torqueSensors.push_back(torqueSensor);
97  arm->sensorDevices.velocitySensors.push_back(velocitySensor);
98  arm->sensorDevices.positionSensors.push_back(positionSensor);
99  };
100 
101  /// init simox inertia class
102  arm->inertiaPtr = std::make_shared<simox::control::geodesics::metric::Inertia>(
103  *simoxControlRobotPtr, nodeSetName);
104 
105  /// set pointer to self-collision avoidance objects
106 
107  arm->collisionPairsPtr = collisionPairs;
108 
109  arm->pointsOnArm = std::make_shared<std::vector<int>>(maxCollisionPairs);
110  arm->pointsOnArmIndex = 0;
111  arm->simoxReducedRobotPtr = simoxReducedRobotPtr;
112 
113  validateConfigData(cfg, arm);
114  arm->rtConfig = cfg;
115  arm->nonRtConfig = cfg;
116  }
117 
118  NJointTaskspaceSafetyImpedanceController::NJointTaskspaceSafetyImpedanceController(
119  const RobotUnitPtr& robotUnit,
120  const NJointControllerConfigPtr& config,
121  const VirtualRobot::RobotPtr&) :
122  robotUnit(robotUnit)
123  {
124  ARMARX_CHECK_EXPRESSION(robotUnit);
125 
126  ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
127  ARMARX_CHECK_EXPRESSION(cfg);
128  userConfig.fromAron(cfg->config);
129 
130  useSynchronizedRtRobot();
131  nonRtRobot = robotUnit->cloneRobot();
132  robotUnit->updateVirtualRobot(nonRtRobot);
133 
134  /// check config for arms
135  preFilterDistance = 0.6f; // in meter
136  std::vector<std::string> poseNodeNames;
137  for (auto& pair : userConfig.limbs)
138  {
139  const auto nodeset = rtGetRobot()->getRobotNodeSet(pair.first);
140  std::string tcpName = nodeset->getTCP()->getName();
141  poseNodeNames.push_back(tcpName);
142  maxCollisionPairs = pair.second.maxCollisionPairs;
143  if (pair.second.distanceThreshold > preFilterDistance)
144  {
145  preFilterDistance = pair.second.distanceThreshold;
146  }
147  }
148  std::string actuatedNodeSet;
149  for (auto& pair : userConfig.limbs)
150  {
151  actuatedNodeSet = pair.second.actuatedJointsNodeSet;
152  }
153 
154  /// add actuated robot joints
155  /// use a robot nodeset with all actuated joints, arms including head and torso joints, which change
156  /// the position of the robot body parts for correct collision checking
157  // auto actRns = rtGetRobot()->getRobotNodeSet(userConfig.limbs.begin()->second.actuatedJointsNodeSet)->getNodeNames();
158  auto actRns = rtGetRobot()->getRobotNodeSet(actuatedNodeSet)->getNodeNames();
159  actuatedJoints.insert(actuatedJoints.end(), actRns.begin(), actRns.end());
160 
161  /// initialize simox control robot for self-collision checking
162  const bool reduceModel = true; // for performance and ignoreCollision detection
163  simoxReducedRobotPtr = std::make_shared<::simox::control::simox::robot::Robot>(
164  rtGetRobot(), actuatedJoints, poseNodeNames, reduceModel);
165 
166  std::string primitiveModelID = "ApproxHand";
167  std::vector<std::string> primitiveModelIDs{primitiveModelID};
168  // loadPrimitiveModel = true and primitive modelIDs = "ApproxHand".
169  collisionRobotPtr =
170  std::make_unique<simox::control::environment::CollisionRobot<hpp::fcl::OBBRSS>>(
171  *simoxReducedRobotPtr, std::string(), true, primitiveModelIDs);
172  collisionPairs = std::make_shared<std::vector<simox::control::environment::DistanceResult>>(
173  maxCollisionPairs);
174 
175  /// create not reduced simox control robot for inertia calculation
176  simoxControlRobotPtr = std::make_shared<::simox::control::simox::robot::Robot>(
177  rtGetRobot(), actuatedJoints, poseNodeNames, false);
178 
179  // initialize vector to store the current joint values of all actuated joints to update
180  // the simox robots with the most recent values, so the simox control robots display the
181  // current joint configuration of the real world model
182  qposActuatedJoints.resize(actuatedJoints.size());
183  qposActuatedJoints.setZero();
184 
185  for (auto& pair : userConfig.limbs)
186  {
187  limb.emplace(pair.first, std::make_unique<ArmData>());
188  limbInit(pair.first, limb.at(pair.first), pair.second, nonRtRobot, collisionPairs);
189  }
190  }
191 
192  std::string
193  NJointTaskspaceSafetyImpedanceController::getClassName(const Ice::Current&) const
194  {
195  return "NJointTaskspaceSafetyImpedanceController";
196  }
197 
198  void
199  NJointTaskspaceSafetyImpedanceController::onInitNJointController()
200  {
201  std::string taskName = getClassName() + "AdditionalTask";
202  runTask(taskName,
203  [&]
204  {
205  armarx::control::ethercat::RTUtility::pinThreadToCPU(
206  4); // please don't set this to 0 as it is the rtRun/control thread
207  armarx::control::ethercat::RTUtility::elevateThreadPriority(
208  armarx::control::ethercat::RTUtility::RT_THREAD_PRIORITY);
209 
210  CycleUtil c(1);
211  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
212  ARMARX_IMPORTANT << "Create a new thread for " << getClassName();
213  while (getState() == eManagedIceObjectStarted)
214  {
215  if (isControllerActive())
216  {
217  additionalTask();
218  }
219  c.waitForCycleDuration();
220  }
221  });
222  }
223 
224  void
225  NJointTaskspaceSafetyImpedanceController::limbNonRT(ArmPtr& arm)
226  {
227  arm->nonRtConfig = arm->bufferConfigUserToNonRt.getUpToDateReadBuffer();
228  arm->bufferConfigNonRtToRt.getWriteBuffer() = arm->nonRtConfig;
229  arm->bufferConfigNonRtToRt.commitWrite();
230  }
231 
232  void
233  NJointTaskspaceSafetyImpedanceController::additionalTask()
234  {
235  robotUnit->updateVirtualRobot(nonRtRobot);
236  bool rtSafe = true;
237  for (auto& pair : limb)
238  {
239  limbNonRT(pair.second);
240  pair.second->rtStatusInNonRT =
241  pair.second->bufferRtStatusToNonRt.getUpToDateReadBuffer();
242  rtSafe = rtSafe and pair.second->rtStatusInNonRT.rtSafe;
243  }
244  if (not rtSafe)
245  {
246  for (auto& pair : limb)
247  {
248  if (not pair.second->rtReady)
249  continue;
250  const auto& desiredPose =
251  pair.second->rtStatusInNonRT.desiredPose.topRightCorner<3, 1>();
252  const auto& currentPose =
253  pair.second->rtStatusInNonRT.currentPose.topRightCorner<3, 1>();
254  if ((desiredPose - currentPose).norm() >
255  pair.second->nonRtConfig.safeDistanceMMToGoal)
256  {
257  ARMARX_RT_LOGF_WARN(
258  "new target \n\n [%.2f, %.2f, %.2f]\n\nis too far away from the "
259  "current pose\n\n [%.2f, %.2f, %.2f]",
260  pair.second->rtStatusInNonRT.desiredPose(0, 3),
261  pair.second->rtStatusInNonRT.desiredPose(1, 3),
262  pair.second->rtStatusInNonRT.desiredPose(2, 3),
263  pair.second->rtStatusInNonRT.currentPose(0, 3),
264  pair.second->rtStatusInNonRT.currentPose(1, 3),
265  pair.second->rtStatusInNonRT.currentPose(2, 3))
266  .deactivateSpam(1.0);
267  }
268  }
269  }
270  }
271 
272  void
273  NJointTaskspaceSafetyImpedanceController::limbRT(ArmPtr& arm, const double deltaT)
274  {
275  double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
276  arm->rtConfig = arm->bufferConfigNonRtToRt.getUpToDateReadBuffer();
277  double time_update_non_rt_buffer =
278  IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
279 
280  arm->rtStatus.deltaT = deltaT;
281  arm->controller.ftsensor.updateStatus(
282  arm->rtConfig.ftConfig, arm->rtStatus.currentForceTorque, arm->rtStatus.deltaT, arm->rtFirstRun.load());
283 
284  double time_update_ft = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
285 
286  size_t nDoF = arm->sensorDevices.positionSensors.size();
287  double time_update_size = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
288  for (size_t i = 0; i < nDoF; ++i)
289  {
290  arm->rtStatus.jointPosition[i] = arm->sensorDevices.positionSensors[i]->position;
291  arm->rtStatus.jointVelocity[i] = arm->sensorDevices.velocitySensors[i]->velocity;
292  arm->rtStatus.jointTorque[i] = arm->sensorDevices.torqueSensors[i]->torque;
293  }
294 
295  double time_update_js = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
296 
297  double time_update_rt_status = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
298 
299  arm->bufferRtStatusToNonRt.getWriteBuffer() = arm->rtStatus;
300  arm->bufferRtStatusToNonRt.commitWrite();
301  arm->bufferRtStatusToUser.getWriteBuffer() = arm->rtStatus;
302  arm->bufferRtStatusToUser.commitWrite();
303  double time_write_rt_buffer = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
304 
305  arm->controller.run(arm->rtConfig,
306  arm->rtStatus,
307  arm->collisionPairsPtr,
308  arm->pointsOnArm,
309  arm->pointsOnArmIndex,
310  arm->inertiaPtr,
311  arm->jointNames);
312  double time_run_rt = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
313 
314  for (unsigned int i = 0; i < arm->rtStatus.desiredJointTorques.size(); i++)
315  {
316  arm->targets.at(i)->torque = arm->rtStatus.desiredJointTorques(i);
317  // arm->targets.at(i)->torque = 0.;
318  if (!arm->targets.at(i)->isValid())
319  {
320  arm->targets.at(i)->torque = 0;
321  }
322  }
323  arm->bufferRtStatusToOnPublish.getWriteBuffer() = arm->rtStatus;
324  arm->bufferRtStatusToOnPublish.commitWrite();
325 
326  arm->bufferConfigRtToOnPublish.getWriteBuffer() = arm->rtConfig;
327  arm->bufferConfigRtToOnPublish.commitWrite();
328 
329  arm->bufferConfigRtToUser.getWriteBuffer() = arm->rtConfig;
330  arm->bufferConfigRtToUser.commitWrite();
331  double time_rt_status_buffer = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
332  timeMeasure = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
333 
334 
335  if (timeMeasure > 300)
336  {
337  ARMARX_RT_LOGF_WARN("---- rt too slow: "
338  "update_non_rt_buffer: %.2f\n"
339  "update_ft: %.2f\n"
340  "update_size: %.2f\n"
341  "update_js: %.2f\n"
342  "update_rt_status: %.2f\n"
343  "write_rt_buffer: %.2f\n"
344  "run_rt: %.2f\n"
345  "rt_status_buffer: %.2f\n"
346  "time all: %.2f\n",
347  time_update_non_rt_buffer, // 0-1 us
348  time_update_ft - time_update_non_rt_buffer, //
349  time_update_size - time_update_ft, //
350  time_update_js - time_update_size, //
351  time_update_rt_status - time_update_non_rt_buffer, // 1-2 us
352  time_write_rt_buffer - time_update_rt_status, // 0-1 us
353  time_run_rt - time_write_rt_buffer, // 26-33 us
354  time_rt_status_buffer - time_run_rt,
355  timeMeasure)
356  .deactivateSpam(1.0f); // 0-1 us
357  }
358  if (arm->rtFirstRun.load())
359  {
360  arm->rtFirstRun.store(false);
361  arm->rtReady.store(true);
362  }
363  }
364 
365  void
366  NJointTaskspaceSafetyImpedanceController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
367  const IceUtil::Time& timeSinceLastIteration)
368  {
369  /// calculation of robot collision pairs
370  collisionPairs->clear(); // empty collision pairs before assigning new ones
371  double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
372 
373  /// !!!individualColObjects has to be set to true (second flag, that is passed)!!!
374  /// if not set to true, the body and the arms become attractive, be aware
375  std::vector<simox::control::environment::DistanceResult> results =
376  collisionRobotPtr->computeMinimumSelfCollisionDistance(true, true);
377 
378  collisionPairs->insert(collisionPairs->end(), results.begin(), results.end());
379 
380  double collisionPairTime = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
381 
382  for (auto& pair : limb)
383  {
384  /// reset temporary pointsOnArmIndex
385  pair.second->pointsOnArmIndex = 0;
386  pair.second->rtStatus.collisionPairTime = collisionPairTime;
387  pair.second->rtStatus.collisionPairsNum = collisionPairs->size();
388  }
389 
390 
391  /// checks whether the point is located on the arm and fills the pointsOnArm vector with the
392  /// corresponding indices
393  /// the entries in the pointsOnArm vector store the indices of the collision pairs that are
394  /// on the arm and which must be taken into account in the control law
395  timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
396  for (size_t i = 0; i < collisionPairs->size(); ++i)
397  {
398  ARMARX_CHECK(collisionPairs->at(i).hasPoints());
399 
400  /// prefilter contact pairs based on distance
401  if (static_cast<float>(collisionPairs->at(i).minDistance) <= preFilterDistance)
402  {
403 
404  /// set index in integer vector of contact point on arm below preFilterDistance
405  for (auto& pair : limb)
406  {
407  if (std::find(pair.second->jointNames.begin(),
408  pair.second->jointNames.end(),
409  collisionPairs->at(i).node1) != pair.second->jointNames.end() or
410  pair.second->tcpName == collisionPairs->at(i).node1)
411  {
412  // note: if node1 and node2 are on arm, this is handeled in law
413  // here it is only important, whether the coll pair is regarded for the arm
414  pair.second->pointsOnArm->at(pair.second->pointsOnArmIndex) = i;
415  pair.second->pointsOnArmIndex++;
416  }
417  else if (std::find(pair.second->jointNames.begin(),
418  pair.second->jointNames.end(),
419  collisionPairs->at(i).node2) !=
420  pair.second->jointNames.end() or
421  pair.second->tcpName == collisionPairs->at(i).node2)
422  {
423  pair.second->pointsOnArm->at(pair.second->pointsOnArmIndex) = i;
424  pair.second->pointsOnArmIndex++;
425  }
426  }
427  }
428  }
429  double preFilterTime = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
430 
431  if (preFilterTime > 100 or collisionPairTime > 100)
432  {
433  ARMARX_RT_LOGF_WARN("---- rt too slow: "
434  "preFilterTime: %.2f\n"
435  "collisionPairTime: %.2f\n",
436  preFilterTime, // 3-4 us
437  collisionPairTime)
438  .deactivateSpam(1.0f); // 28-31 us
439  }
440 
441 
442  for (auto& pair : limb)
443  {
444  /// set all invalid indeces of this rt cycle to -1
445  for (size_t i = pair.second->pointsOnArmIndex; i < pair.second->pointsOnArm->size();
446  ++i)
447  {
448  pair.second->pointsOnArm->at(i) = -1;
449  }
450  pair.second->rtStatus.preFilterTime = preFilterTime;
451  pair.second->rtStatus.activeCollPairsNum = pair.second->pointsOnArmIndex;
452  }
453 
454 
455  double deltaT = timeSinceLastIteration.toSecondsDouble();
456  for (auto& pair : limb)
457  {
458  limbRT(pair.second, deltaT);
459  }
460 
461  /// update joint values for simox Robots
462 
463  for (int i = 0; i < qposActuatedJoints.size(); ++i)
464  {
465  auto const node = rtGetRobot()->getRobotNode(actuatedJoints[i]);
466  double jointValue = static_cast<double>(node->getJointValue());
467  qposActuatedJoints(i) = jointValue;
468  if (node->isTranslationalJoint())
469  {
470  // convert value from mm to m for simox control robot
471  // all translational values of the simox control robots are in meter
472  qposActuatedJoints(i) *= 0.001;
473  }
474  }
475 
476 
477  /// update Simox robots
478  ///
479  /// a VectorXd is needed with nodeSet size (size of actuated joints in order of vector,
480  /// which has been used for initialization in init
481  ///
482  /// It is very important that the simox robots map the current joint configuration of the
483  /// real-world robot
484  simoxReducedRobotPtr->setConfig(qposActuatedJoints);
485  simoxControlRobotPtr->setConfig(qposActuatedJoints);
486  }
487 
488  Ice::FloatSeq
489  NJointTaskspaceSafetyImpedanceController::getTCPVel(const std::string& rns,
490  const Ice::Current& iceCurrent)
491  {
492  std::vector<float> tcpVel;
493  auto& arm = limb.at(rns);
494  auto s = arm->bufferRtStatusToUser.getUpToDateReadBuffer();
495  for (int i = 0; i < s.currentTwist.size(); i++)
496  {
497  tcpVel.push_back(s.currentTwist[i]);
498  }
499  return tcpVel;
500  }
501 
502  void
503  NJointTaskspaceSafetyImpedanceController::updateConfig(
504  const ::armarx::aron::data::dto::DictPtr& dto,
505  const Ice::Current& iceCurrent)
506  {
507  // userConfig = ::armarx::fromAronDict<AronDTO, BO>(dto);
508  userConfig.fromAron(dto);
509  for (auto& pair : userConfig.limbs)
510  {
511  validateConfigData(pair.second, limb.at(pair.first));
512  limb.at(pair.first)->bufferConfigUserToNonRt.getWriteBuffer() = pair.second;
513  limb.at(pair.first)->bufferConfigUserToNonRt.commitWrite();
514  }
515  }
516 
517  void
518  NJointTaskspaceSafetyImpedanceController::enableSafeGuardForceTorque(
519  const std::string& nodeSetName,
520  const bool forceGuard,
521  const bool torqueGuard,
522  const Ice::Current& iceCurrent)
523  {
524  auto it = userConfig.limbs.find(nodeSetName);
525  if (it != userConfig.limbs.end())
526  {
527  it->second.ftConfig.enableSafeGuardForce = forceGuard;
528  it->second.ftConfig.enableSafeGuardTorque = torqueGuard;
529  limb.at(nodeSetName)->controller.ftsensor.enableSafeGuard(forceGuard, torqueGuard);
530  ARMARX_IMPORTANT
531  << "reset safe guard with force torque sensors: safe? "
532  << limb.at(nodeSetName)->controller.ftsensor.isSafe(it->second.ftConfig);
533  limb.at(nodeSetName)->bufferConfigUserToNonRt.getWriteBuffer() = it->second;
534  limb.at(nodeSetName)->bufferConfigUserToNonRt.commitWrite();
535  }
536  else
537  {
538  ARMARX_WARNING << "no robot node set with name " << nodeSetName
539  << " found in the controllers.";
540  }
541  }
542 
543  bool
544  NJointTaskspaceSafetyImpedanceController::isSafeForceTorque(const std::string& nodeSetName,
545  const Ice::Current& iceCurrent)
546  {
547  auto it = userConfig.limbs.find(nodeSetName);
548  if (it != userConfig.limbs.end())
549  {
550  return limb.at(nodeSetName)->controller.ftsensor.ftSafe.load();
551  }
552  else
553  {
554  ARMARX_WARNING << "no robot node set with name " << nodeSetName
555  << " found in the controllers.";
556  }
557  return false;
558  }
559 
560  bool
561  NJointTaskspaceSafetyImpedanceController::updateTargetPose(
562  const TargetPoseMap& targetPoseMap,
563  const TargetNullspaceMap& targetNullspaceMap,
564  const Ice::Current& iceCurrent)
565  {
566  for (auto& pair : userConfig.limbs)
567  {
568  for (size_t i = 0; i < 4; ++i)
569  {
570  for (int j = 0; j < 4; ++j)
571  {
572  pair.second.desiredPose(i, j) = targetPoseMap.at(pair.first)[i][j];
573  }
574  }
575  if (targetNullspaceMap.at(pair.first).size() > 0)
576  {
577  const size_t nDoF = pair.second.desiredNullspaceJointAngles.value().size();
578  ARMARX_CHECK_EQUAL(targetNullspaceMap.at(pair.first).size(), nDoF)
579  << "the joint numbers does not match";
580  for (size_t i = 0; i < nDoF; ++i)
581  {
582  pair.second.desiredNullspaceJointAngles.value()(i) =
583  targetNullspaceMap.at(pair.first)[i];
584  }
585  }
586  limb.at(pair.first)->bufferConfigUserToNonRt.getWriteBuffer() = pair.second;
587  limb.at(pair.first)->bufferConfigUserToNonRt.commitWrite();
588  }
589  return true;
590  }
591 
592  ::armarx::aron::data::dto::DictPtr
593  NJointTaskspaceSafetyImpedanceController::getConfig(const Ice::Current& iceCurrent)
594  {
595  for (auto& pair : limb)
596  {
597  userConfig.limbs.at(pair.first) =
598  pair.second->bufferConfigRtToUser.getUpToDateReadBuffer();
599  }
600  return userConfig.toAronDTO();
601  }
602 
603  void
604  NJointTaskspaceSafetyImpedanceController::validateConfigData(Config& configData, ArmPtr& arm)
605  {
606  const auto nDoF = arm->controller.numOfJoints;
607 
608  const auto checkSize = [nDoF](const auto& v) { ARMARX_CHECK_EQUAL(v.rows(), nDoF); };
609  const auto checkNonNegative = [](const auto& v) { ARMARX_CHECK((v.array() >= 0).all()); };
610 
611  if (!configData.desiredNullspaceJointAngles.has_value())
612  {
613  if (!isControllerActive())
614  {
615  ARMARX_INFO << "No user defined nullspace target, reset to zero with "
616  << VAROUT(nDoF);
617  configData.desiredNullspaceJointAngles = Eigen::VectorXf::Zero(nDoF);
618  arm->reInitPreActivate.store(true);
619  }
620  else
621  {
622  auto currentValue =
623  arm->bufferConfigRtToUser.getUpToDateReadBuffer().desiredNullspaceJointAngles;
624  ARMARX_WARNING
625  << "Controller is active, but no user defined nullspace target. \n"
626  "You should first get up-to-date config of this controller and then update "
627  "it:\n"
628  " auto cfg = ctrl->getConfig(); \n"
629  " cfg.desiredPose = xxx;\n"
630  " ctrl->updateConfig(cfg);\n"
631  "Now, I decide by myself to use the existing values of nullspace target\n"
632  << currentValue.value();
633  configData.desiredNullspaceJointAngles = currentValue;
634  }
635  }
636  checkSize(configData.desiredNullspaceJointAngles.value());
637  checkSize(configData.kdNullspace);
638  checkSize(configData.kpNullspace);
639 
640  checkNonNegative(configData.kdNullspace);
641  checkNonNegative(configData.kpNullspace);
642  checkNonNegative(configData.kdImpedance);
643  checkNonNegative(configData.kpImpedance);
644  /// self-collision config parameters
645  ARMARX_CHECK_NONNEGATIVE(configData.dampingFactor);
646  ARMARX_CHECK_GREATER(configData.dampingFactor, 0.0); // damping can't be exactly zero
647  ARMARX_CHECK_LESS_EQUAL(configData.dampingFactor, 1.0); // for scaling of the damping
648  ARMARX_CHECK_GREATER(configData.maxRepulsiveForce, 0.0);
649  ARMARX_CHECK_NONNEGATIVE(configData.maxRepulsiveForce);
650  ARMARX_CHECK_NONNEGATIVE(configData.distanceThreshold);
651  ARMARX_CHECK_NONNEGATIVE(configData.selfCollActivatorZ1);
652  ARMARX_CHECK_NONNEGATIVE(configData.selfCollActivatorZ2);
653  ARMARX_CHECK_GREATER(configData.selfCollActivatorZ2, configData.selfCollActivatorZ1);
654  ARMARX_CHECK_NONNEGATIVE(configData.maxCollisionPairs);
655  /// joint limit avoidance config parameters
656  ARMARX_CHECK_GREATER(configData.maxRepulsiveTorque, 0.0);
657  ARMARX_CHECK_NONNEGATIVE(configData.maxRepulsiveTorque);
658  ARMARX_CHECK_NONNEGATIVE(configData.jointRangeBufferZone);
659  ARMARX_CHECK_LESS_EQUAL(configData.jointRangeBufferZone, 1.0); // percentage value
660  ARMARX_CHECK_NONNEGATIVE(configData.jointRangeBufferZoneZ1);
661  ARMARX_CHECK_LESS_EQUAL(configData.jointRangeBufferZoneZ1, 1.0); // percentage value
662  ARMARX_CHECK_NONNEGATIVE(configData.jointRangeBufferZoneZ2);
663  ARMARX_CHECK_GREATER(configData.jointRangeBufferZoneZ2, configData.jointRangeBufferZoneZ1);
664  ARMARX_CHECK_LESS_EQUAL(configData.jointRangeBufferZoneZ2, 1.0); // percentage value
665  ARMARX_CHECK_NONNEGATIVE(configData.safetyValFilter);
666  ARMARX_CHECK_LESS_EQUAL(configData.safetyValFilter, 1.0);
667  /// check hierarchy flags for valid config
668  if (configData.topPrioritySelfCollisionAvoidance)
669  {
670  ARMARX_CHECK_EXPRESSION(configData.enableSelfCollisionAvoidance);
671  ARMARX_CHECK_EXPRESSION((configData.topPrioritySelfCollisionAvoidance ==
672  !configData.topPriorityJointLimitAvoidance));
673  }
674  if (configData.topPriorityJointLimitAvoidance)
675  {
676  ARMARX_CHECK_EXPRESSION(configData.enableJointLimitAvoidance);
677  ARMARX_CHECK_EXPRESSION((configData.topPrioritySelfCollisionAvoidance ==
678  !configData.topPriorityJointLimitAvoidance));
679  }
680  }
681 
682  void
683  NJointTaskspaceSafetyImpedanceController::limbPublish(ArmPtr& arm,
684  const DebugObserverInterfacePrx& debugObs)
685  {
686  // double limbTimeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
687 
688  StringVariantBaseMap datafields;
689  auto rtData = arm->bufferConfigRtToOnPublish.getUpToDateReadBuffer();
690  auto rtStatus = arm->bufferRtStatusToOnPublish.getUpToDateReadBuffer();
691  datafields["deltaT"] = new Variant(rtStatus.deltaT);
692 
693  // // common::debugEigenPose(datafields, "current_pose", rtStatus.currentPose);
694  // // common::debugEigenPose(datafields, "desired_pose", rtStatus.desiredPose);
695  // // common::debugEigenVec(datafields, "kpImpedance", rtData.kpImpedance);
696  // // common::debugEigenVec(datafields, "kdImpedance", rtData.kdImpedance);
697  // // datafields["dampingFactor"] = new Variant(rtData.dampingFactor);
698  // // datafields["maxRepForce"] = new Variant(rtData.maxRepulsiveForce);
699  // // datafields["maxRepTorque"] = new Variant(rtData.maxRepulsiveTorque);
700  // datafields["trackingError"] = new Variant(rtStatus.trackingError);
701 
702  // // common::debugEigenVec(datafields, "currentForceTorque", rtStatus.currentForceTorque);
703 
704  // // common::debugEigenVec(datafields, "forceImpedance", rtStatus.forceImpedance);
705  // // common::debugEigenVec(datafields, "nullspaceTorque", rtStatus.nullspaceTorque);
706  // common::debugEigenVec(datafields, "desiredJointTorques", rtStatus.desiredJointTorques);
707  // // common::debugEigenVec(datafields, "jointDampingTorque", rtStatus.jointDampingTorque);
708 
709  // common::debugEigenVec(datafields, "impedanceTorque", rtStatus.impedanceJointTorque);
710  // common::debugEigenVec(datafields, "selfCollisionTorque", rtStatus.selfCollisionJointTorque);
711  // common::debugEigenVec(datafields, "jointLimitTorque", rtStatus.jointLimitJointTorque);
712 
713  // common::debugEigenVec(
714  // datafields, "projectedImpedanceTorque", rtStatus.projImpedanceJointTorque);
715  // // common::debugEigenVec(datafields, "projectedJointLimitTorque", rtStatus.projJointLimTorque);
716  // // common::debugEigenVec(datafields, "projectedForceImpedance", rtStatus.projForceImpedance);
717  // // common::debugEigenVec(
718  // // datafields, "projectedSelfCollisionTorque", rtStatus.projSelfCollTorque);
719 
720  // Eigen::VectorXf selfCollNullspace = rtStatus.selfCollNullSpace.diagonal();
721  // Eigen::VectorXf jointLimNullspace = rtStatus.jointLimNullSpace.diagonal();
722  // common::debugEigenVec(datafields, "selfCollNullspaceDiagonal", selfCollNullspace);
723  // common::debugEigenVec(datafields, "jointLimNullspaceDiagonal", jointLimNullspace);
724 
725  // common::debugEigenVec(datafields, "qpos", rtStatus.qpos);
726  // common::debugEigenVec(datafields, "qvelFiltered", rtStatus.qvelFiltered);
727  // // common::debugStdVec(datafields, "qvelocity", rtStatus.jointVelocity);
728 
729 
730  // if (rtData.enableJointLimitAvoidance)
731  // {
732  // for (size_t i = 0; i < rtStatus.jointLimitData.size(); ++i)
733  // {
734  // if (not rtStatus.jointLimitData[i].isLimitless)
735  // {
736  // datafields["n_des(q)_" + std::to_string(i) + "_" +
737  // rtStatus.jointLimitData[i].jointName] =
738  // new Variant(rtStatus.jointLimitData[i].desiredNSjointLim);
739  // datafields["q_damping" + std::to_string(i) + "_" +
740  // rtStatus.jointLimitData[i].jointName] =
741  // new Variant(rtStatus.jointLimitData[i].totalDamping);
742  // datafields["q_repTorque" + std::to_string(i) + "_" +
743  // rtStatus.jointLimitData[i].jointName] =
744  // new Variant(rtStatus.jointLimitData[i].repulsiveTorque);
745  // }
746  // }
747  // }
748 
749  // // datafields["activeCollPairsNum"] = new Variant(rtStatus.activeCollPairsNum);
750  // // datafields["collisionPairsNum"] = new Variant(rtStatus.collisionPairsNum);
751  // // datafields["jointLimitNullspaceTime"] = new Variant(rtStatus.jointLimitNullspaceTime);
752  // // datafields["jointLimitTorqueTime"] = new Variant(rtStatus.jointLimitTorqueTime);
753  // // datafields["collisionTorqueTime"] = new Variant(rtStatus.collisionTorqueTime);
754  // // datafields["selfCollNullspaceTime"] = new Variant(rtStatus.selfCollNullspaceTime);
755  // // datafields["collisionPairTime"] = new Variant(rtStatus.collisionPairTime);
756  // // datafields["impForceRatio"] = new Variant(rtStatus.impForceRatio);
757  // // datafields["impTorqueRatio"] = new Variant(rtStatus.impTorqueRatio);
758 
759  // // double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
760  // // viz::Layer layer = arviz.layer(getName() + arm->kinematicChainName);
761  // // for (int i = 0; i < rtStatus.activeCollPairsNum; ++i)
762  // // {
763  // // visualize impedance force
764 
765  // // layer.add(viz::Arrow("projImpForce_" + std::to_string(i))
766  // // .fromTo(rtStatus.selfCollDataVec[i].point * 1000.0,
767  // // rtStatus.selfCollDataVec[i].point * 1000.0 +
768  // // rtStatus.selfCollDataVec[i].projectedImpedanceForce *
769  // // rtStatus.selfCollDataVec[i].direction * 5.0)
770  // // .color(simox::Color::purple()));
771 
772  // // layer.add(viz::Arrow("impForce_" + std::to_string(i))
773  // // .fromTo(rtStatus.selfCollDataVec[i].point * 1000.0,
774  // // rtStatus.selfCollDataVec[i].point * 1000.0 +
775  // // rtStatus.forceImpedance.head(3).dot(
776  // // rtStatus.selfCollDataVec[i].direction) *
777  // // rtStatus.selfCollDataVec[i].direction * 5.0)
778  // // .color(simox::Color::yellow())); // project forceImp in dir of collision
779  // // layer.add(viz::Arrow("force_" + std::to_string(i))
780  // // .fromTo(rtStatus.selfCollDataVec[i].point * 1000.0,
781  // // rtStatus.selfCollDataVec[i].point * 1000.0 +
782  // // rtStatus.selfCollDataVec[i].direction * 50.0 *
783  // // rtStatus.selfCollDataVec[i].repulsiveForce)
784  // // .color(simox::Color::blue()));
785 
786  // //
787 
788  // // //size_t index = rtStatus.distanceIndexPairs[i].second;
789  // // datafields[std::to_string(i) + "_minDistance"] =
790  // // new Variant(rtStatus.selfCollDataVec[i].minDistance);
791  // // datafields[std::to_string(i) + "_repForce"] =
792  // // new Variant(rtStatus.selfCollDataVec[i].repulsiveForce);
793  // // datafields[std::to_string(i) + "_dampingForce"] =
794  // // new Variant(-1.0f * rtStatus.selfCollDataVec[i].damping *
795  // // rtStatus.selfCollDataVec[i].distanceVelocity);
796  // // datafields[std::to_string(i) + "_n_des(d)"] =
797  // // new Variant(rtStatus.selfCollDataVec[i].desiredNSColl);
798  // // common::debugEigenVec(
799  // // datafields, std::to_string(i) + "_point", rtStatus.selfCollDataVec[i].point);
800  // // common::debugEigenVec(
801  // // datafields, std::to_string(i) + "_dir", rtStatus.selfCollDataVec[i].direction);
802  // //
803 
804  // // }
805 
806 
807  // /// prepare test case config
808  // const float tolerance = 1e-9f;
809  // if (rtData.testConfig == 1)
810  // {
811  // if (rtStatus.evalData[0].nodeName == "ArmL8_Wri2")
812  // {
813  // // set distance
814  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_dist"] =
815  // new Variant(rtStatus.evalData[0].minDistance);
816  // // set f_rep, damping, desired NS space
817 
818 
819  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_repF"] =
820  // new Variant(rtStatus.evalData[0].repulsiveForce);
821  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_projMass"] =
822  // new Variant(rtStatus.evalData[0].projectedMass);
823  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_locStiffness"] =
824  // new Variant(rtStatus.evalData[0].localStiffness);
825  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_dampFactor"] =
826  // new Variant(rtStatus.evalData[0].damping);
827  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_distVel"] =
828  // new Variant(rtStatus.evalData[0].distanceVelocity);
829  // if (fabs(rtStatus.evalData[0].damping + 1.0f) < tolerance)
830  // {
831  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_dampForce"] =
832  // new Variant(0.0);
833  // }
834  // else
835  // {
836  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_dampForce"] =
837  // new Variant(-1.0 * rtStatus.evalData[0].damping *
838  // rtStatus.evalData[0].distanceVelocity);
839  // }
840 
841  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_n_des(d)"] =
842  // new Variant(rtStatus.evalData[0].desiredNSColl);
843  // }
844  // // if (rtStatus.evalData[1].nodeName == "ArmL8_Wri2")
845  // // {
846  // // // set distance
847  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_dist"] =
848  // // new Variant(rtStatus.evalData[1].minDistance);
849  // // // set f_rep, damping, desired NS space
850 
851  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_repF"] =
852  // // new Variant(rtStatus.evalData[1].repulsiveForce);
853  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_projMass"] =
854  // // new Variant(rtStatus.evalData[1].projectedMass);
855  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_locStiffness"] =
856  // // new Variant(rtStatus.evalData[1].localStiffness);
857  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_dampFactor"] =
858  // // new Variant(rtStatus.evalData[1].damping);
859  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_distVel"] =
860  // // new Variant(rtStatus.evalData[1].distanceVelocity);
861  // // if (fabs(rtStatus.evalData[1].damping + 1.0f) < tolerance)
862  // // {
863  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_dampForce"] =
864  // // new Variant(0.0);
865  // // }
866  // // else
867  // // {
868  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_dampForce"] =
869  // // new Variant(-1.0 * rtStatus.evalData[1].damping *
870  // // rtStatus.evalData[1].distanceVelocity);
871  // // }
872 
873  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_n_des(d)"] =
874  // // new Variant(rtStatus.evalData[1].desiredNSColl);
875  // // }
876  // // if (rtStatus.evalData[2].nodeName == "ArmL7_Wri1")
877  // // {
878  // // // set distance
879  // // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_dist"] =
880  // // new Variant(rtStatus.evalData[2].minDistance);
881  // // // set f_rep, damping, desired NS space
882 
883  // // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_repF"] =
884  // // new Variant(rtStatus.evalData[2].repulsiveForce);
885 
886  // // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_projMass"] =
887  // // new Variant(rtStatus.evalData[2].projectedMass);
888  // // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_locStiffness"] =
889  // // new Variant(rtStatus.evalData[2].localStiffness);
890  // // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_dampFactor"] =
891  // // new Variant(rtStatus.evalData[2].damping);
892  // // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_distVel"] =
893  // // new Variant(rtStatus.evalData[2].distanceVelocity);
894  // // if (fabs(rtStatus.evalData[2].damping + 1.0f) < tolerance)
895  // // {
896  // // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_dampForce"] =
897  // // new Variant(0.0);
898  // // }
899  // // else
900  // // {
901  // // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_dampForce"] =
902  // // new Variant(-1.0 * rtStatus.evalData[2].damping *
903  // // rtStatus.evalData[2].distanceVelocity);
904  // // }
905 
906 
907  // // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_n_des(d)"] =
908  // // new Variant(rtStatus.evalData[2].desiredNSColl);
909  // // }
910  // // if (rtStatus.evalData[3].nodeName == "ArmL8_Wri2")
911  // // {
912  // // // set distance
913  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_dist"] =
914  // // new Variant(rtStatus.evalData[3].minDistance);
915  // // // set f_rep, damping, desired NS space
916 
917  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_repF"] =
918  // // new Variant(rtStatus.evalData[3].repulsiveForce);
919 
920  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_projMass"] =
921  // // new Variant(rtStatus.evalData[3].projectedMass);
922  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_locStiffness"] =
923  // // new Variant(rtStatus.evalData[3].localStiffness);
924  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_dampFactor"] =
925  // // new Variant(rtStatus.evalData[3].damping);
926  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_distVel"] =
927  // // new Variant(rtStatus.evalData[3].distanceVelocity);
928  // // if (fabs(rtStatus.evalData[3].damping + 1.0f) < tolerance)
929  // // {
930  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_dampForce"] =
931  // // new Variant(0.0);
932  // // }
933  // // else
934  // // {
935  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_dampForce"] =
936  // // new Variant(-1.0 * rtStatus.evalData[3].damping *
937  // // rtStatus.evalData[3].distanceVelocity);
938  // // }
939 
940 
941  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_n_des(d)"] =
942  // // new Variant(rtStatus.evalData[3].desiredNSColl);
943  // // }
944 
945  // // if (rtStatus.evalData[4].nodeName == "ArmL8_Wri2")
946  // // {
947  // // // set distance
948  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_dist"] =
949  // // new Variant(rtStatus.evalData[4].minDistance);
950  // // // set f_rep, damping, desired NS space
951 
952  // // // values have been calculated
953  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_repF"] =
954  // // new Variant(rtStatus.evalData[4].repulsiveForce);
955 
956  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_projMass"] =
957  // // new Variant(rtStatus.evalData[4].projectedMass);
958  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_locStiffness"] =
959  // // new Variant(rtStatus.evalData[4].localStiffness);
960  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_dampFactor"] =
961  // // new Variant(rtStatus.evalData[4].damping);
962  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_distVel"] =
963  // // new Variant(rtStatus.evalData[4].distanceVelocity);
964  // // if (fabs(rtStatus.evalData[4].damping + 1.0f) < tolerance)
965  // // {
966  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_dampForce"] =
967  // // new Variant(0.0);
968  // // }
969  // // else
970  // // {
971  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_dampForce"] =
972  // // new Variant(-1.0 * rtStatus.evalData[4].damping *
973  // // rtStatus.evalData[4].distanceVelocity);
974  // // }
975 
976 
977  // // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_n_des(d)"] =
978  // // new Variant(rtStatus.evalData[4].desiredNSColl);
979  // // }
980  // if (rtStatus.evalData[5].nodeName == "ArmL8_Wri2")
981  // {
982  // // set distance
983  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_dist"] =
984  // new Variant(rtStatus.evalData[5].minDistance);
985 
986  // // values have been calculated
987  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_repF"] =
988  // new Variant(rtStatus.evalData[5].repulsiveForce);
989 
990  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_projMass"] =
991  // new Variant(rtStatus.evalData[5].projectedMass);
992  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_locStiffness"] =
993  // new Variant(rtStatus.evalData[5].localStiffness);
994  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_dampFactor"] =
995  // new Variant(rtStatus.evalData[5].damping);
996  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_distVel"] =
997  // new Variant(rtStatus.evalData[5].distanceVelocity);
998  // if (fabs(rtStatus.evalData[5].damping + 1.0f) < tolerance)
999  // {
1000  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_dampForce"] =
1001  // new Variant(0.0);
1002  // }
1003  // else
1004  // {
1005  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_dampForce"] =
1006  // new Variant(-1.0 * rtStatus.evalData[5].damping *
1007  // rtStatus.evalData[5].distanceVelocity);
1008  // }
1009 
1010 
1011  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_n_des(d)"] =
1012  // new Variant(rtStatus.evalData[5].desiredNSColl);
1013  // }
1014  // if (rtStatus.evalData[6].nodeName == "ArmL5_Elb1")
1015  // {
1016  // // set distance
1017  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_dist"] =
1018  // new Variant(rtStatus.evalData[6].minDistance);
1019 
1020  // // values have been calculated
1021  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_repF"] =
1022  // new Variant(rtStatus.evalData[6].repulsiveForce);
1023 
1024  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_projMass"] =
1025  // new Variant(rtStatus.evalData[6].projectedMass);
1026  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_locStiffness"] =
1027  // new Variant(rtStatus.evalData[6].localStiffness);
1028  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_dampFactor"] =
1029  // new Variant(rtStatus.evalData[6].damping);
1030  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_distVel"] =
1031  // new Variant(rtStatus.evalData[6].distanceVelocity);
1032  // if (fabs(rtStatus.evalData[6].damping + 1.0f) < tolerance)
1033  // {
1034  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_dampForce"] =
1035  // new Variant(0.0);
1036  // }
1037  // else
1038  // {
1039  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_dampForce"] =
1040  // new Variant(-1.0 * rtStatus.evalData[6].damping *
1041  // rtStatus.evalData[6].distanceVelocity);
1042  // }
1043 
1044 
1045  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_n_des(d)"] =
1046  // new Variant(rtStatus.evalData[6].desiredNSColl);
1047  // }
1048  // }
1049 
1050  // // visualize impedance force
1051 
1052  // // layer.add(viz::Arrow("impForce")
1053  // // .fromTo(rtStatus.currentPose.block<3, 1>(0, 3),
1054  // // rtStatus.currentPose.block<3, 1>(0, 3) +
1055  // // rtStatus.forceImpedance.head(3) * 1000.0)
1056  // // .color(simox::Color::yellow()));
1057  // // layer.add(viz::Arrow("projImpForce")
1058  // // .fromTo(rtStatus.currentPose.block<3, 1>(0, 3),
1059  // // rtStatus.currentPose.block<3, 1>(0, 3) +
1060  // // rtStatus.projForceImpedance.head(3) * 1000.0)
1061  // // .color(simox::Color::purple()));
1062 
1063  // //arviz.commit(layer);
1064 
1065 
1066  // //double selfCollDebugTime = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
1067  // //datafields["selfCollDebugTime"] = new Variant(selfCollDebugTime);
1068 
1069  // // debug value assignment torso, neck joints
1070  // // datafields["torsoJointValue_float"] = new Variant(rtStatus.torsoJointValuef);
1071  // // datafields["neck1JointValue_float"] = new Variant(rtStatus.neck1JointValuef);
1072  // // datafields["neck2JointValue_float"] = new Variant(rtStatus.neck2JointValuef);
1073 
1074  // // datafields["torsoJointValue_double"] = new Variant(rtStatus.torsoJointValued);
1075  // // datafields["neck1JointValue_double"] = new Variant(rtStatus.neck1JointValued);
1076  // // datafields["neck2JointValue_double"] = new Variant(rtStatus.neck2JointValued);
1077 
1078  // // common::debugEigenVec(
1079  // // datafields, "set_ActuatedJointValues", rtStatus.setActuatedJointValues.cast<float>());
1080  // // common::debugEigenVec(
1081  // // datafields, "get_ActuatedJointValues", rtStatus.getActuatedJointValues.cast<float>());
1082 
1083  // double limbTime = IceUtil::Time::now().toMicroSecondsDouble() - limbTimeMeasure;
1084  // datafields["limbOnPublishTime"] = new Variant(limbTime);
1085 
1086  debugObs->setDebugChannel(
1087  "NJointTaskspaceSafetyImpedanceController_" + arm->kinematicChainName, datafields);
1088  }
1089 
1090  void
1091  NJointTaskspaceSafetyImpedanceController::onPublish(const SensorAndControl&,
1092  const DebugDrawerInterfacePrx&,
1093  const DebugObserverInterfacePrx& debugObs)
1094  {
1095  for (auto& pair : limb)
1096  {
1097  if (not pair.second->rtReady.load())
1098  continue;
1099 
1100  limbPublish(pair.second, debugObs);
1101  }
1102  }
1103 
1104  void
1105  NJointTaskspaceSafetyImpedanceController::calibrateFTSensor(const Ice::Current&)
1106  {
1107  for (auto& pair : limb)
1108  {
1109  pair.second->rtReady.store(false);
1110  }
1111  }
1112 
1113  void
1114  NJointTaskspaceSafetyImpedanceController::limbReInit(ArmPtr& arm)
1115  {
1116  VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(arm->kinematicChainName);
1117  Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
1118  ARMARX_IMPORTANT << "rt preactivate controller with target pose\n\n" << currentPose;
1119 
1120  if (arm->reInitPreActivate.load())
1121  {
1122  arm->rtConfig.desiredNullspaceJointAngles.value() = rns->getJointValuesEigen();
1123  arm->rtConfig.desiredPose = currentPose;
1124 
1125  arm->bufferConfigUserToNonRt.reinitAllBuffers(arm->rtConfig);
1126  arm->reInitPreActivate.store(false);
1127  }
1128  {
1129  arm->nonRtConfig = arm->rtConfig;
1130  arm->bufferConfigNonRtToRt.reinitAllBuffers(arm->rtConfig);
1131  arm->bufferConfigRtToOnPublish.reinitAllBuffers(arm->rtConfig);
1132  arm->bufferConfigRtToUser.reinitAllBuffers(arm->rtConfig);
1133  }
1134 
1135  const auto nDoF = rns->getSize();
1136  {
1137  arm->rtStatus.numJoints = nDoF;
1138  arm->rtStatus.desiredJointTorques.setZero(nDoF);
1139  arm->rtStatus.forceImpedance.setZero();
1140  arm->rtStatus.kdForceImpedance.setZero();
1141  arm->rtStatus.nullspaceTorque.setZero(nDoF);
1142  arm->rtStatus.jointDampingTorque.setZero(nDoF);
1143 
1144  arm->rtStatus.jointPosition.resize(nDoF, 0.);
1145  arm->rtStatus.jointVelocity.resize(nDoF, 0.);
1146  arm->rtStatus.jointTorque.resize(nDoF, 0.);
1147  for (size_t i = 0; i < nDoF; ++i)
1148  {
1149  arm->rtStatus.jointPosition[i] = arm->sensorDevices.positionSensors[i]->position;
1150  arm->rtStatus.jointVelocity[i] = arm->sensorDevices.velocitySensors[i]->velocity;
1151  arm->rtStatus.jointTorque[i] = arm->sensorDevices.torqueSensors[i]->torque;
1152  }
1153 
1154  arm->rtStatus.qpos = rns->getJointValuesEigen();
1155  arm->rtStatus.qvel.setZero(nDoF);
1156  arm->rtStatus.qvelFiltered.setZero(nDoF);
1157 
1158  arm->rtStatus.currentPose = currentPose;
1159  arm->rtStatus.desiredPose = currentPose;
1160  arm->rtStatus.currentTwist.setZero();
1161  arm->rtStatus.poseDiffMatImp.setZero();
1162  arm->rtStatus.poseErrorImp.setZero();
1163  arm->rtStatus.trackingError = 0.0;
1164  arm->rtStatus.jacobi.setZero(6, nDoF);
1165  arm->rtStatus.jtpinv.setZero(6, nDoF);
1166 
1167  arm->rtStatus.dirErrorImp.setZero();
1168  arm->rtStatus.totalForceImpedance = 0.0;
1169  arm->rtStatus.projForceImpedance.setZero();
1170  arm->rtStatus.projTotalForceImpedance = 0.0;
1171 
1172  arm->rtStatus.impedanceJointTorque.setZero(nDoF);
1173  arm->rtStatus.kdImpedanceTorque.setZero(nDoF);
1174  arm->rtStatus.selfCollisionJointTorque.setZero(nDoF);
1175  arm->rtStatus.jointLimitJointTorque.setZero(nDoF);
1176 
1177  arm->rtStatus.selfCollisionTorqueFiltered.setZero(nDoF);
1178  arm->rtStatus.jointLimitTorqueFiltered.setZero(nDoF);
1179 
1180  arm->rtStatus.projImpedanceJointTorque.setZero(nDoF);
1181  arm->rtStatus.projSelfCollJointTorque.setZero(nDoF);
1182  arm->rtStatus.projJointLimJointTorque.setZero(nDoF);
1183 
1184  arm->rtStatus.impForceRatio = 0.0f;
1185  arm->rtStatus.impTorqueRatio = 0.0f;
1186 
1187  arm->rtStatus.selfCollNullSpace.resize(nDoF, nDoF);
1188  arm->rtStatus.selfCollNullSpace.setZero();
1189  arm->rtStatus.tempNullSpaceMatrix.resize(nDoF, nDoF);
1190  arm->rtStatus.tempNullSpaceMatrix.setZero();
1191  arm->rtStatus.jointLimNullSpace.resize(nDoF, nDoF);
1192  arm->rtStatus.jointLimNullSpace.setZero();
1193 
1194  arm->rtStatus.selfCollNullSpaceFiltered.resize(nDoF, nDoF);
1195  arm->rtStatus.selfCollNullSpaceFiltered.setZero();
1196  arm->rtStatus.jointLimNullSpaceFiltered.resize(nDoF, nDoF);
1197  arm->rtStatus.jointLimNullSpaceFiltered.setZero();
1198 
1199  arm->rtStatus.desiredNullSpace = 0.0;
1200  arm->rtStatus.selfCollisionNullSpaceWeights.setZero();
1201  arm->rtStatus.k1 = 0.0;
1202  arm->rtStatus.k2 = 0.0;
1203  arm->rtStatus.k3 = 0.0;
1204  arm->rtStatus.k4 = 0.0;
1205  arm->rtStatus.k1Lo = 0.0;
1206  arm->rtStatus.k2Lo = 0.0;
1207  arm->rtStatus.k3Lo = 0.0;
1208  arm->rtStatus.k4Lo = 0.0;
1209  arm->rtStatus.k1Hi = 0.0;
1210  arm->rtStatus.k2Hi = 0.0;
1211  arm->rtStatus.k3Hi = 0.0;
1212  arm->rtStatus.k4Hi = 0.0;
1213 
1214  arm->rtStatus.normalizedJacT.setZero(nDoF);
1215 
1216  arm->rtStatus.jointLimitData.resize(nDoF);
1217  arm->rtStatus.jointVel = 0.0;
1218  arm->rtStatus.jointInertia = 0.0;
1219  arm->rtStatus.localStiffnessJointLim = 0.0;
1220  arm->rtStatus.dampingJointLim = 0.0;
1221 
1222  arm->rtStatus.selfCollDataVec.resize(
1223  maxCollisionPairs,
1224  law::SafetyTaskspaceImpedanceController::RtStatus::selfCollisionData(nDoF));
1225  arm->rtStatus.evalData.resize(
1226  10, law::SafetyTaskspaceImpedanceController::RtStatus::selfCollisionData(nDoF));
1227  arm->rtStatus.evalDataIndex = 0;
1228 
1229  Eigen::Vector3d point1(0.0, 0.0, 0.0);
1230  Eigen::Vector3d point2(0.0, 0.0, 0.0);
1231  Eigen::Vector3d norm(0.0, 0.0, 0.0);
1232  arm->rtStatus.activeDistPair.node1 = "";
1233  arm->rtStatus.activeDistPair.node2 = "";
1234  arm->rtStatus.activeDistPair.point1 = point1;
1235  arm->rtStatus.activeDistPair.point2 = point2;
1236  arm->rtStatus.activeDistPair.normalVec = norm;
1237  arm->rtStatus.activeDistPair.minDistance = -1.0;
1238 
1239  arm->rtStatus.inertia.resize(nDoF, nDoF);
1240  arm->rtStatus.inertia.setZero();
1241 
1242  arm->rtStatus.inertiaInverse.resize(nDoF, nDoF);
1243  arm->rtStatus.inertiaInverse.setZero();
1244 
1245  arm->rtStatus.deltaT = 0.0;
1246  arm->rtStatus.rtSafe = false;
1247 
1248  arm->rtStatus.currentForceTorque.setZero();
1249 
1250  arm->rtStatusInNonRT = arm->rtStatus;
1251  arm->bufferRtStatusToOnPublish.reinitAllBuffers(arm->rtStatus);
1252  arm->bufferRtStatusToNonRt.reinitAllBuffers(arm->rtStatus);
1253  arm->bufferRtStatusToUser.reinitAllBuffers(arm->rtStatus);
1254 
1255  for (size_t i = 0; i < arm->pointsOnArm->size(); i++)
1256  {
1257  arm->pointsOnArm->at(i) = -1;
1258  }
1259 
1260  /// init time measures with zero
1261  arm->rtStatus.collisionPairTime = 0.0;
1262  arm->rtStatus.preFilterTime = 0.0;
1263  arm->rtStatus.collisionTorqueTime = 0.0;
1264  arm->rtStatus.jointLimitTorqueTime = 0.0;
1265  arm->rtStatus.selfCollNullspaceTime = 0.0;
1266  arm->rtStatus.jointLimitNullspaceTime = 0.0;
1267 
1268  arm->rtStatus.collisionPairsNum = 0;
1269  arm->rtStatus.activeCollPairsNum = 0;
1270 
1271  arm->rtStatus.localTransformation = Eigen::Isometry3d::Identity();
1272  arm->rtStatus.node1OnArm = false;
1273  arm->rtStatus.node2OnArm = false;
1274  arm->rtStatus.node1Index = 0;
1275  arm->rtStatus.node2Index = 0;
1276 
1277  arm->rtStatus.torsoJointValuef = 0.0f;
1278  arm->rtStatus.neck1JointValuef = 0.0f;
1279  arm->rtStatus.neck2JointValuef = 0.0f;
1280  arm->rtStatus.torsoJointValued = 0.0;
1281  arm->rtStatus.neck1JointValued = 0.0;
1282  arm->rtStatus.neck2JointValued = 0.0;
1283  arm->rtStatus.setActuatedJointValues.resize(18);
1284  arm->rtStatus.setActuatedJointValues.setZero();
1285  arm->rtStatus.getActuatedJointValues.resize(18);
1286  arm->rtStatus.setActuatedJointValues.setZero();
1287  }
1288 
1289  arm->controller.preactivateInit(rns, arm->rtConfig, arm->rtStatus);
1290  }
1291 
1292  void
1293  NJointTaskspaceSafetyImpedanceController::rtPreActivateController()
1294  {
1295  collisionPairs->reserve(maxCollisionPairs);
1296 
1297  for (auto& pair : limb)
1298  {
1299  ARMARX_INFO << "rtPreActivateController for " << pair.first;
1300  limbReInit(pair.second);
1301  userConfig.limbs.at(pair.first) = pair.second->rtConfig;
1302  }
1303  }
1304 
1305  void
1306  NJointTaskspaceSafetyImpedanceController::rtPostDeactivateController()
1307  {
1308  for (auto& pair : limb)
1309  {
1310  pair.second->rtReady.store(false);
1311  pair.second->rtFirstRun.store(true);
1312  }
1313 
1314  ARMARX_INFO << "-- post deactivate --";
1315  // for (auto& pair : limb)
1316  // {
1317  // pair.second->controller.isInitialized.store(false);
1318  // }
1319  }
1320 } // namespace armarx::control::njoint_controller::task_space
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::additionalTask
virtual void additionalTask()
Definition: SafetyImpedanceController.cpp:233
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::collisionRobotPtr
std::unique_ptr<::simox::control::environment::CollisionRobot< hpp::fcl::OBBRSS > > collisionRobotPtr
Definition: SafetyImpedanceController.h:168
armarx::ControlTargetBase::asA
const T * asA() const
Definition: ControlTargetBase.h:76
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::validateConfigData
void validateConfigData(Config &config, ArmPtr &arm)
Definition: SafetyImpedanceController.cpp:604
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::isSafeForceTorque
bool isSafeForceTorque(const std::string &nodeSetName, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: SafetyImpedanceController.cpp:544
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::onPublish
void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition: SafetyImpedanceController.cpp:1091
ARMARX_CHECK_NONNEGATIVE
#define ARMARX_CHECK_NONNEGATIVE(number)
Check whether number is nonnegative (>= 0). If it is not, throw an ExpressionException with the expre...
Definition: ExpressionException.h:152
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::qposActuatedJoints
Eigen::VectorXd qposActuatedJoints
Definition: SafetyImpedanceController.h:177
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::maxCollisionPairs
int maxCollisionPairs
Definition: SafetyImpedanceController.h:175
armarx::control::ethercat::RTUtility::RT_THREAD_PRIORITY
static constexpr int RT_THREAD_PRIORITY
The thread priority for rt-threads.
Definition: RTUtility.h:24
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::updateConfig
void updateConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
NJointController interface.
Definition: SafetyImpedanceController.cpp:503
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
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::NJointTaskspaceSafetyImpedanceController::limb
std::map< std::string, ArmPtr > limb
Definition: SafetyImpedanceController.h:164
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::njoint_controller::task_space
Definition: AdmittanceController.cpp:37
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::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::limbReInit
void limbReInit(ArmPtr &arm)
Definition: SafetyImpedanceController.cpp:1114
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:725
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::getTCPVel
Ice::FloatSeq getTCPVel(const std::string &rns, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: SafetyImpedanceController.cpp:489
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::onInitNJointController
void onInitNJointController() override
NJointControllerBase interface.
Definition: SafetyImpedanceController.cpp:199
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::NJointControllerBase::ConfigPtrT
NJointControllerConfigPtr ConfigPtrT
Definition: NJointControllerBase.h:586
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::collisionPairs
std::shared_ptr< std::vector<::simox::control::environment::DistanceResult > > collisionPairs
store collision pairs in vector to be able to preallocate memory
Definition: SafetyImpedanceController.h:173
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:845
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:177
armarx::control::TargetNullspaceMap
dictionary< string, Ice::FloatSeq > TargetNullspaceMap
Definition: ControllerInterface.ice:40
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::actuatedJoints
std::vector< std::string > actuatedJoints
Definition: SafetyImpedanceController.h:176
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::rtPostDeactivateController
void rtPostDeactivateController() override
This function is called after the controller is deactivated.
Definition: SafetyImpedanceController.cpp:1306
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition: NJointControllerBase.h:777
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:753
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: SafetyImpedanceController.cpp:1293
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::NJointTaskspaceSafetyImpedanceController
NJointTaskspaceSafetyImpedanceController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: SafetyImpedanceController.cpp:118
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::Config
law::SafetyTaskspaceImpedanceController::Config Config
Definition: SafetyImpedanceController.h:62
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: SafetyImpedanceController.cpp:366
ArmarXObjectScheduler.h
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::NJointTaskspaceSafetyImpedanceController::getConfig
::armarx::aron::data::dto::DictPtr getConfig(const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: SafetyImpedanceController.cpp:593
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
ARMARX_CHECK_LESS_EQUAL
#define ARMARX_CHECK_LESS_EQUAL(lhs, rhs)
This macro evaluates whether lhs is less or equal (<=) rhs and if it turns out to be false it will th...
Definition: ExpressionException.h:109
utils.h
CycleUtil.h
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::simoxReducedRobotPtr
std::shared_ptr< simox::control::simox::robot::Robot > simoxReducedRobotPtr
Definition: SafetyImpedanceController.h:170
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::limbRT
void limbRT(ArmPtr &arm, const double deltaT)
Definition: SafetyImpedanceController.cpp:273
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::nonRtRobot
VirtualRobot::RobotPtr nonRtRobot
Definition: SafetyImpedanceController.h:165
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
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
SafetyImpedanceController.h
TimeUtil.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::enableSafeGuardForceTorque
void enableSafeGuardForceTorque(const std::string &nodeSetName, const bool forceGuard, const bool torqueGuard, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: SafetyImpedanceController.cpp:518
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::userConfig
ConfigDict userConfig
Definition: SafetyImpedanceController.h:171
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::limbPublish
void limbPublish(ArmPtr &arm, const DebugObserverInterfacePrx &debugObs)
Definition: SafetyImpedanceController.cpp:683
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::simoxControlRobotPtr
std::shared_ptr<::simox::control::simox::robot::Robot > simoxControlRobotPtr
Definition: SafetyImpedanceController.h:169
IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface >
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::robotUnit
RobotUnitPtr robotUnit
Definition: SafetyImpedanceController.h:166
armarx::control::TargetPoseMap
dictionary< string, FloatSeqSeq > TargetPoseMap
Definition: ControllerInterface.ice:39
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::limbInit
void limbInit(const std::string nodeSetName, ArmPtr &arm, Config &cfg, VirtualRobot::RobotPtr &nonRtRobotPtr, std::shared_ptr< std::vector<::simox::control::environment::DistanceResult >> collisionPairs)
Definition: SafetyImpedanceController.cpp:44
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::preFilterDistance
float preFilterDistance
Definition: SafetyImpedanceController.h:174
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:186
ARMARX_RT_LOGF_WARN
#define ARMARX_RT_LOGF_WARN(...)
Definition: ControlThreadOutputBuffer.h:345
armarx::control::NJointTaskspaceSafetyImpedanceControllerInterface::calibrateFTSensor
void calibrateFTSensor()
ft sensor
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::ArmPtr
std::unique_ptr< ArmData > ArmPtr
Definition: SafetyImpedanceController.h:112
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::getClassName
std::string getClassName(const Ice::Current &=Ice::emptyCurrent) const override
Definition: SafetyImpedanceController.cpp:193
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
RTUtility.h
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:731
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
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
armarx::control::njoint_controller::task_space::registrationControllerNJointTaskspaceSafetyImpedanceController
NJointControllerRegistration< NJointTaskspaceSafetyImpedanceController > registrationControllerNJointTaskspaceSafetyImpedanceController("NJointTaskspaceSafetyImpedanceController")
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::updateTargetPose
bool updateTargetPose(const TargetPoseMap &targetPoseMap, const TargetNullspaceMap &targetNullspaceMap, const Ice::Current &=Ice::emptyCurrent) override
Definition: SafetyImpedanceController.cpp:561
armarx::control::njoint_controller::task_space::NJointTaskspaceSafetyImpedanceController::limbNonRT
void limbNonRT(ArmPtr &arm)
Definition: SafetyImpedanceController.cpp:225
norm
double norm(const Point &a)
Definition: point.hpp:94