SafetyTaskspaceImpedanceController.cpp
Go to the documentation of this file.
1 #include "SafetyTaskspaceImpedanceController.h"
2 
3 #include <SimoxUtility/math/convert/mat4f_to_quat.h>
4 #include <VirtualRobot/MathTools.h>
5 
6 #include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
7 
8 #include "../utils.h"
9 #include <simox/control/robot/NodeInterface.h>
10 
11 namespace armarx::control::common::control_law
12 {
13 
14  void
15  SafetyTaskspaceImpedanceController::initialize(const VirtualRobot::RobotNodeSetPtr& rns,
16  const VirtualRobot::RobotNodeSetPtr& rtRns,
17  SimoxRobotPtr& simoxControlRobot)
18  {
19  ARMARX_CHECK_NOT_NULL(rns);
20  tcp = rns->getTCP();
21  rtTCP = rtRns->getTCP();
22  ARMARX_CHECK_NOT_NULL(tcp);
23  ik.reset(new VirtualRobot::DifferentialIK(
24  rtRns, rtRns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
25 
26  // simox nodeset is needed for the jacobian calculation in the calculateSelfCollisionTorque
27  // method
28  simoxNodeSet = simoxControlRobot->getSubNodeSet(rns->getName());
29  ARMARX_CHECK_NOT_NULL(simoxNodeSet);
30 
31  numOfJoints = rns->getSize();
32  I = Eigen::MatrixXf::Identity(numOfJoints, numOfJoints);
33  }
34 
35  void
36  SafetyTaskspaceImpedanceController::preactivateInit(const VirtualRobot::RobotNodeSetPtr& rns,
37  Config& c,
38  RtStatus& rtStatus)
39  {
40  /// calculate weights for self-collision nullspace transition function
41  /// range between z1 and z2
42  /// z1: below this distance [m] the collision direction is fully locked
43  /// z2: above this distance collision direction is unrestricted
44  rtStatus.selfCollisionNullSpaceWeights =
45  calculateTransitionWeights(c.selfCollActivatorZ1, c.selfCollActivatorZ2);
46 
47  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rns->getSize());
48  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jointLimitData.size());
49 
50  /// obtain joint information (limits, limitless) from VirtualRobot
51  for (size_t i = 0; i < rns->getSize(); ++i)
52  {
53  rtStatus.jointLimitData[i].jointName = rns->getNode(i)->getName();
54  rtStatus.jointLimitData[i].isLimitless = rns->getNode(i)->isLimitless();
55 
56  if (not rtStatus.jointLimitData[i].isLimitless)
57  {
58  rtStatus.jointLimitData[i].qLimLow = rns->getNode(i)->getJointLimitLow();
59  rtStatus.jointLimitData[i].qLimHigh = rns->getNode(i)->getJointLimitHigh();
60  }
61  }
62 
63  // std::vector<std::string> jointInfo;
64  for (auto& joint : rtStatus.jointLimitData)
65  {
66  if (not joint.isLimitless)
67  {
68  /// set joint limit avoidance threshold parameters for not limiteless joints
69  float qRange = joint.qLimHigh - joint.qLimLow;
70  joint.thresholdRange = c.jointRangeBufferZone * qRange;
71  joint.qposThresholdLow = joint.qLimLow + joint.thresholdRange;
72  joint.qposThresholdHigh = joint.qLimHigh - joint.thresholdRange;
73  float qposZ1 = c.jointRangeBufferZoneZ1 * qRange;
74  float qposZ2 = c.jointRangeBufferZoneZ2 * qRange;
75  joint.qposZ1Low = joint.qLimLow + qposZ1;
76  joint.qposZ2Low = joint.qLimLow + qposZ2;
77  joint.qposZ1High = joint.qLimHigh - qposZ1;
78  joint.qposZ2High = joint.qLimHigh - qposZ2;
79  joint.jointLimitNullSpaceWeightsLow =
80  calculateTransitionWeights(joint.qposZ1Low, joint.qposZ2Low);
81  joint.jointLimitNullSpaceWeightsHigh =
82  calculateTransitionWeights(joint.qposZ1High, joint.qposZ2High);
83 
84  /// from here: only the parameter output information is prepared
85  // std::vector<float> params = {joint.qLimLow,
86  // joint.qLimHigh,
87  // joint.qposThresholdLow,
88  // joint.qposThresholdHigh,
89  // joint.qposZ1Low,
90  // joint.qposZ2Low,
91  // joint.qposZ1High,
92  // joint.qposZ2High,
93  // joint.jointLimitNullSpaceWeightsLow(0),
94  // joint.jointLimitNullSpaceWeightsLow(1),
95  // joint.jointLimitNullSpaceWeightsLow(2),
96  // joint.jointLimitNullSpaceWeightsLow(3),
97  // joint.jointLimitNullSpaceWeightsHigh(0),
98  // joint.jointLimitNullSpaceWeightsHigh(1),
99  // joint.jointLimitNullSpaceWeightsHigh(2),
100  // joint.jointLimitNullSpaceWeightsHigh(3)};
101  // std::vector<std::string> stringParams;
102  // // convert float values to string rounded to two digits
103  // for (auto& param : params)
104  // {
105  // std::stringstream ss;
106  // ss << std::fixed << std::setprecision(2) << param;
107  // stringParams.push_back(ss.str());
108  // }
109  // std::string info = joint.jointName + ": LimLo: " + stringParams[0] +
110  // " - LimHi: " + stringParams[1] + " - q0L: " + stringParams[2] +
111  // " - q0H: " + stringParams[3] + " - z1L: " + stringParams[4] +
112  // " z2L: " + stringParams[5] + " z1H: " + stringParams[6] +
113  // " z2H: " + stringParams[7] + " k1L: " + stringParams[8] +
114  // " k2L: " + stringParams[9] + " k3L: " + stringParams[10] +
115  // " k4L: " + stringParams[11] + " k1H: " + stringParams[12] +
116  // " k2H: " + stringParams[13] + " k3H: " + stringParams[14] +
117  // " k4H: " + stringParams[15];
118  // jointInfo.push_back(info);
119  }
120  }
121 
122  // std::ostringstream oss;
123  // for (const auto& str : jointInfo)
124  // {
125  // oss << str << "\n";
126  // }
127  // std::string initMessage = oss.str();
128  // ARMARX_INFO << "joint limit avoidance parameters initialized for: \n" << initMessage;
129  }
130 
131  void
132  SafetyTaskspaceImpedanceController::firstRun()
133  {
134  ftsensor.reset();
135  }
136 
137  // void
138  // SafetyTaskspaceImpedanceController::updateFT(const FTConfig& c, RtStatus& rtStatus)
139  // {
140  // /// run in rt thread
141  // ftsensor.updateStatus();
142  // if (c.reCalibrate)
143  // {
144  // ftsensor.calibrated.store(false);
145  // /// TODO how to toggle recalibration?
146  // }
147  // ftsensor.compensateTCPGravity(c);
148  // if (!ftsensor.calibrated.load())
149  // {
150  // if (ftsensor.calibrate(c, rtStatus.deltaT))
151  // {
152  // ftsensor.calibrated.store(true);
153  // }
154  // }
155  // else
156  // {
157  // rtStatus.currentForceTorque = ftsensor.getFilteredForceTorque(c);
158  // }
159  // }
160 
161  /// methods for self-collision and joint limit avoidance
162  /// avoidance torque methods
163  void
164  SafetyTaskspaceImpedanceController::calculateSelfCollisionTorque(
165  Config& c,
166  RtStatus& rts,
167  std::shared_ptr<std::vector<::simox::control::environment::DistanceResult>> collisionPairs,
168  std::shared_ptr<std::vector<int>> pointsOnArm,
169  int pointsOnArmIndex,
170  std::vector<std::string> selfCollisionNodes)
171  {
172  /// self-collision avoidance algorithm following the methods in Dietrich et al. (2012):
173  ///
174  /// A. Dietrich, T. Wimbock, A. Albu-Schaffer and G. Hirzinger, "Integration of Reactive,
175  /// Torque-Based Self-Collision Avoidance Into a Task Hierarchy," in IEEE Transactions on
176  /// Robotics, vol. 28, no. 6, pp. 1278-1293, Dec. 2012, doi: 10.1109/TRO.2012.2208667.
177  ///
178  /// see: https://ieeexplore.ieee.org/document/6255795
179  ///
180  /// method:
181  ///
182  /// computes the joint torques to avoid self-collisions -> selfCollisionJointTorque
183 
184  ARMARX_CHECK_GREATER(collisionPairs->size(), 0);
185 
186  /// clear values before new cycle
187  rts.selfCollisionJointTorque.setZero();
188 
189  for (auto& data : rts.selfCollDataVec)
190  {
191  data.clearValues();
192  }
193 
194  for (auto& data : rts.evalData)
195  {
196  // clear eval data each cycle
197  data.clearValues();
198  }
199 
200  ARMARX_CHECK_LESS_EQUAL(pointsOnArmIndex, static_cast<int>(rts.selfCollDataVec.size()));
201 
202  /// check collision pairs, which have are below the prefilter distance
203  for (int i = 0; i < pointsOnArmIndex; ++i)
204  {
205  ARMARX_CHECK_AND_THROW((pointsOnArm->at(i) != -1), std::out_of_range);
206 
207  // copy data into preallocated space
208  rts.activeDistPair.node1 = collisionPairs->at(pointsOnArm->at(i)).node1;
209  rts.activeDistPair.node2 = collisionPairs->at(pointsOnArm->at(i)).node2;
210  rts.activeDistPair.point1 = collisionPairs->at(pointsOnArm->at(i)).point1;
211  rts.activeDistPair.point2 = collisionPairs->at(pointsOnArm->at(i)).point2;
212  rts.activeDistPair.normalVec = collisionPairs->at(pointsOnArm->at(i)).normalVec;
213  rts.activeDistPair.minDistance = collisionPairs->at(pointsOnArm->at(i)).minDistance;
214 
215  /// check, whether both nodes are on arm
216  rts.node1OnArm = false;
217  rts.node2OnArm = false;
218 
219  if (std::find(selfCollisionNodes.begin(),
220  selfCollisionNodes.end(),
221  rts.activeDistPair.node1) != selfCollisionNodes.end())
222  {
223  rts.node1OnArm = true;
224  }
225  if (std::find(selfCollisionNodes.begin(),
226  selfCollisionNodes.end(),
227  rts.activeDistPair.node2) != selfCollisionNodes.end())
228  {
229  rts.node2OnArm = true;
230  }
231 
232 
233  // all distances are in meter
234  if (static_cast<float>(rts.activeDistPair.minDistance) < c.distanceThreshold)
235  {
236  if (rts.node1OnArm and rts.node2OnArm)
237  {
238  // check, which node is further away from root in kinematicChain
239  // apply force only to contact point further away from root
240  rts.node1Index = simoxNodeSet->getNodeIndex(rts.activeDistPair.node1);
241  rts.node2Index = simoxNodeSet->getNodeIndex(rts.activeDistPair.node2);
242  if (rts.node1Index < rts.node2Index)
243  {
244  // node2 is active node
245  rts.node1OnArm = false;
246  }
247  else
248  {
249  // node1 is active node
250  rts.node2OnArm = false;
251  }
252  }
253 
254  if (rts.node1OnArm)
255  {
256  /// node 1 is the current contact point
257  rts.selfCollDataVec[i].nodeName = rts.activeDistPair.node1;
258  rts.selfCollDataVec[i].otherName = rts.activeDistPair.node2;
259  rts.selfCollDataVec[i].minDistance =
260  static_cast<float>(rts.activeDistPair.minDistance);
261  rts.selfCollDataVec[i].point.x() =
262  static_cast<float>(rts.activeDistPair.point1->x());
263  rts.selfCollDataVec[i].point.y() =
264  static_cast<float>(rts.activeDistPair.point1->y());
265  rts.selfCollDataVec[i].point.z() =
266  static_cast<float>(rts.activeDistPair.point1->z());
267 
268  // get jacobian
269  // todo: optimize calculation of the jacobian (time efficiency)
270  rts.node = simoxNodeSet->getNode(rts.activeDistPair.node1);
271 
272  rts.localTransformation = Eigen::Isometry3d::Identity();
273  rts.localTransformation.translation() =
274  rts.node->getGlobalPose().inverse() * rts.activeDistPair.point1.value();
275 
276  rts.selfCollDataVec[i].jacobian =
277  simoxNodeSet
278  ->computeJacobian(*rts.node,
279  simox::control::robot::JacobianParams{
280  .mode = simox::control::utils::CartesianSelection::Position,
281  .localTransformation = rts.localTransformation})
282  .cast<float>();
283 
284  ARMARX_CHECK_EQUAL(rts.numJoints, rts.selfCollDataVec[i].jacobian.cols());
285  ARMARX_CHECK_EQUAL(3, rts.selfCollDataVec[i].jacobian.rows());
286 
287  // direction is pointing away from collision
288  // (make sure individualColObjects in computeMinimumSelfCollisionDistance method
289  // are set to true)
290  rts.selfCollDataVec[i].direction =
291  (rts.activeDistPair.point1.value() - rts.activeDistPair.point2.value())
292  .cast<float>();
293  }
294  else if (rts.node2OnArm)
295  {
296  /// node 2 is the current contact point
297 
298  rts.selfCollDataVec[i].nodeName = rts.activeDistPair.node2;
299  rts.selfCollDataVec[i].otherName = rts.activeDistPair.node1;
300  rts.selfCollDataVec[i].minDistance =
301  static_cast<float>(rts.activeDistPair.minDistance);
302 
303  rts.selfCollDataVec[i].point.x() =
304  static_cast<float>(rts.activeDistPair.point2->x());
305  rts.selfCollDataVec[i].point.y() =
306  static_cast<float>(rts.activeDistPair.point2->y());
307  rts.selfCollDataVec[i].point.z() =
308  static_cast<float>(rts.activeDistPair.point2->z());
309 
310  // get jacobian
311  // todo: optimize calculation of the jacobian (time efficiency)
312  rts.node = simoxNodeSet->getNode(rts.activeDistPair.node2);
313 
314  rts.localTransformation = Eigen::Isometry3d::Identity();
315  rts.localTransformation.translation() =
316  rts.node->getGlobalPose().inverse() * rts.activeDistPair.point1.value();
317 
318  rts.selfCollDataVec[i].jacobian =
319  simoxNodeSet
320  ->computeJacobian(*rts.node,
321  simox::control::robot::JacobianParams{
322  .mode = simox::control::utils::CartesianSelection::Position,
323  .localTransformation = rts.localTransformation})
324  .cast<float>();
325 
326  ARMARX_CHECK_EQUAL(rts.numJoints, rts.selfCollDataVec[i].jacobian.cols());
327  ARMARX_CHECK_EQUAL(3, rts.selfCollDataVec[i].jacobian.rows());
328 
329 
330  // direction is pointing away from collision
331  // (make sure individualColObjects in computeMinimumSelfCollisionDistance method
332  // are set to true)
333  rts.selfCollDataVec[i].direction =
334  (rts.activeDistPair.point2.value() - rts.activeDistPair.point1.value())
335  .cast<float>();
336  }
337  else
338  {
339  ARMARX_RT_LOGF_ERROR(
340  "The indeces in the pointsOnArm vector must correspond"
341  "to a contact point pair, where at least one node is "
342  "located on the arm. \n There is a problem in the preFilter"
343  "method in the rtRun method.")
344  .deactivateSpam(1.0);
345  continue;
346  }
347 
348 
349  /// check for negative distance and flip normal vector if true
350  if (rts.selfCollDataVec[i].minDistance < 0.0f)
351  {
352  rts.selfCollDataVec[i].direction *= -1.0f;
353  rts.selfCollDataVec[i].minDistance = 0.0f;
354  // check for overlapping spheres or capsules overlapping spheres
355  // (returned points are exactly the same)
356  if (rts.activeDistPair.point1->isApprox(*rts.activeDistPair.point2, 1e-8) and
357  rts.activeDistPair.normalVec != std::nullopt)
358  {
359  /// if the points are the same, normalVec comes with a value
360 
361  // todo: how to make sure the normalVec is always pointing away from the
362  // collision, in normally the vector points out from the contact point within
363  // the sphere
364  // issue: if the arm is modeled with a sphere, it will point towards the collision
365  // No faulty behavior has been detected so far, however there is the possibility
366  // to get a direction vector pointing towards the collision
367 
368  // solution: check if the node is a sphere, if the repulsive force is suppose to apply on the sphere, use the negative normal direction
369  rts.selfCollDataVec[i].direction.x() =
370  static_cast<float>(rts.activeDistPair.normalVec->x());
371  rts.selfCollDataVec[i].direction.y() =
372  static_cast<float>(rts.activeDistPair.normalVec->y());
373  rts.selfCollDataVec[i].direction.z() =
374  static_cast<float>(rts.activeDistPair.normalVec->z());
375  }
376  }
377  rts.selfCollDataVec[i].direction.normalize();
378 
379  ARMARX_CHECK_NONNEGATIVE(rts.selfCollDataVec[i].minDistance);
380 
381  /// calculate common collision pair values
382 
383  rts.selfCollDataVec[i].repulsiveForce =
384  c.maxRepulsiveForce / std::pow(c.distanceThreshold, 2) *
385  std::pow(rts.selfCollDataVec[i].minDistance - c.distanceThreshold, 2);
386  rts.selfCollDataVec[i].localStiffness =
387  -2.0 * c.maxRepulsiveForce / std::pow(c.distanceThreshold, 2) *
388  (rts.selfCollDataVec[i].minDistance - c.distanceThreshold);
389  // minimum distance is always lower than the threshold distance
390  // (local stiffness is always positive)
391  rts.selfCollDataVec[i].repulsiveForce =
392  std::clamp(rts.selfCollDataVec[i].repulsiveForce, 0.0f, c.maxRepulsiveForce);
393  ARMARX_CHECK_NONNEGATIVE(rts.selfCollDataVec[i].repulsiveForce);
394  ARMARX_CHECK_NONNEGATIVE(rts.selfCollDataVec[i].localStiffness);
395 
396  ARMARX_CHECK_EQUAL(3, rts.selfCollDataVec[i].direction.rows());
397  // projected in direction of collision (1xn), is saved transposed (nx1)
398  rts.selfCollDataVec[i].projectedJacT =
399  (rts.selfCollDataVec[i].direction.transpose() * rts.selfCollDataVec[i].jacobian)
400  .transpose();
401 
402  ARMARX_CHECK_EQUAL(1, rts.selfCollDataVec[i].projectedJacT.cols());
403  ARMARX_CHECK_EQUAL(rts.numJoints, rts.selfCollDataVec[i].projectedJacT.rows());
404  ARMARX_CHECK_EQUAL(rts.numJoints, rts.qvelFiltered.rows());
405 
406  rts.selfCollDataVec[i].distanceVelocity =
407  rts.selfCollDataVec[i].projectedJacT.transpose() * rts.qvelFiltered;
408 
409  ARMARX_CHECK_EQUAL(rts.numJoints, rts.inertiaInverse.rows());
410  ARMARX_CHECK_EQUAL(rts.numJoints, rts.inertiaInverse.cols());
411 
412  rts.selfCollDataVec[i].projectedMass =
413  1 / (rts.selfCollDataVec[i].projectedJacT.transpose() * rts.inertiaInverse *
414  rts.selfCollDataVec[i].projectedJacT);
415 
416  ARMARX_CHECK_NONNEGATIVE(rts.selfCollDataVec[i].projectedMass);
417 
418  rts.selfCollDataVec[i].damping =
419  2 * std::sqrt(rts.selfCollDataVec[i].localStiffness) *
420  std::sqrt(rts.selfCollDataVec[i].projectedMass) * c.dampingFactor;
421 
422  ARMARX_CHECK_NONNEGATIVE(rts.selfCollDataVec[i].damping);
423 
424  rts.selfCollisionJointTorque +=
425  rts.selfCollDataVec[i].projectedJacT *
426  (rts.selfCollDataVec[i].repulsiveForce -
427  rts.selfCollDataVec[i].damping * rts.selfCollDataVec[i].distanceVelocity);
428  }
429 
430  /// from here it is not part of the self-collision torque algorithm
431  /// here the contact point information of specific contact points is saved for
432  /// debugging/evaluation purposes
433 
434  /// check for eval pairs
435  if (c.testConfig == 1)
436  {
437  // check for specific pairs
438  if (rts.activeDistPair.node1 == "ArmL8_Wri2" and rts.activeDistPair.node2 == "root")
439  {
440  rts.evalData[0].setDistValues(
441  rts.activeDistPair.node1,
442  rts.activeDistPair.node2,
443  static_cast<float>(rts.activeDistPair.minDistance));
444  if (rts.evalData[0].minDistance < c.distanceThreshold)
445  {
446  rts.evalData[0].setCalcValues(rts.selfCollDataVec[i].jacobian,
447  rts.selfCollDataVec[i].repulsiveForce,
448  rts.selfCollDataVec[i].localStiffness,
449  rts.selfCollDataVec[i].projectedJacT,
450  rts.selfCollDataVec[i].distanceVelocity,
451  rts.selfCollDataVec[i].direction,
452  rts.selfCollDataVec[i].point,
453  rts.selfCollDataVec[i].damping,
454  rts.selfCollDataVec[i].projectedMass,
455  rts.selfCollDataVec[i].desiredNSColl);
456  }
457  }
458  else if (rts.activeDistPair.node2 == "ArmL8_Wri2" and
459  rts.activeDistPair.node1 == "root")
460  {
461  rts.evalData[0].setDistValues(
462  rts.activeDistPair.node2,
463  rts.activeDistPair.node1,
464  static_cast<float>(rts.activeDistPair.minDistance));
465  if (rts.evalData[0].minDistance < c.distanceThreshold)
466  {
467  rts.evalData[0].setCalcValues(rts.selfCollDataVec[i].jacobian,
468  rts.selfCollDataVec[i].repulsiveForce,
469  rts.selfCollDataVec[i].localStiffness,
470  rts.selfCollDataVec[i].projectedJacT,
471  rts.selfCollDataVec[i].distanceVelocity,
472  rts.selfCollDataVec[i].direction,
473  rts.selfCollDataVec[i].point,
474  rts.selfCollDataVec[i].damping,
475  rts.selfCollDataVec[i].projectedMass,
476  rts.selfCollDataVec[i].desiredNSColl);
477  }
478  }
479  if (rts.activeDistPair.node1 == "ArmL8_Wri2" and
480  rts.activeDistPair.node2 == "ArmR8_Wri2")
481  {
482  rts.evalData[1].setDistValues(
483  rts.activeDistPair.node1,
484  rts.activeDistPair.node2,
485  static_cast<float>(rts.activeDistPair.minDistance));
486  if (rts.evalData[1].minDistance < c.distanceThreshold)
487  {
488  rts.evalData[1].setCalcValues(rts.selfCollDataVec[i].jacobian,
489  rts.selfCollDataVec[i].repulsiveForce,
490  rts.selfCollDataVec[i].localStiffness,
491  rts.selfCollDataVec[i].projectedJacT,
492  rts.selfCollDataVec[i].distanceVelocity,
493  rts.selfCollDataVec[i].direction,
494  rts.selfCollDataVec[i].point,
495  rts.selfCollDataVec[i].damping,
496  rts.selfCollDataVec[i].projectedMass,
497  rts.selfCollDataVec[i].desiredNSColl);
498  }
499  }
500  else if (rts.activeDistPair.node2 == "ArmL8_Wri2" and
501  rts.activeDistPair.node1 == "ArmR8_Wri2")
502  {
503  rts.evalData[1].setDistValues(
504  rts.activeDistPair.node2,
505  rts.activeDistPair.node1,
506  static_cast<float>(rts.activeDistPair.minDistance));
507  if (rts.evalData[1].minDistance < c.distanceThreshold)
508  {
509  rts.evalData[1].setCalcValues(rts.selfCollDataVec[i].jacobian,
510  rts.selfCollDataVec[i].repulsiveForce,
511  rts.selfCollDataVec[i].localStiffness,
512  rts.selfCollDataVec[i].projectedJacT,
513  rts.selfCollDataVec[i].distanceVelocity,
514  rts.selfCollDataVec[i].direction,
515  rts.selfCollDataVec[i].point,
516  rts.selfCollDataVec[i].damping,
517  rts.selfCollDataVec[i].projectedMass,
518  rts.selfCollDataVec[i].desiredNSColl);
519  }
520  }
521  if (rts.activeDistPair.node1 == "ArmL7_Wri1" and
522  rts.activeDistPair.node2 == "ArmR8_Wri2")
523  {
524  rts.evalData[2].setDistValues(
525  rts.activeDistPair.node1,
526  rts.activeDistPair.node2,
527  static_cast<float>(rts.activeDistPair.minDistance));
528  if (rts.evalData[2].minDistance < c.distanceThreshold)
529  {
530  rts.evalData[2].setCalcValues(rts.selfCollDataVec[i].jacobian,
531  rts.selfCollDataVec[i].repulsiveForce,
532  rts.selfCollDataVec[i].localStiffness,
533  rts.selfCollDataVec[i].projectedJacT,
534  rts.selfCollDataVec[i].distanceVelocity,
535  rts.selfCollDataVec[i].direction,
536  rts.selfCollDataVec[i].point,
537  rts.selfCollDataVec[i].damping,
538  rts.selfCollDataVec[i].projectedMass,
539  rts.selfCollDataVec[i].desiredNSColl);
540  }
541  }
542  else if (rts.activeDistPair.node2 == "ArmL7_Wri1" and
543  rts.activeDistPair.node1 == "ArmR8_Wri2")
544  {
545  rts.evalData[2].setDistValues(
546  rts.activeDistPair.node2,
547  rts.activeDistPair.node1,
548  static_cast<float>(rts.activeDistPair.minDistance));
549  if (rts.evalData[2].minDistance < c.distanceThreshold)
550  {
551  rts.evalData[2].setCalcValues(rts.selfCollDataVec[i].jacobian,
552  rts.selfCollDataVec[i].repulsiveForce,
553  rts.selfCollDataVec[i].localStiffness,
554  rts.selfCollDataVec[i].projectedJacT,
555  rts.selfCollDataVec[i].distanceVelocity,
556  rts.selfCollDataVec[i].direction,
557  rts.selfCollDataVec[i].point,
558  rts.selfCollDataVec[i].damping,
559  rts.selfCollDataVec[i].projectedMass,
560  rts.selfCollDataVec[i].desiredNSColl);
561  }
562  }
563  if (rts.activeDistPair.node1 == "ArmL8_Wri2" and
564  rts.activeDistPair.node2 == "ArmR7_Wri1")
565  {
566  rts.evalData[3].setDistValues(
567  rts.activeDistPair.node1,
568  rts.activeDistPair.node2,
569  static_cast<float>(rts.activeDistPair.minDistance));
570  if (rts.evalData[3].minDistance < c.distanceThreshold)
571  {
572  rts.evalData[3].setCalcValues(rts.selfCollDataVec[i].jacobian,
573  rts.selfCollDataVec[i].repulsiveForce,
574  rts.selfCollDataVec[i].localStiffness,
575  rts.selfCollDataVec[i].projectedJacT,
576  rts.selfCollDataVec[i].distanceVelocity,
577  rts.selfCollDataVec[i].direction,
578  rts.selfCollDataVec[i].point,
579  rts.selfCollDataVec[i].damping,
580  rts.selfCollDataVec[i].projectedMass,
581  rts.selfCollDataVec[i].desiredNSColl);
582  }
583  }
584  else if (rts.activeDistPair.node2 == "ArmL8_Wri2" and
585  rts.activeDistPair.node1 == "ArmR7_Wri1")
586  {
587  rts.evalData[3].setDistValues(
588  rts.activeDistPair.node2,
589  rts.activeDistPair.node1,
590  static_cast<float>(rts.activeDistPair.minDistance));
591  if (rts.evalData[3].minDistance < c.distanceThreshold)
592  {
593  rts.evalData[3].setCalcValues(rts.selfCollDataVec[i].jacobian,
594  rts.selfCollDataVec[i].repulsiveForce,
595  rts.selfCollDataVec[i].localStiffness,
596  rts.selfCollDataVec[i].projectedJacT,
597  rts.selfCollDataVec[i].distanceVelocity,
598  rts.selfCollDataVec[i].direction,
599  rts.selfCollDataVec[i].point,
600  rts.selfCollDataVec[i].damping,
601  rts.selfCollDataVec[i].projectedMass,
602  rts.selfCollDataVec[i].desiredNSColl);
603  }
604  }
605  if (rts.activeDistPair.node1 == "ArmL8_Wri2" and
606  rts.activeDistPair.node2 == "Neck_2_Pitch")
607  {
608  rts.evalData[4].setDistValues(
609  rts.activeDistPair.node1,
610  rts.activeDistPair.node2,
611  static_cast<float>(rts.activeDistPair.minDistance));
612  if (rts.evalData[4].minDistance < c.distanceThreshold)
613  {
614  rts.evalData[4].setCalcValues(rts.selfCollDataVec[i].jacobian,
615  rts.selfCollDataVec[i].repulsiveForce,
616  rts.selfCollDataVec[i].localStiffness,
617  rts.selfCollDataVec[i].projectedJacT,
618  rts.selfCollDataVec[i].distanceVelocity,
619  rts.selfCollDataVec[i].direction,
620  rts.selfCollDataVec[i].point,
621  rts.selfCollDataVec[i].damping,
622  rts.selfCollDataVec[i].projectedMass,
623  rts.selfCollDataVec[i].desiredNSColl);
624  }
625  }
626  else if (rts.activeDistPair.node2 == "ArmL8_Wri2" and
627  rts.activeDistPair.node1 == "Neck_2_Pitch")
628  {
629  rts.evalData[4].setDistValues(
630  rts.activeDistPair.node2,
631  rts.activeDistPair.node1,
632  static_cast<float>(rts.activeDistPair.minDistance));
633  if (rts.evalData[4].minDistance < c.distanceThreshold)
634  {
635  rts.evalData[4].setCalcValues(rts.selfCollDataVec[i].jacobian,
636  rts.selfCollDataVec[i].repulsiveForce,
637  rts.selfCollDataVec[i].localStiffness,
638  rts.selfCollDataVec[i].projectedJacT,
639  rts.selfCollDataVec[i].distanceVelocity,
640  rts.selfCollDataVec[i].direction,
641  rts.selfCollDataVec[i].point,
642  rts.selfCollDataVec[i].damping,
643  rts.selfCollDataVec[i].projectedMass,
644  rts.selfCollDataVec[i].desiredNSColl);
645  }
646  }
647  if (rts.activeDistPair.node1 == "ArmL8_Wri2" and
648  rts.activeDistPair.node2 == "TorsoJoint")
649  {
650  rts.evalData[5].setDistValues(
651  rts.activeDistPair.node1,
652  rts.activeDistPair.node2,
653  static_cast<float>(rts.activeDistPair.minDistance));
654  if (rts.evalData[5].minDistance < c.distanceThreshold)
655  {
656  rts.evalData[5].setCalcValues(rts.selfCollDataVec[i].jacobian,
657  rts.selfCollDataVec[i].repulsiveForce,
658  rts.selfCollDataVec[i].localStiffness,
659  rts.selfCollDataVec[i].projectedJacT,
660  rts.selfCollDataVec[i].distanceVelocity,
661  rts.selfCollDataVec[i].direction,
662  rts.selfCollDataVec[i].point,
663  rts.selfCollDataVec[i].damping,
664  rts.selfCollDataVec[i].projectedMass,
665  rts.selfCollDataVec[i].desiredNSColl);
666  }
667  }
668  else if (rts.activeDistPair.node2 == "ArmL8_Wri2" and
669  rts.activeDistPair.node1 == "TorsoJoint")
670  {
671  rts.evalData[5].setDistValues(
672  rts.activeDistPair.node2,
673  rts.activeDistPair.node1,
674  static_cast<float>(rts.activeDistPair.minDistance));
675  if (rts.evalData[5].minDistance < c.distanceThreshold)
676  {
677  rts.evalData[5].setCalcValues(rts.selfCollDataVec[i].jacobian,
678  rts.selfCollDataVec[i].repulsiveForce,
679  rts.selfCollDataVec[i].localStiffness,
680  rts.selfCollDataVec[i].projectedJacT,
681  rts.selfCollDataVec[i].distanceVelocity,
682  rts.selfCollDataVec[i].direction,
683  rts.selfCollDataVec[i].point,
684  rts.selfCollDataVec[i].damping,
685  rts.selfCollDataVec[i].projectedMass,
686  rts.selfCollDataVec[i].desiredNSColl);
687  }
688  }
689  if (rts.activeDistPair.node1 == "ArmL5_Elb1" and
690  rts.activeDistPair.node2 == "TorsoJoint")
691  {
692  rts.evalData[6].setDistValues(
693  rts.activeDistPair.node1,
694  rts.activeDistPair.node2,
695  static_cast<float>(rts.activeDistPair.minDistance));
696  if (rts.evalData[6].minDistance < c.distanceThreshold)
697  {
698  rts.evalData[6].setCalcValues(rts.selfCollDataVec[i].jacobian,
699  rts.selfCollDataVec[i].repulsiveForce,
700  rts.selfCollDataVec[i].localStiffness,
701  rts.selfCollDataVec[i].projectedJacT,
702  rts.selfCollDataVec[i].distanceVelocity,
703  rts.selfCollDataVec[i].direction,
704  rts.selfCollDataVec[i].point,
705  rts.selfCollDataVec[i].damping,
706  rts.selfCollDataVec[i].projectedMass,
707  rts.selfCollDataVec[i].desiredNSColl);
708  }
709  }
710  else if (rts.activeDistPair.node2 == "ArmL5_Elb1" and
711  rts.activeDistPair.node1 == "TorsoJoint")
712  {
713  rts.evalData[6].setDistValues(
714  rts.activeDistPair.node2,
715  rts.activeDistPair.node1,
716  static_cast<float>(rts.activeDistPair.minDistance));
717  if (rts.evalData[6].minDistance < c.distanceThreshold)
718  {
719  rts.evalData[6].setCalcValues(rts.selfCollDataVec[i].jacobian,
720  rts.selfCollDataVec[i].repulsiveForce,
721  rts.selfCollDataVec[i].localStiffness,
722  rts.selfCollDataVec[i].projectedJacT,
723  rts.selfCollDataVec[i].distanceVelocity,
724  rts.selfCollDataVec[i].direction,
725  rts.selfCollDataVec[i].point,
726  rts.selfCollDataVec[i].damping,
727  rts.selfCollDataVec[i].projectedMass,
728  rts.selfCollDataVec[i].desiredNSColl);
729  }
730  }
731  }
732  }
733  }
734 
735  void
736  SafetyTaskspaceImpedanceController::calculateJointLimitTorque(Config& c, RtStatus& rts)
737  {
738  /// method:
739  ///
740  /// computes the joint torques to avoid joint limits -> jointLimitJointTorque
741 
742  rts.jointLimitJointTorque.setZero();
743  ARMARX_CHECK_EQUAL(rts.numJoints, rts.jointLimitJointTorque.rows());
744 
745  ARMARX_CHECK_EQUAL(rts.numJoints, rts.jointLimitData.size());
746  ARMARX_CHECK_EQUAL(rts.numJoints, rts.inertia.cols());
747  ARMARX_CHECK_EQUAL(rts.numJoints, rts.inertia.rows());
748 
749  /// iterate over all joints and check the joint positions for not limiteless joints
750  for (size_t i = 0; i < rts.jointLimitData.size(); ++i)
751  {
752  if (rts.jointLimitData[i].isLimitless)
753  continue;
754 
755  auto& joint = rts.jointLimitData[i];
756  rts.jointInertia = rts.inertia(i, i);
757  if (rts.qpos[i] < rts.jointLimitData[i].qposThresholdLow)
758  {
759  joint.repulsiveTorque =
760  c.maxRepulsiveTorque / std::pow(rts.jointLimitData[i].thresholdRange, 2) *
761  std::pow(rts.jointLimitData[i].qposThresholdLow - rts.qpos[i], 2);
762  ARMARX_CHECK_NONNEGATIVE(joint.repulsiveTorque);
763  joint.repulsiveTorque =
764  std::clamp(joint.repulsiveTorque, 0.0f, c.maxRepulsiveTorque);
765 
766  rts.localStiffnessJointLim = 2 * c.maxRepulsiveTorque /
767  std::pow(rts.jointLimitData[i].thresholdRange, 2) *
768  (rts.jointLimitData[i].qposThresholdLow - rts.qpos[i]);
769 
770  ARMARX_CHECK_NONNEGATIVE(rts.localStiffnessJointLim);
771  rts.dampingJointLim = 2 * std::sqrt(rts.jointInertia) *
772  std::sqrt(rts.localStiffnessJointLim) * c.dampingFactor;
773 
774  ARMARX_CHECK_NONNEGATIVE(rts.dampingJointLim);
775  joint.totalDamping = rts.dampingJointLim * rts.qvelFiltered[i];
776  rts.jointLimitJointTorque(i) =
777  joint.repulsiveTorque - rts.dampingJointLim * rts.qvelFiltered[i];
778 
779  ARMARX_TRACE_LITE;
780  }
781  else if (rts.qpos[i] > rts.jointLimitData[i].qposThresholdHigh)
782  {
783  joint.repulsiveTorque =
784  c.maxRepulsiveTorque / std::pow(rts.jointLimitData[i].thresholdRange, 2) *
785  std::pow(rts.qpos[i] - rts.jointLimitData[i].qposThresholdHigh, 2);
786 
787  ARMARX_CHECK_NONNEGATIVE(joint.repulsiveTorque);
788  joint.repulsiveTorque =
789  std::clamp(joint.repulsiveTorque, 0.0f, c.maxRepulsiveTorque);
790 
791  rts.localStiffnessJointLim =
792  2 * c.maxRepulsiveTorque / std::pow(rts.jointLimitData[i].thresholdRange, 2) *
793  (rts.qpos[i] - rts.jointLimitData[i].qposThresholdHigh);
794  ARMARX_TRACE_LITE;
795  ARMARX_CHECK_NONNEGATIVE(rts.localStiffnessJointLim);
796  rts.dampingJointLim = 2 * std::sqrt(rts.jointInertia) *
797  std::sqrt(rts.localStiffnessJointLim) * c.dampingFactor;
798  ARMARX_CHECK_NONNEGATIVE(rts.dampingJointLim);
799  joint.totalDamping = rts.dampingJointLim * rts.qvelFiltered[i];
800  rts.jointLimitJointTorque(i) =
801  -(joint.repulsiveTorque + rts.dampingJointLim * rts.qvelFiltered[i]);
802  }
803  }
804  }
805 
806  /// caclulate null spaces
807  void
808  SafetyTaskspaceImpedanceController::calculateSelfCollisionNullspace(Config& c, RtStatus& rts)
809  {
810  rts.selfCollNullSpace.setIdentity();
811 
812  ARMARX_CHECK_EQUAL(rts.numJoints, rts.selfCollNullSpace.rows());
813  ARMARX_CHECK_EQUAL(rts.numJoints, rts.selfCollNullSpace.cols());
814 
815  ARMARX_CHECK_EQUAL(4, rts.selfCollisionNullSpaceWeights.rows());
816  rts.k1 = rts.selfCollisionNullSpaceWeights(0);
817  rts.k2 = rts.selfCollisionNullSpaceWeights(1);
818  rts.k3 = rts.selfCollisionNullSpaceWeights(2);
819  rts.k4 = rts.selfCollisionNullSpaceWeights(3);
820 
821  ARMARX_TRACE_LITE;
822 
823  for (auto& active : rts.selfCollDataVec)
824  {
825  /// get desired null space value based on proximity to collision
826  if (active.minDistance < c.selfCollActivatorZ1)
827  {
828  /// full locking in direction of collision
829  rts.desiredNullSpace = 0.0f;
830  }
831  else if (c.selfCollActivatorZ1 <= active.minDistance &&
832  active.minDistance <= c.selfCollActivatorZ2)
833  {
834  /// in transition state
835  rts.desiredNullSpace = rts.k1 * std::pow(active.minDistance, 3) +
836  rts.k2 * std::pow(active.minDistance, 2) +
837  rts.k3 * active.minDistance + rts.k4;
838  if (rts.desiredNullSpace > 1.0f)
839  {
840  ARMARX_WARNING
841  << "desired null space value should not be higher than 1.0, was "
842  << rts.desiredNullSpace
843  << ", in self-collision null space calulation, weights: " << rts.k1 << ", "
844  << rts.k2 << ", " << rts.k3 << ", " << rts.k4;
845  }
846  }
847  else
848  {
849  /// unrestricted movement in direction of collision
850  rts.desiredNullSpace = 1.0f;
851  }
852 
853 
854  active.desiredNSColl = rts.desiredNullSpace;
855  rts.desiredNullSpace = std::clamp(rts.desiredNullSpace, 0.0f, 1.0f);
856  ARMARX_CHECK_NONNEGATIVE(rts.desiredNullSpace);
857  /// normalize projected Jacobian
858  rts.normalizedJacT = active.projectedJacT.normalized();
859 
860  ARMARX_CHECK_EQUAL(rts.numJoints, rts.normalizedJacT.rows());
861  ARMARX_CHECK_EQUAL(rts.numJoints, I.rows());
862  ARMARX_CHECK_EQUAL(rts.numJoints, I.cols());
863 
864  rts.tempNullSpaceMatrix = I - (rts.normalizedJacT * (1.0f - rts.desiredNullSpace) *
865  rts.normalizedJacT.transpose());
866 
867  if (c.onlyLimitCollDirection)
868  {
869  /// check, whether impedance joint torque acts against the collision direction
870  ARMARX_CHECK_EQUAL(active.projectedJacT.rows(), rts.impedanceJointTorque.rows());
871  for (int i = 0; i < rts.impedanceJointTorque.rows(); ++i)
872  {
873  if ((active.projectedJacT(i) < 0.0f and rts.impedanceJointTorque(i) < 0.0f) ||
874  (active.projectedJacT(i) > 0.0f and rts.impedanceJointTorque(i) > 0.0f))
875  {
876  // if both have the same sign (both negative or both positive), do not limit DoF
877  rts.tempNullSpaceMatrix(i, i) = 1.0f;
878  }
879  }
880  }
881 
882  /// project desired null space in corresponding direction via the Norm-Jacobian
883  rts.selfCollNullSpace *= rts.tempNullSpaceMatrix;
884  // todo: clamp the diagonal values between 0 and 1
885 
886  // for debugging/testdata
887  if (c.testConfig == 1)
888  {
889  if (active.nodeName == "ArmL8_Wri2")
890  {
891  rts.evalData[0].desiredNSColl = active.desiredNSColl;
892  rts.evalData[1].desiredNSColl = active.desiredNSColl;
893  rts.evalData[3].desiredNSColl = active.desiredNSColl;
894  rts.evalData[4].desiredNSColl = active.desiredNSColl;
895  }
896  else if (active.nodeName == "ArmL7_Wri1")
897  {
898  rts.evalData[2].desiredNSColl = active.desiredNSColl;
899  }
900  }
901  }
902  }
903 
904  void
905  SafetyTaskspaceImpedanceController::calculateJointLimitNullspace(Config& c, RtStatus& rts)
906  {
907  /// check fundamentals thesis BA Jan Fenker
908  rts.jointLimNullSpace.setIdentity();
909 
910  ARMARX_CHECK_EQUAL(rts.numJoints, rts.jointLimNullSpace.rows());
911  ARMARX_CHECK_EQUAL(rts.numJoints, rts.jointLimNullSpace.cols());
912 
913  ARMARX_CHECK_EQUAL(rts.numJoints, rts.jointLimitData.size());
914 
915  for (size_t i = 0; i < rts.jointLimitData.size(); ++i)
916  {
917  auto& joint = rts.jointLimitData[i];
918  ARMARX_CHECK_EQUAL(4, joint.jointLimitNullSpaceWeightsLow.rows());
919  ARMARX_CHECK_EQUAL(4, joint.jointLimitNullSpaceWeightsHigh.rows());
920  rts.k1Lo = joint.jointLimitNullSpaceWeightsLow(0);
921  rts.k2Lo = joint.jointLimitNullSpaceWeightsLow(1);
922  rts.k3Lo = joint.jointLimitNullSpaceWeightsLow(2);
923  rts.k4Lo = joint.jointLimitNullSpaceWeightsLow(3);
924  rts.k1Hi = joint.jointLimitNullSpaceWeightsHigh(0);
925  rts.k2Hi = joint.jointLimitNullSpaceWeightsHigh(1);
926  rts.k3Hi = joint.jointLimitNullSpaceWeightsHigh(2);
927  rts.k4Hi = joint.jointLimitNullSpaceWeightsHigh(3);
928 
929  ARMARX_TRACE_LITE;
930 
931  if (not joint.isLimitless)
932  {
933  /// get desired null space value based on proximity to joint boundaries
934  if (rts.qpos[i] < joint.qposZ1Low)
935  {
936  /// full locking
937  rts.desiredNullSpace = 0.0f;
938  }
939  else if (joint.qposZ1Low <= rts.qpos[i] && rts.qpos[i] <= joint.qposZ2Low)
940  {
941  /// in transition state
942  rts.desiredNullSpace = rts.k1Lo * std::pow(rts.qpos[i], 3) +
943  rts.k2Lo * std::pow(rts.qpos[i], 2) +
944  rts.k3Lo * rts.qpos[i] + rts.k4Lo;
945  if (rts.desiredNullSpace > 1.0f)
946  {
947  ARMARX_WARNING
948  << "desired null space value should not be higher than 1.0, was "
949  << rts.desiredNullSpace
950  << ", approaching lower joint limit, weights: " << rts.k1Lo << ", "
951  << rts.k2Lo << ", " << rts.k3Lo << ", " << rts.k4Lo;
952  }
953  }
954  else if (joint.qposZ2High <= rts.qpos[i] && rts.qpos[i] <= joint.qposZ1High)
955  {
956  /// in transition state
957  rts.desiredNullSpace = rts.k1Hi * std::pow(rts.qpos[i], 3) +
958  rts.k2Hi * std::pow(rts.qpos[i], 2) +
959  rts.k3Hi * rts.qpos[i] + rts.k4Hi;
960  if (rts.desiredNullSpace > 1.0f)
961  {
962  ARMARX_WARNING
963  << "desired null space value should not be higher than 1.0, was "
964  << rts.desiredNullSpace
965  << ", approaching upper joint limit, weights: " << rts.k1Hi << ", "
966  << rts.k2Hi << ", " << rts.k3Hi << ", " << rts.k4Hi;
967  }
968  }
969  else if (joint.qposZ1High < rts.qpos[i])
970  {
971  /// full locking
972  rts.desiredNullSpace = 0.0f;
973  }
974  else
975  {
976  /// unrestricted movement
977  rts.desiredNullSpace = 1.0f;
978  }
979 
980 
981  joint.desiredNSjointLim = rts.desiredNullSpace;
982  rts.desiredNullSpace = std::clamp(rts.desiredNullSpace, 0.0f, 1.0f);
983  ARMARX_CHECK_NONNEGATIVE(rts.desiredNullSpace);
984 
985  /// set desired null space value in null space matrix
986  rts.jointLimNullSpace(i, i) = 1.0f - (1.0f - rts.desiredNullSpace);
987  }
988  }
989  }
990 
991  /// helpers
992 
993  Eigen::Vector4f
994  SafetyTaskspaceImpedanceController::calculateTransitionWeights(float z1, float z2)
995  {
996 
997  /// Define the constraint matrix for the null space transition function
998  ///
999  /// in transition state (state between full locking and unrestricted movement for a contin-
1000  /// uous control law) a third-order polynomial with four weight values is required
1001  /// In this method the weights are calculated in the preactivate method based on the set
1002  /// threshold parameters z1 and z2
1003  ///
1004  /// z1: state of full locking starts with this value
1005  /// z2: state of unrestricted movement starts with this value
1006  Eigen::Matrix4f constraintMatrix;
1007  constraintMatrix << std::pow(z1, 3), std::pow(z1, 2), z1, 1, std::pow(z2, 3),
1008  std::pow(z2, 2), z2, 1, 3 * std::pow(z1, 2), 2 * z1, 1, 0, 3 * std::pow(z2, 2), 2 * z2,
1009  1, 0;
1010 
1011  /// Define the constraint result values for the null space transition function
1012  Eigen::Vector4f b;
1013  b << 0, 1, 0, 0;
1014 
1015  /// weights for continuous task transition
1016  return constraintMatrix.colPivHouseholderQr().solve(b);
1017  }
1018 
1019  /// --------------------------------- main rt-loop ------------------------------------------
1020 
1021  void
1022  SafetyTaskspaceImpedanceController::run(
1023  Config& c,
1024  RtStatus& rtStatus,
1025  std::shared_ptr<std::vector<::simox::control::environment::DistanceResult>>& collisionPairs,
1026  std::shared_ptr<std::vector<int>>& pointsOnArm,
1027  int pointsOnArmIndex,
1028  std::shared_ptr<simox::control::geodesics::metric::Inertia>& inertiaInstance,
1029  std::vector<std::string>& selfCollisionNodes)
1030  {
1031  ARMARX_CHECK_NOT_NULL(collisionPairs);
1032  ARMARX_CHECK_NOT_NULL(pointsOnArm);
1033  ARMARX_CHECK_NOT_NULL(inertiaInstance);
1034 
1035  /// run in rt thread
1036  rtStatus.currentPose = rtTCP->getPoseInRootFrame();
1037  /// this should not be moved to non rt, just in case non rt thread dies, rt still functions as safely
1038  if (not ftsensor.isSafe(c.ftConfig))
1039  {
1040  ARMARX_INFO << "FTGuard indicates RT not safe";
1041  c.desiredPose = rtStatus.desiredPose;
1042  rtStatus.rtSafe = false;
1043  }
1044  else if ((rtStatus.currentPose.block<3, 1>(0, 3) - c.desiredPose.block<3, 1>(0, 3)).norm() >
1045  c.safeDistanceMMToGoal)
1046  {
1047  c.desiredPose = rtStatus.desiredPose;
1048  rtStatus.rtSafe = false;
1049  }
1050  else
1051  {
1052  rtStatus.desiredPose = c.desiredPose;
1053  rtStatus.rtSafe = true;
1054  }
1055 
1056  /// ------------------------------ compute jacobi -------------------------------------------------------------
1057  rtStatus.jacobi =
1058  ik->getJacobianMatrix(rtTCP, VirtualRobot::IKSolver::CartesianSelection::All);
1059  rtStatus.jacobi.block(0, 0, 3, rtStatus.numJoints) =
1060  0.001 * rtStatus.jacobi.block(0, 0, 3, rtStatus.numJoints);
1061  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jacobi.cols());
1062  ARMARX_CHECK_EQUAL(6, rtStatus.jacobi.rows());
1063 
1064  rtStatus.jtpinv =
1065  ik->computePseudoInverseJacobianMatrix(rtStatus.jacobi.transpose(), lambda);
1066  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jtpinv.cols());
1067  ARMARX_CHECK_EQUAL(6, rtStatus.jtpinv.rows());
1068 
1069  /// ------------------------------ update velocity/twist ------------------------------------------------------
1070  for (size_t i = 0; i < rtStatus.jointVelocity.size(); ++i)
1071  {
1072  rtStatus.qpos(i) = rtStatus.jointPosition[i];
1073  rtStatus.qvel(i) = rtStatus.jointVelocity[i];
1074  }
1075  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.qpos.rows());
1076  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.qvel.rows());
1077  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.qvelFiltered.rows());
1078 
1079  rtStatus.qvelFiltered =
1080  (1 - c.qvelFilter) * rtStatus.qvelFiltered + c.qvelFilter * rtStatus.qvel;
1081  rtStatus.currentTwist = rtStatus.jacobi * rtStatus.qvelFiltered;
1082 
1083  /// ------------------------------------- Impedance control ---------------------------------------------------
1084  /// calculate pose error between target pose and current pose
1085  /// !!! This is very important: you have to keep postion and orientation both
1086  /// with UI unit (meter, radian) to calculate impedance force.
1087 
1088  rtStatus.poseDiffMatImp = rtStatus.desiredPose.block<3, 3>(0, 0) *
1089  rtStatus.currentPose.block<3, 3>(0, 0).transpose();
1090  rtStatus.poseErrorImp.head(3) = 0.001 * (rtStatus.desiredPose.block<3, 1>(0, 3) -
1091  rtStatus.currentPose.block<3, 1>(0, 3));
1092  rtStatus.trackingError = rtStatus.poseErrorImp.head(3).norm();
1093  rtStatus.poseErrorImp.tail(3) =
1094  VirtualRobot::MathTools::eigen3f2rpy(rtStatus.poseDiffMatImp);
1095  if (c.splitImpedanceForce)
1096  {
1097  rtStatus.forceImpedance = c.kpImpedance.cwiseProduct(rtStatus.poseErrorImp);
1098  rtStatus.kdForceImpedance = -1.0f * c.kdImpedance.cwiseProduct(rtStatus.currentTwist);
1099  }
1100  else
1101  {
1102  rtStatus.forceImpedance = c.kpImpedance.cwiseProduct(rtStatus.poseErrorImp) -
1103  c.kdImpedance.cwiseProduct(rtStatus.currentTwist);
1104  }
1105 
1106 
1107  /// ----------------------------- Nullspace PD Control --------------------------------------------------
1108  ARMARX_CHECK_EQUAL(rtStatus.numJoints, c.kpNullspace.rows());
1109  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.nullspaceTorque.rows());
1110  ARMARX_CHECK_EQUAL(rtStatus.numJoints, c.desiredNullspaceJointAngles.value().size());
1111  rtStatus.nullspaceTorque =
1112  c.kpNullspace.cwiseProduct(c.desiredNullspaceJointAngles.value() - rtStatus.qpos) -
1113  c.kdNullspace.cwiseProduct(rtStatus.qvelFiltered);
1114 
1115  /// ----------------------------- Map TS target force to JS --------------------------------------------------
1116  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.desiredJointTorques.rows());
1117  rtStatus.impedanceJointTorque =
1118  rtStatus.jacobi.transpose() * rtStatus.forceImpedance +
1119  (I - rtStatus.jacobi.transpose() * rtStatus.jtpinv) * rtStatus.nullspaceTorque;
1120  if (c.splitImpedanceForce)
1121  {
1122  rtStatus.kdImpedanceTorque = rtStatus.jacobi.transpose() * rtStatus.kdForceImpedance;
1123  }
1124  /// ----------------------------- Joint Space Damping --------------------------------------------------
1125  // possibility to add additional taskspace damping for zero torque operations
1126  rtStatus.jointDampingTorque = -c.kdJointSpace.cwiseProduct(rtStatus.qvelFiltered);
1127 
1128  /// ----------------------------- inertia calculations --------------------------------------------------
1129  Eigen::VectorXd qposXd = rtStatus.qpos.cast<double>();
1130  rtStatus.inertia = inertiaInstance->compute_metric(rtStatus.qpos.cast<double>(), false)
1131  .metric.cast<float>();
1132  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.inertia.rows());
1133  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.inertia.cols());
1134 
1135  rtStatus.inertiaInverse = rtStatus.inertia.inverse();
1136  /// ----------------------------- safety constraints --------------------------------------------------
1137  if (c.enableSelfCollisionAvoidance)
1138  {
1139  // calculation of self-collision avoidance torque
1140  double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
1141  calculateSelfCollisionTorque(
1142  c, rtStatus, collisionPairs, pointsOnArm, pointsOnArmIndex, selfCollisionNodes);
1143  rtStatus.collisionTorqueTime =
1144  IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
1145  }
1146  if (c.enableJointLimitAvoidance)
1147  {
1148  // calculation of joint limit avoidance torque
1149  double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
1150  calculateJointLimitTorque(c, rtStatus);
1151  rtStatus.jointLimitTorqueTime =
1152  IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
1153  }
1154  if (!c.samePriority)
1155  {
1156  if (c.enableSelfCollisionAvoidance)
1157  {
1158  // calculation of null space matrix for self-collision avoidance
1159  double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
1160  calculateSelfCollisionNullspace(c, rtStatus);
1161  rtStatus.selfCollNullspaceTime =
1162  IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
1163  }
1164  if (c.enableJointLimitAvoidance)
1165  {
1166  // calculation of null space matrix for joint limit avoidance
1167  double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
1168  calculateJointLimitNullspace(c, rtStatus);
1169  rtStatus.jointLimitNullspaceTime =
1170  IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
1171  }
1172  }
1173  ARMARX_TRACE_LITE;
1174 
1175  /// --------------------------- apply EMA low pass filter -----------------------------------------------
1176  if (c.filterSafetyValues)
1177  {
1178  /// filter safety constraint values using EMA low pass filter
1179  for (int i = 0; i < rtStatus.selfCollisionJointTorque.size(); ++i)
1180  {
1181  rtStatus.selfCollisionTorqueFiltered(i) =
1182  (1 - c.safetyValFilter) * rtStatus.selfCollisionTorqueFiltered(i) +
1183  c.safetyValFilter * rtStatus.selfCollisionJointTorque(i);
1184  }
1185  for (int i = 0; i < rtStatus.jointLimitJointTorque.size(); ++i)
1186  {
1187  rtStatus.jointLimitTorqueFiltered(i) =
1188  (1 - c.safetyValFilter) * rtStatus.jointLimitTorqueFiltered(i) +
1189  c.safetyValFilter * rtStatus.jointLimitJointTorque(i);
1190  }
1191 
1192  for (int i = 0; i < rtStatus.selfCollNullSpace.diagonalSize(); ++i)
1193  {
1194  // the computed values are only on the diagonal, so only these need to be filtered
1195  rtStatus.selfCollNullSpaceFiltered(i, i) =
1196  (1 - c.safetyValFilter) * rtStatus.selfCollNullSpaceFiltered(i, i) +
1197  c.safetyValFilter * rtStatus.selfCollNullSpace(i, i);
1198  }
1199  for (int i = 0; i < rtStatus.jointLimNullSpace.diagonalSize(); ++i)
1200  {
1201  // the computed values are only on the diagonal, so only these need to be filtered
1202  rtStatus.jointLimNullSpaceFiltered(i, i) =
1203  (1 - c.safetyValFilter) * rtStatus.jointLimNullSpaceFiltered(i, i) +
1204  c.safetyValFilter * rtStatus.jointLimNullSpace(i, i);
1205  }
1206  /// assign filtered values
1207  rtStatus.selfCollisionJointTorque = rtStatus.selfCollisionTorqueFiltered;
1208  rtStatus.jointLimitJointTorque = rtStatus.jointLimitTorqueFiltered;
1209  rtStatus.selfCollNullSpace = rtStatus.selfCollNullSpaceFiltered;
1210  rtStatus.jointLimNullSpace = rtStatus.jointLimNullSpaceFiltered;
1211  }
1212 
1213  ARMARX_TRACE_LITE;
1214  /// ----------------------------- hierarchical control --------------------------------------------------
1215  /// The following control hierarchies are considered:
1216  ///
1217  /// three tasks in hierarchy: "enableSelfCollisionAvoidance": true, "enableJointLimitAvoidance": true
1218  ///
1219  /// flag: "samePriority": true
1220  /// same priority: torque_coll + torque_jl + torque_imp
1221  ///
1222  /// flag: "topPrioritySelfCollisionAvoidance": true, "samePriority": false
1223  /// self-coll avoidance on top: torque_coll -> torque_jl -> torque_imp
1224  ///
1225  /// flag: "topPriorityJointLimitAvoidance": true, "samePriority": false
1226  /// joint limit avoidance on top: torque_jl -> torque_coll -> torque_imp
1227  ///
1228  /// flag: "topPrioritySelfCollisionAvoidance": false, "topPriorityJointLimitAvoidance": false,
1229  /// "samePriority": false
1230  /// (torque_coll + torque_jl) -> torque_imp
1231  /// self-coll and joint limit avoidance on same priority level:
1232  ///
1233  /// two tasks in hierarchy:
1234  ///
1235  /// "enableSelfCollisionAvoidance": true, "enableJointLimitAvoidance": false
1236  /// self-coll avoidance on top: torque_coll -> torque_imp
1237  ///
1238  /// "enableSelfCollisionAvoidance": false, "enableJointLimitAvoidance": true
1239  /// joint limit avoidance on top: torque_jl -> torque_imp
1240  ///
1241  /// one task in hierarchy (impedance control):
1242  ///
1243  /// "enableSelfCollisionAvoidance": false, "enableJointLimitAvoidance": false
1244  /// only impedance control: torque_imp
1245 
1246 
1247  if (c.samePriority)
1248  {
1249  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.selfCollisionJointTorque.rows());
1250  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jointLimitJointTorque.rows());
1251  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.impedanceJointTorque.rows());
1252  rtStatus.desiredJointTorques =
1253  rtStatus.selfCollisionJointTorque + rtStatus.jointLimitJointTorque +
1254  rtStatus.impedanceJointTorque + rtStatus.kdImpedanceTorque;
1255  ARMARX_TRACE_LITE;
1256  }
1257  else if (c.enableSelfCollisionAvoidance and c.enableJointLimitAvoidance)
1258  {
1259  /// three tasks in hierarchy considered
1260 
1261  if (c.topPrioritySelfCollisionAvoidance)
1262  {
1263 
1264  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.selfCollNullSpace.rows());
1265  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.selfCollNullSpace.cols());
1266  ARMARX_CHECK_EQUAL(rtStatus.impedanceJointTorque.rows(),
1267  rtStatus.selfCollNullSpace.cols());
1268  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jointLimNullSpace.rows());
1269  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jointLimNullSpace.cols());
1270  ARMARX_CHECK_EQUAL(rtStatus.jointLimitJointTorque.rows(),
1271  rtStatus.selfCollNullSpace.cols());
1272 
1273  rtStatus.desiredJointTorques =
1274  rtStatus.selfCollisionJointTorque +
1275  rtStatus.selfCollNullSpace *
1276  (rtStatus.jointLimitJointTorque +
1277  rtStatus.jointLimNullSpace * rtStatus.impedanceJointTorque) +
1278  rtStatus.kdImpedanceTorque;
1279 
1280  // debug values
1281  rtStatus.projJointLimJointTorque =
1282  rtStatus.selfCollNullSpace * rtStatus.jointLimitJointTorque;
1283  rtStatus.projImpedanceJointTorque =
1284  rtStatus.selfCollNullSpace *
1285  (rtStatus.jointLimNullSpace * rtStatus.impedanceJointTorque);
1286  }
1287  else if (c.topPriorityJointLimitAvoidance)
1288  {
1289  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.selfCollNullSpace.rows());
1290  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.selfCollNullSpace.cols());
1291  ARMARX_CHECK_EQUAL(rtStatus.impedanceJointTorque.rows(),
1292  rtStatus.selfCollNullSpace.cols());
1293  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jointLimNullSpace.rows());
1294  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jointLimNullSpace.cols());
1295  ARMARX_CHECK_EQUAL(rtStatus.selfCollisionJointTorque.rows(),
1296  rtStatus.jointLimNullSpace.cols());
1297 
1298  rtStatus.desiredJointTorques =
1299  rtStatus.jointLimitJointTorque +
1300  rtStatus.jointLimNullSpace *
1301  (rtStatus.selfCollisionJointTorque +
1302  rtStatus.selfCollNullSpace * rtStatus.impedanceJointTorque) +
1303  rtStatus.kdImpedanceTorque;
1304 
1305  // debug values
1306  rtStatus.projSelfCollJointTorque =
1307  rtStatus.jointLimNullSpace * rtStatus.selfCollisionJointTorque;
1308  rtStatus.projImpedanceJointTorque =
1309  rtStatus.jointLimNullSpace *
1310  (rtStatus.selfCollNullSpace * rtStatus.impedanceJointTorque);
1311  }
1312  else
1313  {
1314  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.selfCollNullSpace.rows());
1315  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.selfCollNullSpace.cols());
1316  ARMARX_CHECK_EQUAL(rtStatus.impedanceJointTorque.rows(),
1317  rtStatus.selfCollNullSpace.cols());
1318  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jointLimNullSpace.rows());
1319  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jointLimNullSpace.cols());
1320  ARMARX_CHECK_EQUAL(rtStatus.selfCollisionJointTorque.rows(),
1321  rtStatus.jointLimNullSpace.cols());
1322  rtStatus.desiredJointTorques =
1323  rtStatus.jointLimitJointTorque + rtStatus.selfCollisionJointTorque +
1324  (rtStatus.jointLimNullSpace * rtStatus.selfCollNullSpace) *
1325  rtStatus.impedanceJointTorque +
1326  rtStatus.kdImpedanceTorque;
1327 
1328  // debug values
1329  rtStatus.projImpedanceJointTorque =
1330  (rtStatus.jointLimNullSpace * rtStatus.selfCollNullSpace) *
1331  rtStatus.impedanceJointTorque;
1332  }
1333  }
1334  else if (c.enableSelfCollisionAvoidance)
1335  {
1336  /// impedance control and self-collision avoidance
1337 
1338  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.selfCollNullSpace.rows());
1339  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.selfCollNullSpace.cols());
1340  ARMARX_CHECK_EQUAL(rtStatus.impedanceJointTorque.rows(),
1341  rtStatus.selfCollNullSpace.cols());
1342 
1343  rtStatus.desiredJointTorques =
1344  rtStatus.selfCollisionJointTorque +
1345  rtStatus.selfCollNullSpace * rtStatus.impedanceJointTorque +
1346  rtStatus.kdImpedanceTorque;
1347 
1348  // debugging
1349  rtStatus.projImpedanceJointTorque =
1350  rtStatus.selfCollNullSpace * rtStatus.impedanceJointTorque;
1351  }
1352  else if (c.enableJointLimitAvoidance)
1353  {
1354  /// impedance control and joint limit avoidance
1355 
1356  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jointLimNullSpace.rows());
1357  ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jointLimNullSpace.cols());
1358  ARMARX_CHECK_EQUAL(rtStatus.impedanceJointTorque.rows(),
1359  rtStatus.jointLimNullSpace.cols());
1360 
1361  rtStatus.desiredJointTorques =
1362  rtStatus.jointLimitJointTorque +
1363  rtStatus.jointLimNullSpace * rtStatus.impedanceJointTorque +
1364  rtStatus.kdImpedanceTorque;
1365 
1366  // debugging
1367  rtStatus.projImpedanceJointTorque =
1368  rtStatus.jointLimNullSpace * rtStatus.impedanceJointTorque;
1369  }
1370  else
1371  {
1372  /// plain impedance control
1373  rtStatus.desiredJointTorques =
1374  rtStatus.impedanceJointTorque + rtStatus.kdImpedanceTorque;
1375  }
1376 
1377  rtStatus.desiredJointTorques += rtStatus.jointDampingTorque;
1378 
1379  ARMARX_TRACE_LITE;
1380 
1381  /// ------------------------- write impedance forces to buffer -----------------------------------------
1382  // this section is purely for visualization purposes in the viewer
1383  rtStatus.dirErrorImp = rtStatus.poseErrorImp.head(3).normalized();
1384  rtStatus.totalForceImpedance = rtStatus.forceImpedance.head(3).norm();
1385  rtStatus.projForceImpedance = rtStatus.jtpinv * rtStatus.projImpedanceJointTorque;
1386  rtStatus.projTotalForceImpedance = rtStatus.projForceImpedance.head(3).norm();
1387  rtStatus.impForceRatio = rtStatus.projTotalForceImpedance / rtStatus.totalForceImpedance;
1388  rtStatus.impTorqueRatio =
1389  rtStatus.projImpedanceJointTorque.norm() / rtStatus.impedanceJointTorque.norm();
1390 
1391 
1392  /// ----------------------------- write torque target --------------------------------------------------
1393  for (int i = 0; i < rtStatus.desiredJointTorques.rows(); ++i)
1394  {
1395  rtStatus.desiredJointTorques(i) =
1396  std::clamp(rtStatus.desiredJointTorques(i), -c.torqueLimit, c.torqueLimit);
1397  }
1398  }
1399 } // namespace armarx::control::common::control_law
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::numOfJoints
unsigned int numOfJoints
Definition: SafetyTaskspaceImpedanceController.h:324
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::jointLimitNullspaceTime
double jointLimitNullspaceTime
Definition: SafetyTaskspaceImpedanceController.h:306
armarx::control::common::ft::FTSensor::isSafe
bool isSafe(const FTConfig &c)
Definition: FTSensor.cpp:192
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::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::jointLimitTorqueFiltered
Eigen::VectorXf jointLimitTorqueFiltered
Definition: SafetyTaskspaceImpedanceController.h:98
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::dirErrorImp
Eigen::Vector3f dirErrorImp
Definition: SafetyTaskspaceImpedanceController.h:86
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::jointLimitJointTorque
Eigen::VectorXf jointLimitJointTorque
Definition: SafetyTaskspaceImpedanceController.h:95
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::qvel
Eigen::VectorXf qvel
Definition: SafetyTaskspaceImpedanceController.h:74
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::impedanceJointTorque
Eigen::VectorXf impedanceJointTorque
intermediate torque results
Definition: SafetyTaskspaceImpedanceController.h:92
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::preactivateInit
void preactivateInit(const VirtualRobot::RobotNodeSetPtr &rns, Config &c, RtStatus &rtStatus)
Definition: SafetyTaskspaceImpedanceController.cpp:36
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::inertiaInverse
Eigen::MatrixXf inertiaInverse
Definition: SafetyTaskspaceImpedanceController.h:297
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::forceImpedance
Eigen::Vector6f forceImpedance
task space variables
Definition: SafetyTaskspaceImpedanceController.h:69
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::nullspaceTorque
Eigen::VectorXf nullspaceTorque
Definition: SafetyTaskspaceImpedanceController.h:62
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::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::kdForceImpedance
Eigen::Vector6f kdForceImpedance
Definition: SafetyTaskspaceImpedanceController.h:70
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::currentTwist
Eigen::Vector6f currentTwist
Definition: SafetyTaskspaceImpedanceController.h:78
armarx::control::common::control_law::RobotStatus::jointVelocity
std::vector< float > jointVelocity
Definition: common.h:33
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::impTorqueRatio
float impTorqueRatio
Definition: SafetyTaskspaceImpedanceController.h:105
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::poseDiffMatImp
Eigen::Matrix3f poseDiffMatImp
intermediate impedance results
Definition: SafetyTaskspaceImpedanceController.h:82
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::run
void run(Config &c, RtStatus &robotStatus, DistanceResultsPtr &collisionPairs, std::shared_ptr< std::vector< int >> &pointsOnArm, int pointsOnArmIndex, InertiaPtr &inertiaInstance, std::vector< std::string > &selfCollisionNodes)
------------------------------— main rt-loop ---------------------------------------—
Definition: SafetyTaskspaceImpedanceController.cpp:1022
ARMARX_TRACE_LITE
#define ARMARX_TRACE_LITE
Definition: trace.h:85
clamp
double clamp(double x, double a, double b)
Definition: point.hpp:125
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::jointLimNullSpace
Eigen::MatrixXf jointLimNullSpace
Definition: SafetyTaskspaceImpedanceController.h:110
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::selfCollNullspaceTime
double selfCollNullspaceTime
Definition: SafetyTaskspaceImpedanceController.h:305
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::qpos
Eigen::VectorXf qpos
current status
Definition: SafetyTaskspaceImpedanceController.h:73
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::projTotalForceImpedance
float projTotalForceImpedance
Definition: SafetyTaskspaceImpedanceController.h:89
ARMARX_CHECK_AND_THROW
#define ARMARX_CHECK_AND_THROW(expression, ExceptionType)
This macro evaluates the expression and if it turns out to be false it will throw an exception of the...
Definition: ExpressionException.h:245
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::projForceImpedance
Eigen::Vector6f projForceImpedance
Definition: SafetyTaskspaceImpedanceController.h:88
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::jointLimitData
std::vector< jointRangeBufferZoneData > jointLimitData
Definition: SafetyTaskspaceImpedanceController.h:281
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::jointLimNullSpaceFiltered
Eigen::MatrixXf jointLimNullSpaceFiltered
Definition: SafetyTaskspaceImpedanceController.h:114
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::jointLimitTorqueTime
double jointLimitTorqueTime
Definition: SafetyTaskspaceImpedanceController.h:304
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus
internal status of the controller, containing intermediate variables, mutable targets
Definition: SafetyTaskspaceImpedanceController.h:58
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::collisionTorqueTime
double collisionTorqueTime
Definition: SafetyTaskspaceImpedanceController.h:303
if
if(!yyvaluep)
Definition: Grammar.cpp:724
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::desiredJointTorques
Eigen::VectorXf desiredJointTorques
targets
Definition: SafetyTaskspaceImpedanceController.h:61
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::projSelfCollJointTorque
Eigen::VectorXf projSelfCollJointTorque
Definition: SafetyTaskspaceImpedanceController.h:102
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::Config
common::control_law::arondto::SafetyTaskspaceImpedanceControllerConfig Config
Definition: SafetyTaskspaceImpedanceController.h:53
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::kdImpedanceTorque
Eigen::VectorXf kdImpedanceTorque
Definition: SafetyTaskspaceImpedanceController.h:93
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::currentPose
Eigen::Matrix4f currentPose
Definition: SafetyTaskspaceImpedanceController.h:77
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::rtTCP
VirtualRobot::RobotNodePtr rtTCP
Definition: SafetyTaskspaceImpedanceController.h:358
armarx::control::common::control_law
This file is part of ArmarX.
Definition: aron_conversions.cpp:68
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::firstRun
void firstRun()
Definition: SafetyTaskspaceImpedanceController.cpp:132
ARMARX_RT_LOGF_ERROR
#define ARMARX_RT_LOGF_ERROR(...)
Definition: ControlThreadOutputBuffer.h:347
armarx::control::common::ft::FTSensor::reset
void reset()
Definition: FTSensor.cpp:87
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::SimoxRobotPtr
std::shared_ptr< simox::control::simox::robot::Robot > SimoxRobotPtr
Definition: SafetyTaskspaceImpedanceController.h:51
armarx::control::common::control_law::RobotStatus::jointPosition
std::vector< float > jointPosition
Definition: common.h:32
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
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::inertia
Eigen::MatrixXf inertia
Definition: SafetyTaskspaceImpedanceController.h:296
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::selfCollNullSpaceFiltered
Eigen::MatrixXf selfCollNullSpaceFiltered
Definition: SafetyTaskspaceImpedanceController.h:113
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::rtSafe
bool rtSafe
Definition: SafetyTaskspaceImpedanceController.h:298
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::selfCollisionNullSpaceWeights
Eigen::Vector4f selfCollisionNullSpaceWeights
self-collision avoidance initialization parameters
Definition: SafetyTaskspaceImpedanceController.h:117
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::projImpedanceJointTorque
Eigen::VectorXf projImpedanceJointTorque
intermediate projected torques via null space matrices
Definition: SafetyTaskspaceImpedanceController.h:101
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::qvelFiltered
Eigen::VectorXf qvelFiltered
Definition: SafetyTaskspaceImpedanceController.h:75
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::trackingError
float trackingError
Definition: SafetyTaskspaceImpedanceController.h:84
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::jointDampingTorque
Eigen::VectorXf jointDampingTorque
Definition: SafetyTaskspaceImpedanceController.h:63
SafetyTaskspaceImpedanceController.h
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::tcp
VirtualRobot::RobotNodePtr tcp
Definition: SafetyTaskspaceImpedanceController.h:357
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::initialize
void initialize(const VirtualRobot::RobotNodeSetPtr &rns, const VirtualRobot::RobotNodeSetPtr &rtRns, SimoxRobotPtr &simoxControlRobot)
Definition: SafetyTaskspaceImpedanceController.cpp:15
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::totalForceImpedance
float totalForceImpedance
Definition: SafetyTaskspaceImpedanceController.h:87
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::projJointLimJointTorque
Eigen::VectorXf projJointLimJointTorque
Definition: SafetyTaskspaceImpedanceController.h:103
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::poseErrorImp
Eigen::Vector6f poseErrorImp
Definition: SafetyTaskspaceImpedanceController.h:83
ControlThreadOutputBuffer.h
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::jacobi
Eigen::MatrixXf jacobi
others
Definition: SafetyTaskspaceImpedanceController.h:294
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::impForceRatio
float impForceRatio
Definition: SafetyTaskspaceImpedanceController.h:106
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::selfCollisionTorqueFiltered
Eigen::VectorXf selfCollisionTorqueFiltered
Definition: SafetyTaskspaceImpedanceController.h:97
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::selfCollisionJointTorque
Eigen::VectorXf selfCollisionJointTorque
Definition: SafetyTaskspaceImpedanceController.h:94
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::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:706
armarx::control::common::control_law::RobotStatus::numJoints
unsigned int numJoints
Definition: common.h:39
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::selfCollNullSpace
Eigen::MatrixXf selfCollNullSpace
intermediate null space matrices (self-collision and joint limit avoidance)
Definition: SafetyTaskspaceImpedanceController.h:109
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::ftsensor
common::ft::FTSensor ftsensor
Definition: SafetyTaskspaceImpedanceController.h:359
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::desiredPose
Eigen::Matrix4f desiredPose
Definition: SafetyTaskspaceImpedanceController.h:79
armarx::control::common::control_law::SafetyTaskspaceImpedanceController::RtStatus::jtpinv
Eigen::MatrixXf jtpinv
Definition: SafetyTaskspaceImpedanceController.h:295