CollisionAvoidance.cpp
Go to the documentation of this file.
1 #include "CollisionAvoidance.h"
2 
3 #include <SimoxUtility/math/convert/mat4f_to_quat.h>
4 #include <VirtualRobot/MathTools.h>
5 #include <VirtualRobot/Nodes/RobotNode.h>
6 #include <VirtualRobot/RobotNodeSet.h>
7 
8 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
9 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
10 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
11 #include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
12 
13 #include "../utils.h"
14 
15 #include <simox/control/dynamics/RBDLModel.h>
16 #include <simox/control/robot/NodeInterface.h>
17 
18 namespace armarx::control::common::control_law
19 {
20 
21  CollisionAvoidanceController::CollisionAvoidanceController(
22  const simox::control::robot::NodeSetInterface* nodeSet) :
23  nodeSet(nodeSet),
24  I(nodeSet ? Eigen::MatrixXf::Identity(nodeSet->getSize(), nodeSet->getSize()) : Eigen::MatrixXf())
25  {
26  ARMARX_CHECK_EXPRESSION(nodeSet);
27  ARMARX_CHECK_GREATER(numNodes(), 0);
28  }
29 
30  CollisionAvoidanceController::~CollisionAvoidanceController()
31  {
32  ARMARX_INFO << "Destruction of CollisionAvoidanceController";
33  }
34 
35  /// methods for self-collision and joint limit avoidance
36  /// avoidance torque methods
37  void
38  CollisionAvoidanceController::calculateSelfCollisionTorque(
39  const Config& c,
40  RtStatusForSafetyStrategy& rts,
41  const DistanceResults& collisionPairs,
42  const CollisionRobotIndices& collisionRobotIndices,
43  const Eigen::VectorXf& qvelFiltered) const
44  {
45  /// self-collision avoidance algorithm following the methods in Dietrich et al. (2012):
46  ///
47  /// A. Dietrich, T. Wimbock, A. Albu-Schaffer and G. Hirzinger, "Integration of Reactive,
48  /// Torque-Based Self-Collision Avoidance Into a Task Hierarchy," in IEEE Transactions on
49  /// Robotics, vol. 28, no. 6, pp. 1278-1293, Dec. 2012, doi: 10.1109/TRO.2012.2208667.
50  ///
51  /// see: https://ieeexplore.ieee.org/document/6255795
52  ///
53  /// method:
54  ///
55  /// computes the joint torques to avoid self-collisions -> selfCollisionJointTorque
56 
57  ARMARX_CHECK_GREATER(collisionPairs.size(), 0);
58 
59  /// clear values before new cycle
60  rts.selfCollisionJointTorque.setZero();
61  rts.activeCollPairsNum = 0;
62 
63  for (auto& data : rts.selfCollDataVec)
64  {
65  data.clearValues();
66  }
67 
68  /// check collision pairs, which have are below the prefilter distance
69  for (const DistanceResult& collisionPair : collisionPairs)
70  {
71  if (static_cast<float>(collisionPair.minDistance) > c.distanceThreshold)
72  {
73  continue;
74  }
75 
76  if (collisionRobotIndices.count(collisionPair.node1) == 0 and
77  collisionRobotIndices.count(collisionPair.node2) == 0)
78  {
79  continue;
80  }
81 
82  ARMARX_CHECK(collisionPair.hasPoints());
83 
84  auto& selfCollDataVec = rts.selfCollDataVec[rts.activeCollPairsNum++];
85 
86  selfCollDataVec.minDistance =
87  static_cast<float>(collisionPair.minDistance);
88 
89  bool flipped = false;
90  if (collisionRobotIndices.count(collisionPair.node1) > 0)
91  {
92  // First node on nodeset
93 
94  if (collisionRobotIndices.count(collisionPair.node2) > 0)
95  {
96  // Check that second node comes later (equals larger index)
97  ARMARX_CHECK_GREATER(collisionPair.node2, collisionPair.node1);
98  }
99 
100  selfCollDataVec.node1 = collisionPair.node1;
101  selfCollDataVec.node2 = collisionPair.node2;
102  selfCollDataVec.point1 = collisionPair.point1->cast<float>();
103  selfCollDataVec.point2 = collisionPair.point2->cast<float>();
104  }
105  else
106  {
107  // Second node on nodeset -> flip collision pair
108 
109  flipped = true;
110  selfCollDataVec.node1 = collisionPair.node2;
111  selfCollDataVec.node2 = collisionPair.node1;
112  selfCollDataVec.point1 = collisionPair.point2->cast<float>();
113  selfCollDataVec.point2 = collisionPair.point1->cast<float>();
114  }
115 
116  // direction is pointing away from collision
117  selfCollDataVec.direction = selfCollDataVec.point1 - selfCollDataVec.point2;
118 
119  if (selfCollDataVec.minDistance < 0.0f)
120  {
121  selfCollDataVec.minDistance = 0.0f;
122 
123  // check for overlapping spheres or capsules overlapping spheres
124  // (returned points are exactly the same)
125  if (selfCollDataVec.point1.isApprox(selfCollDataVec.point2, 1e-8)
126  and collisionPair.normalVec != std::nullopt)
127  {
128  /// if the points are the same, normalVec comes with a value
129 
130  // todo: how to make sure the normalVec is always pointing away from the
131  // collision, in normally the vector points out from the contact point within
132  // the sphere
133  // issue: if the arm is modeled with a sphere, it will point towards the collision
134  // No faulty behavior has been detected so far, however there is the possibility
135  // to get a direction vector pointing towards the collision
136 
137  // solution: check if the node is a sphere, if the repulsive force is
138  // suppose to apply on the sphere, use the negative normal direction
139  selfCollDataVec.direction = collisionPair.normalVec->cast<float>();
140  if (flipped)
141  {
142  selfCollDataVec.direction *= -1.0f;
143  }
144  }
145  else
146  {
147  selfCollDataVec.direction *= -1.0f;
148  }
149  }
150 
151  selfCollDataVec.direction.normalize();
152 
153 
154  // calculate jacobian
155  {
156  const auto* node = collisionRobotIndices.at(selfCollDataVec.node1);
157  auto localTransformation = Eigen::Isometry3d::Identity();
158  localTransformation.translation() =
159  node->getGlobalPose().inverse() * selfCollDataVec.point1.cast<double>();
160  const bool result = nodeSet->computeJacobian(
161  selfCollDataVec.jacobian,
162  *node,
163  simox::control::robot::JacobianParams{
164  .mode = simox::control::utils::CartesianSelection::Position,
165  .localTransformation = localTransformation});
166  ARMARX_CHECK_EXPRESSION(result);
167  }
168 
169 
170  /// calculate common collision pair values
171  selfCollDataVec.repulsiveForce =
172  c.maxRepulsiveForce / std::pow(c.distanceThreshold, 2) *
173  std::pow(selfCollDataVec.minDistance - c.distanceThreshold, 2);
174  selfCollDataVec.localStiffness =
175  -2.0 * c.maxRepulsiveForce / std::pow(c.distanceThreshold, 2) *
176  (selfCollDataVec.minDistance - c.distanceThreshold);
177  // minimum distance is always lower than the threshold distance
178  // (local stiffness is always positive)
179  selfCollDataVec.repulsiveForce =
180  std::clamp(selfCollDataVec.repulsiveForce, 0.0f, c.maxRepulsiveForce);
181  ARMARX_CHECK_NONNEGATIVE(selfCollDataVec.repulsiveForce);
182  ARMARX_CHECK_NONNEGATIVE(selfCollDataVec.localStiffness);
183 
184  ARMARX_CHECK_EQUAL(3, selfCollDataVec.direction.rows());
185  // projected in direction of collision (1xn), is saved transposed (nx1)
186  selfCollDataVec.projectedJacT =
187  (selfCollDataVec.direction.transpose() * selfCollDataVec.jacobian.cast<float>())
188  .transpose();
189 
190  ARMARX_CHECK_EQUAL(1, selfCollDataVec.projectedJacT.cols());
191  ARMARX_CHECK_EQUAL(numNodes(), selfCollDataVec.projectedJacT.rows());
192  ARMARX_CHECK_EQUAL(numNodes(), qvelFiltered.rows());
193 
194  selfCollDataVec.distanceVelocity =
195  selfCollDataVec.projectedJacT.transpose() * qvelFiltered;
196 
197  ARMARX_CHECK_EQUAL(numNodes(), rts.inertiaInverse.rows());
198  ARMARX_CHECK_EQUAL(numNodes(), rts.inertiaInverse.cols());
199 
200  selfCollDataVec.projectedMass =
201  1 / (selfCollDataVec.projectedJacT.transpose() * rts.inertiaInverse *
202  selfCollDataVec.projectedJacT);
203 
204  ARMARX_CHECK_NONNEGATIVE(selfCollDataVec.projectedMass);
205 
206  selfCollDataVec.damping =
207  2 * std::sqrt(selfCollDataVec.localStiffness) *
208  std::sqrt(selfCollDataVec.projectedMass) * c.dampingFactor;
209 
210  ARMARX_CHECK_NONNEGATIVE(selfCollDataVec.damping);
211 
212  rts.selfCollisionJointTorque +=
213  selfCollDataVec.projectedJacT *
214  (selfCollDataVec.repulsiveForce -
215  selfCollDataVec.damping * selfCollDataVec.distanceVelocity);
216  }
217  }
218 
219  void
220  CollisionAvoidanceController::calculateJointLimitTorque(const Config& c,
221  RtStatusForSafetyStrategy& rts,
222  const Eigen::VectorXf& qpos,
223  const Eigen::VectorXf& qvelFiltered) const
224  {
225  /// method:
226  ///
227  /// computes the joint torques to avoid joint limits -> jointLimitJointTorque
228 
229  rts.jointLimitJointTorque.setZero();
230  ARMARX_CHECK_EQUAL(numNodes(), rts.jointLimitJointTorque.rows());
231 
232  ARMARX_CHECK_EQUAL(numNodes(), rts.jointLimitData.size());
233  ARMARX_CHECK_EQUAL(numNodes(), rts.inertia.cols());
234  ARMARX_CHECK_EQUAL(numNodes(), rts.inertia.rows());
235 
236  /// iterate over all joints and check the joint positions for not limiteless joints
237  for (size_t i = 0; i < rts.jointLimitData.size(); ++i)
238  {
239  if (rts.jointLimitData[i].isLimitless)
240  {
241  continue;
242  }
243 
244  auto& joint = rts.jointLimitData[i];
245  rts.jointInertia = rts.inertia(i, i);
246  if (qpos[i] < rts.jointLimitData[i].qposThresholdLow)
247  {
248  joint.repulsiveTorque =
249  c.maxRepulsiveTorque / std::pow(rts.jointLimitData[i].thresholdRange, 2) *
250  std::pow(rts.jointLimitData[i].qposThresholdLow - qpos[i], 2);
251  ARMARX_CHECK_NONNEGATIVE(joint.repulsiveTorque);
252  joint.repulsiveTorque =
253  std::clamp(joint.repulsiveTorque, 0.0f, c.maxRepulsiveTorque);
254 
255  rts.localStiffnessJointLim = 2 * c.maxRepulsiveTorque /
256  std::pow(rts.jointLimitData[i].thresholdRange, 2) *
257  (rts.jointLimitData[i].qposThresholdLow - qpos[i]);
258 
259  ARMARX_CHECK_NONNEGATIVE(rts.localStiffnessJointLim);
260  rts.dampingJointLim = 2 * std::sqrt(rts.jointInertia) *
261  std::sqrt(rts.localStiffnessJointLim) * c.dampingFactor;
262 
263  ARMARX_CHECK_NONNEGATIVE(rts.dampingJointLim);
264  joint.totalDamping = rts.dampingJointLim * qvelFiltered[i];
265  rts.jointLimitJointTorque(i) =
266  joint.repulsiveTorque - rts.dampingJointLim * qvelFiltered[i];
267 
268  ARMARX_TRACE_LITE;
269  }
270  else if (qpos[i] > rts.jointLimitData[i].qposThresholdHigh)
271  {
272  joint.repulsiveTorque =
273  c.maxRepulsiveTorque / std::pow(rts.jointLimitData[i].thresholdRange, 2) *
274  std::pow(qpos[i] - rts.jointLimitData[i].qposThresholdHigh, 2);
275 
276  ARMARX_CHECK_NONNEGATIVE(joint.repulsiveTorque);
277  joint.repulsiveTorque =
278  std::clamp(joint.repulsiveTorque, 0.0f, c.maxRepulsiveTorque);
279 
280  rts.localStiffnessJointLim = 2 * c.maxRepulsiveTorque /
281 
282  std::pow(rts.jointLimitData[i].thresholdRange, 2) *
283  (qpos[i] - rts.jointLimitData[i].qposThresholdHigh);
284  ARMARX_TRACE_LITE;
285  ARMARX_CHECK_NONNEGATIVE(rts.localStiffnessJointLim);
286  rts.dampingJointLim = 2 * std::sqrt(rts.jointInertia) *
287  std::sqrt(rts.localStiffnessJointLim) * c.dampingFactor;
288  ARMARX_CHECK_NONNEGATIVE(rts.dampingJointLim);
289  joint.totalDamping = rts.dampingJointLim * qvelFiltered[i];
290  rts.jointLimitJointTorque(i) =
291  -(joint.repulsiveTorque + rts.dampingJointLim * qvelFiltered[i]);
292  }
293  }
294  }
295 
296  /// caclulate null spaces
297  void
298  CollisionAvoidanceController::calculateSelfCollisionNullspace(const Config& c,
299  RtStatusForSafetyStrategy& rts) const
300  {
301  rts.selfCollNullSpace.setIdentity();
302 
303  ARMARX_CHECK_EQUAL(numNodes(), rts.selfCollNullSpace.rows());
304  ARMARX_CHECK_EQUAL(numNodes(), rts.selfCollNullSpace.cols());
305 
306  ARMARX_CHECK_EQUAL(4, rts.selfCollisionNullSpaceWeights.rows());
307  rts.k1 = rts.selfCollisionNullSpaceWeights(0);
308  rts.k2 = rts.selfCollisionNullSpaceWeights(1);
309  rts.k3 = rts.selfCollisionNullSpaceWeights(2);
310  rts.k4 = rts.selfCollisionNullSpaceWeights(3);
311 
312  ARMARX_TRACE_LITE;
313 
314  for (unsigned int i = 0; i < rts.activeCollPairsNum; i++)
315  {
316  auto& active = rts.selfCollDataVec[i];
317 
318  /// get desired null space value based on proximity to collision
319  if (active.minDistance < c.selfCollActivatorZ1)
320  {
321  /// full locking in direction of collision
322  rts.desiredNullSpace = 0.0f;
323  }
324  else if (c.selfCollActivatorZ1 <= active.minDistance &&
325  active.minDistance <= c.selfCollActivatorZ2)
326  {
327  /// in transition state
328  rts.desiredNullSpace = rts.k1 * std::pow(active.minDistance, 3) +
329  rts.k2 * std::pow(active.minDistance, 2) +
330  rts.k3 * active.minDistance + rts.k4;
331  if (rts.desiredNullSpace > 1.0f)
332  {
333  ARMARX_WARNING
334  << "desired null space value should not be higher than 1.0, was "
335  << rts.desiredNullSpace
336  << ", in self-collision null space calulation, weights: " << rts.k1 << ", "
337  << rts.k2 << ", " << rts.k3 << ", " << rts.k4;
338  }
339  }
340  else
341  {
342  /// unrestricted movement in direction of collision
343  rts.desiredNullSpace = 1.0f;
344  }
345 
346 
347  active.desiredNSColl = rts.desiredNullSpace;
348  rts.desiredNullSpace = std::clamp(rts.desiredNullSpace, 0.0f, 1.0f);
349  ARMARX_CHECK_NONNEGATIVE(rts.desiredNullSpace);
350  /// normalize projected Jacobian
351  rts.normalizedJacT = active.projectedJacT.normalized();
352 
353  ARMARX_CHECK_EQUAL(numNodes(), rts.normalizedJacT.rows());
354 
355  rts.tempNullSpaceMatrix = I - (rts.normalizedJacT * (1.0f - rts.desiredNullSpace) *
356  rts.normalizedJacT.transpose());
357 
358  if (c.onlyLimitCollDirection)
359  {
360  /// check, whether impedance joint torque acts against the collision direction
361  ARMARX_CHECK_EQUAL(active.projectedJacT.rows(), rts.impedanceJointTorque.rows());
362  for (int i = 0; i < rts.impedanceJointTorque.rows(); ++i)
363  {
364  if ((active.projectedJacT(i) < 0.0f and rts.impedanceJointTorque(i) < 0.0f) ||
365  (active.projectedJacT(i) > 0.0f and rts.impedanceJointTorque(i) > 0.0f))
366  {
367  // if both have the same sign (both negative or both positive), do not limit DoF
368  rts.tempNullSpaceMatrix(i, i) = 1.0f;
369  }
370  }
371  }
372 
373  /// project desired null space in corresponding direction via the Norm-Jacobian
374  rts.selfCollNullSpace *= rts.tempNullSpaceMatrix;
375  // todo: clamp the diagonal values between 0 and 1
376  }
377  }
378 
379  void
380  CollisionAvoidanceController::calculateJointLimitNullspace(const Config& c,
381  RtStatusForSafetyStrategy& rts,
382  const Eigen::VectorXf& qpos) const
383  {
384  /// check fundamentals thesis BA Jan Fenker
385  rts.jointLimNullSpace.setIdentity();
386 
387  ARMARX_CHECK_EQUAL(numNodes(), rts.jointLimNullSpace.rows());
388  ARMARX_CHECK_EQUAL(numNodes(), rts.jointLimNullSpace.cols());
389 
390  ARMARX_CHECK_EQUAL(numNodes(), rts.jointLimitData.size());
391 
392  for (size_t i = 0; i < rts.jointLimitData.size(); ++i)
393  {
394  auto& joint = rts.jointLimitData[i];
395  ARMARX_CHECK_EQUAL(4, joint.jointLimitNullSpaceWeightsLow.rows());
396  ARMARX_CHECK_EQUAL(4, joint.jointLimitNullSpaceWeightsHigh.rows());
397  rts.k1Lo = joint.jointLimitNullSpaceWeightsLow(0);
398  rts.k2Lo = joint.jointLimitNullSpaceWeightsLow(1);
399  rts.k3Lo = joint.jointLimitNullSpaceWeightsLow(2);
400  rts.k4Lo = joint.jointLimitNullSpaceWeightsLow(3);
401  rts.k1Hi = joint.jointLimitNullSpaceWeightsHigh(0);
402  rts.k2Hi = joint.jointLimitNullSpaceWeightsHigh(1);
403  rts.k3Hi = joint.jointLimitNullSpaceWeightsHigh(2);
404  rts.k4Hi = joint.jointLimitNullSpaceWeightsHigh(3);
405 
406  ARMARX_TRACE_LITE;
407 
408  if (not joint.isLimitless)
409  {
410  /// get desired null space value based on proximity to joint boundaries
411  if (qpos[i] < joint.qposZ1Low)
412  {
413  /// full locking
414  rts.desiredNullSpace = 0.0f;
415  }
416  else if (joint.qposZ1Low <= qpos[i] && qpos[i] <= joint.qposZ2Low)
417  {
418  /// in transition state
419  rts.desiredNullSpace = rts.k1Lo * std::pow(qpos[i], 3) +
420  rts.k2Lo * std::pow(qpos[i], 2) + rts.k3Lo * qpos[i] +
421  rts.k4Lo;
422  if (rts.desiredNullSpace > 1.0f)
423  {
424  ARMARX_WARNING
425  << "desired null space value should not be higher than 1.0, was "
426  << rts.desiredNullSpace
427  << ", approaching lower joint limit, weights: " << rts.k1Lo << ", "
428  << rts.k2Lo << ", " << rts.k3Lo << ", " << rts.k4Lo;
429  }
430  }
431  else if (joint.qposZ2High <= qpos[i] && qpos[i] <= joint.qposZ1High)
432  {
433  /// in transition state
434  rts.desiredNullSpace = rts.k1Hi * std::pow(qpos[i], 3) +
435  rts.k2Hi * std::pow(qpos[i], 2) + rts.k3Hi * qpos[i] +
436  rts.k4Hi;
437  if (rts.desiredNullSpace > 1.0f)
438  {
439  ARMARX_WARNING
440  << "desired null space value should not be higher than 1.0, was "
441  << rts.desiredNullSpace
442  << ", approaching upper joint limit, weights: " << rts.k1Hi << ", "
443  << rts.k2Hi << ", " << rts.k3Hi << ", " << rts.k4Hi;
444  }
445  }
446  else if (joint.qposZ1High < qpos[i])
447  {
448  /// full locking
449  rts.desiredNullSpace = 0.0f;
450  }
451  else
452  {
453  /// unrestricted movement
454  rts.desiredNullSpace = 1.0f;
455  }
456 
457 
458  joint.desiredNSjointLim = rts.desiredNullSpace;
459  rts.desiredNullSpace = std::clamp(rts.desiredNullSpace, 0.0f, 1.0f);
460  ARMARX_CHECK_NONNEGATIVE(rts.desiredNullSpace);
461 
462  /// set desired null space value in null space matrix
463  rts.jointLimNullSpace(i, i) = 1.0f - (1.0f - rts.desiredNullSpace);
464  }
465  }
466  }
467 
468  /// helpers
469 
470  Eigen::Vector4f
471  helper::calculateTransitionWeights(float z1, float z2)
472  {
473  /// Define the constraint matrix for the null space transition function
474  ///
475  /// in transition state (state between full locking and unrestricted movement for a contin-
476  /// uous control law) a third-order polynomial with four weight values is required
477  /// In this method the weights are calculated in the preactivate method based on the set
478  /// threshold parameters z1 and z2
479  ///
480  /// z1: state of full locking starts with this value
481  /// z2: state of unrestricted movement starts with this value
482  Eigen::Matrix4f constraintMatrix;
483  constraintMatrix << std::pow(z1, 3), std::pow(z1, 2), z1, 1, std::pow(z2, 3),
484  std::pow(z2, 2), z2, 1, 3 * std::pow(z1, 2), 2 * z1, 1, 0, 3 * std::pow(z2, 2), 2 * z2,
485  1, 0;
486 
487  /// Define the constraint result values for the null space transition function
488  Eigen::Vector4f b;
489  b << 0, 1, 0, 0;
490 
491  /// weights for continuous task transition
492  return constraintMatrix.colPivHouseholderQr().solve(b);
493  }
494 
495  /// --------------------------------- main rt-loop ------------------------------------------
496 
497  void
498  CollisionAvoidanceController::run(
499  const Config& c,
500  RtStatusForSafetyStrategy& rtStatus,
501  const DistanceResults& collisionPairs,
502  const CollisionRobotIndices& collisionRobotIndices,
503  DynamicsModel& dynamicsModel,
504  const Eigen::VectorXf& qpos,
505  const Eigen::VectorXf& qvelFiltered,
506  float torqueLimit)
507  {
508  // /// run in rt thread
509 
510  /// ----------------------------- inertia calculations --------------------------------------------------
511  dynamicsModel.getInertiaMatrix(qpos.cast<double>(), rtStatus.inertia);
512 
513  // rtStatus.inertiaInverse = rtStatus.inertia.cast<float>().inverse();
514  // inertia is positive definite matrix
515  rtStatus.inertiaInverse = rtStatus.inertia.cast<float>().llt().solve(I);
516 
517  /// ----------------------------- safety constraints --------------------------------------------------
518  if (c.enableSelfCollisionAvoidance)
519  {
520  // calculation of self-collision avoidance torque
521  const double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
522  calculateSelfCollisionTorque(c,
523  rtStatus,
524  collisionPairs,
525  collisionRobotIndices,
526  qvelFiltered);
527  rtStatus.collisionTorqueTime =
528  IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
529  }
530  if (c.enableJointLimitAvoidance)
531  {
532  // calculation of joint limit avoidance torque
533  const double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
534  calculateJointLimitTorque(c, rtStatus, qpos, qvelFiltered);
535  rtStatus.jointLimitTorqueTime =
536  IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
537  }
538  if (!c.samePriority)
539  {
540  if (c.enableSelfCollisionAvoidance)
541  {
542  // calculation of null space matrix for self-collision avoidance
543  const double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
544  calculateSelfCollisionNullspace(c, rtStatus);
545  rtStatus.selfCollNullspaceTime =
546  IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
547  }
548  if (c.enableJointLimitAvoidance)
549  {
550  // calculation of null space matrix for joint limit avoidance
551  const double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
552  calculateJointLimitNullspace(c, rtStatus, qpos);
553  rtStatus.jointLimitNullspaceTime =
554  IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
555  }
556  }
557  ARMARX_TRACE_LITE;
558 
559  /// --------------------------- apply EMA low pass filter -----------------------------------------------
560  if (c.filterSafetyValues)
561  {
562  /// filter safety constraint values using EMA low pass filter
563  for (int i = 0; i < rtStatus.selfCollisionJointTorque.size(); ++i)
564  {
565  rtStatus.selfCollisionTorqueFiltered(i) =
566  (1 - c.safetyValFilter) * rtStatus.selfCollisionTorqueFiltered(i) +
567  c.safetyValFilter * rtStatus.selfCollisionJointTorque(i);
568  }
569  for (int i = 0; i < rtStatus.jointLimitJointTorque.size(); ++i)
570  {
571  rtStatus.jointLimitTorqueFiltered(i) =
572  (1 - c.safetyValFilter) * rtStatus.jointLimitTorqueFiltered(i) +
573  c.safetyValFilter * rtStatus.jointLimitJointTorque(i);
574  }
575 
576  for (int i = 0; i < rtStatus.selfCollNullSpace.diagonalSize(); ++i)
577  {
578  // the computed values are only on the diagonal, so only these need to be filtered
579  rtStatus.selfCollNullSpaceFiltered(i, i) =
580  (1 - c.safetyValFilter) * rtStatus.selfCollNullSpaceFiltered(i, i) +
581  c.safetyValFilter * rtStatus.selfCollNullSpace(i, i);
582  }
583  for (int i = 0; i < rtStatus.jointLimNullSpace.diagonalSize(); ++i)
584  {
585  // the computed values are only on the diagonal, so only these need to be filtered
586  rtStatus.jointLimNullSpaceFiltered(i, i) =
587  (1 - c.safetyValFilter) * rtStatus.jointLimNullSpaceFiltered(i, i) +
588  c.safetyValFilter * rtStatus.jointLimNullSpace(i, i);
589  }
590  /// assign filtered values
591  rtStatus.selfCollisionJointTorque = rtStatus.selfCollisionTorqueFiltered;
592  rtStatus.jointLimitJointTorque = rtStatus.jointLimitTorqueFiltered;
593  rtStatus.selfCollNullSpace = rtStatus.selfCollNullSpaceFiltered;
594  rtStatus.jointLimNullSpace = rtStatus.jointLimNullSpaceFiltered;
595  }
596 
597  ARMARX_TRACE_LITE;
598  /// ----------------------------- hierarchical control --------------------------------------------------
599  /// The following control hierarchies are considered:
600  ///
601  /// three tasks in hierarchy: "enableSelfCollisionAvoidance": true, "enableJointLimitAvoidance": true
602  ///
603  /// flag: "samePriority": true
604  /// same priority: torque_coll + torque_jl + torque_imp
605  ///
606  /// flag: "topPrioritySelfCollisionAvoidance": true, "samePriority": false
607  /// self-coll avoidance on top: torque_coll -> torque_jl -> torque_imp
608  ///
609  /// flag: "topPriorityJointLimitAvoidance": true, "samePriority": false
610  /// joint limit avoidance on top: torque_jl -> torque_coll -> torque_imp
611  ///
612  /// flag: "topPrioritySelfCollisionAvoidance": false, "topPriorityJointLimitAvoidance": false,
613  /// "samePriority": false
614  /// (torque_coll + torque_jl) -> torque_imp
615  /// self-coll and joint limit avoidance on same priority level:
616  ///
617  /// two tasks in hierarchy:
618  ///
619  /// "enableSelfCollisionAvoidance": true, "enableJointLimitAvoidance": false
620  /// self-coll avoidance on top: torque_coll -> torque_imp
621  ///
622  /// "enableSelfCollisionAvoidance": false, "enableJointLimitAvoidance": true
623  /// joint limit avoidance on top: torque_jl -> torque_imp
624  ///
625  /// one task in hierarchy (impedance control):
626  ///
627  /// "enableSelfCollisionAvoidance": false, "enableJointLimitAvoidance": false
628  /// only impedance control: torque_imp
629 
630  // ARMARX_INFO << VAROUT(c.samePriority);
631  if (c.samePriority)
632  {
633  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollisionJointTorque.rows());
634  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimitJointTorque.rows());
635  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.impedanceJointTorque.rows());
636  rtStatus.desiredJointTorques =
637  rtStatus.selfCollisionJointTorque + rtStatus.jointLimitJointTorque +
638  rtStatus.impedanceJointTorque + rtStatus.kdImpedanceTorque;
639  ARMARX_TRACE_LITE;
640  }
641  else if (c.enableSelfCollisionAvoidance and c.enableJointLimitAvoidance)
642  {
643  /// three tasks in hierarchy considered
644 
645  if (c.topPrioritySelfCollisionAvoidance)
646  {
647 
648  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.rows());
649  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.cols());
650  ARMARX_CHECK_EQUAL(rtStatus.impedanceJointTorque.rows(),
651  rtStatus.selfCollNullSpace.cols());
652  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.rows());
653  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.cols());
654  ARMARX_CHECK_EQUAL(rtStatus.jointLimitJointTorque.rows(),
655  rtStatus.selfCollNullSpace.cols());
656 
657  rtStatus.desiredJointTorques =
658  rtStatus.selfCollisionJointTorque +
659  rtStatus.selfCollNullSpace *
660  (rtStatus.jointLimitJointTorque +
661  rtStatus.jointLimNullSpace * rtStatus.impedanceJointTorque) +
662  rtStatus.kdImpedanceTorque;
663 
664  // debug values
665  rtStatus.projJointLimJointTorque =
666  rtStatus.selfCollNullSpace * rtStatus.jointLimitJointTorque;
667  rtStatus.projImpedanceJointTorque =
668  rtStatus.selfCollNullSpace *
669  (rtStatus.jointLimNullSpace * rtStatus.impedanceJointTorque);
670  }
671  else if (c.topPriorityJointLimitAvoidance)
672  {
673  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.rows());
674  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.cols());
675  ARMARX_CHECK_EQUAL(rtStatus.impedanceJointTorque.rows(),
676  rtStatus.selfCollNullSpace.cols());
677  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.rows());
678  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.cols());
679  ARMARX_CHECK_EQUAL(rtStatus.selfCollisionJointTorque.rows(),
680  rtStatus.jointLimNullSpace.cols());
681 
682  rtStatus.desiredJointTorques =
683  rtStatus.jointLimitJointTorque +
684  rtStatus.jointLimNullSpace *
685  (rtStatus.selfCollisionJointTorque +
686  rtStatus.selfCollNullSpace * rtStatus.impedanceJointTorque) +
687  rtStatus.kdImpedanceTorque;
688 
689  // debug values
690  rtStatus.projSelfCollJointTorque =
691  rtStatus.jointLimNullSpace * rtStatus.selfCollisionJointTorque;
692  rtStatus.projImpedanceJointTorque =
693  rtStatus.jointLimNullSpace *
694  (rtStatus.selfCollNullSpace * rtStatus.impedanceJointTorque);
695  }
696  else
697  {
698  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.rows());
699  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.cols());
700  ARMARX_CHECK_EQUAL(rtStatus.impedanceJointTorque.rows(),
701  rtStatus.selfCollNullSpace.cols());
702  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.rows());
703  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.cols());
704  ARMARX_CHECK_EQUAL(rtStatus.selfCollisionJointTorque.rows(),
705  rtStatus.jointLimNullSpace.cols());
706  rtStatus.desiredJointTorques =
707  rtStatus.jointLimitJointTorque + rtStatus.selfCollisionJointTorque +
708  (rtStatus.jointLimNullSpace * rtStatus.selfCollNullSpace) *
709  rtStatus.impedanceJointTorque +
710  rtStatus.kdImpedanceTorque;
711 
712  // debug values
713  rtStatus.projImpedanceJointTorque =
714  (rtStatus.jointLimNullSpace * rtStatus.selfCollNullSpace) *
715  rtStatus.impedanceJointTorque;
716  }
717  }
718  else if (c.enableSelfCollisionAvoidance)
719  {
720  /// impedance control and self-collision avoidance
721 
722  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.rows());
723  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.cols());
724  ARMARX_CHECK_EQUAL(rtStatus.impedanceJointTorque.rows(),
725  rtStatus.selfCollNullSpace.cols());
726 
727  rtStatus.desiredJointTorques =
728  rtStatus.selfCollisionJointTorque +
729  rtStatus.selfCollNullSpace * rtStatus.impedanceJointTorque +
730  rtStatus.kdImpedanceTorque;
731 
732  // debugging
733  rtStatus.projImpedanceJointTorque =
734  rtStatus.selfCollNullSpace * rtStatus.impedanceJointTorque;
735  }
736  else if (c.enableJointLimitAvoidance)
737  {
738  /// impedance control and joint limit avoidance
739 
740  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.rows());
741  ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.cols());
742  ARMARX_CHECK_EQUAL(rtStatus.impedanceJointTorque.rows(),
743  rtStatus.jointLimNullSpace.cols());
744 
745  rtStatus.desiredJointTorques =
746  rtStatus.jointLimitJointTorque +
747  rtStatus.jointLimNullSpace * rtStatus.impedanceJointTorque +
748  rtStatus.kdImpedanceTorque;
749 
750  // debugging
751  rtStatus.projImpedanceJointTorque =
752  rtStatus.jointLimNullSpace * rtStatus.impedanceJointTorque;
753  }
754  else
755  {
756  /// plain impedance control
757  rtStatus.desiredJointTorques =
758  rtStatus.impedanceJointTorque + rtStatus.kdImpedanceTorque;
759  }
760 
761  // rtStatus.desiredJointTorques += rtStatus.jointDampingTorque;
762 
763  ARMARX_TRACE_LITE;
764 
765  /// ------------------------- write impedance forces to buffer -----------------------------------------
766  // this section is purely for visualization purposes in the viewer
767  // rtStatus.dirErrorImp = rtStatus.poseErrorImp.head<3>().normalized();
768  // rtStatus.totalForceImpedance = rtStatus.forceImpedance.head<3>().norm();
769  // rtStatus.projForceImpedance = rtStatus.jtpinv * rtStatus.projImpedanceJointTorque;
770  // rtStatus.projTotalForceImpedance = rtStatus.projForceImpedance.head<3>().norm();
771  // rtStatus.impForceRatio = rtStatus.projTotalForceImpedance / rtStatus.totalForceImpedance;
772  // rtStatus.impTorqueRatio =
773  // rtStatus.projImpedanceJointTorque.norm() / rtStatus.impedanceJointTorque.norm();
774 
775 
776  /// ----------------------------- write torque target --------------------------------------------------
777  for (int i = 0; i < rtStatus.desiredJointTorques.rows(); ++i)
778  {
779  rtStatus.desiredJointTorques(i) =
780  std::clamp(rtStatus.desiredJointTorques(i), -torqueLimit, torqueLimit);
781  }
782  }
783 
784  void
785  CollisionAvoidanceController::RtStatusForSafetyStrategy::reset(
786  const Config& c,
787  const unsigned int nDoF,
788  const unsigned int maxCollisionPairs,
789  VirtualRobot::RobotNodeSetPtr& rns)
790  {
791  trackingError = 0.0;
792  dirErrorImp.setZero();
793  totalForceImpedance = 0.0;
794  projForceImpedance.setZero();
795  projTotalForceImpedance = 0.0;
796 
797  /// intermediate torque results
798  impedanceJointTorque.setZero(nDoF);
799  kdImpedanceTorque.setZero(nDoF);
800  selfCollisionJointTorque.setZero(nDoF);
801  jointLimitJointTorque.setZero(nDoF);
802 
803  selfCollisionTorqueFiltered.setZero(nDoF);
804  jointLimitTorqueFiltered.setZero(nDoF);
805 
806  /// intermediate projected torques via null space matrices
807  projImpedanceJointTorque.setZero(nDoF);
808  projSelfCollJointTorque.setZero(nDoF);
809  projJointLimJointTorque.setZero(nDoF);
810 
811  impForceRatio = 0.0f;
812  impTorqueRatio = 0.0f;
813 
814  /// intermediate null space matrices (self-collision and joint limit avoidance)
815  selfCollNullSpace.resize(nDoF, nDoF);
816  selfCollNullSpace.setZero();
817  jointLimNullSpace.resize(nDoF, nDoF);
818  jointLimNullSpace.setZero();
819  desiredNullSpace = 0.0;
820 
821  selfCollNullSpaceFiltered.resize(nDoF, nDoF);
822  selfCollNullSpaceFiltered.setZero();
823  jointLimNullSpaceFiltered.resize(nDoF, nDoF);
824  jointLimNullSpaceFiltered.setZero();
825 
826  /// self-collision avoidance initialization parameters
827  /// calculate weights for self-collision nullspace transition function
828  /// range between z1 and z2
829  /// z1: below this distance [m] the collision direction is fully locked
830  /// z2: above this distance collision direction is unrestricted
831  // selfCollisionNullSpaceWeights.setZero();
832  selfCollisionNullSpaceWeights =
833  helper::calculateTransitionWeights(c.selfCollActivatorZ1, c.selfCollActivatorZ2);
834 
835  /// distance results
836  Eigen::Vector3d point1(0.0, 0.0, 0.0);
837  Eigen::Vector3d point2(0.0, 0.0, 0.0);
838  Eigen::Vector3d norm(0.0, 0.0, 0.0);
839 
840  selfCollDataVec.resize(maxCollisionPairs,
841  CollisionAvoidanceController::SelfCollisionData(nDoF));
842 
843  /// self-collision avoidance null space intermediate results
844  k1 = 0.0;
845  k2 = 0.0;
846  k3 = 0.0;
847  k4 = 0.0;
848  normalizedJacT.setZero(nDoF);
849  tempNullSpaceMatrix.resize(nDoF, nDoF);
850  tempNullSpaceMatrix.setZero();
851 
852  /// joint limit avoidance initialization parameters
853  CollisionAvoidanceController::RtStatusForSafetyStrategy::initJointLimtDataVec(c, nDoF, rns);
854 
855  /// others
856  inertia.resize(nDoF, nDoF);
857  inertia.setZero();
858  inertiaInverse.resize(nDoF, nDoF);
859  inertiaInverse.setZero();
860 
861  /// time status
862  collisionPairTime = 0.0;
863  preFilterTime = 0.0;
864  collisionTorqueTime = 0.0;
865  jointLimitTorqueTime = 0.0;
866  selfCollNullspaceTime = 0.0;
867  jointLimitNullspaceTime = 0.0;
868 
869  /// collision pair info
870  collisionPairsNum = 0;
871  activeCollPairsNum = 0;
872  }
873 
874  void
875  CollisionAvoidanceController::RtStatusForSafetyStrategy::initJointLimtDataVec(
876  const Config& c,
877  const unsigned int nDoF,
878  VirtualRobot::RobotNodeSetPtr& rns)
879  {
880  jointVel = 0.0;
881  jointInertia = 0.0;
882 
883  localStiffnessJointLim = 0.0;
884  dampingJointLim = 0.0;
885 
886  k1Lo = 0.0;
887  k2Lo = 0.0;
888  k3Lo = 0.0;
889  k4Lo = 0.0;
890  k1Hi = 0.0;
891  k2Hi = 0.0;
892  k3Hi = 0.0;
893  k4Hi = 0.0;
894 
895  jointLimitData.resize(nDoF);
896 
897  /// obtain joint information (limits, limitless) from VirtualRobot
898  for (size_t i = 0; i < nDoF; ++i)
899  {
900  jointLimitData[i].jointName = rns->getNode(i)->getName();
901  jointLimitData[i].isLimitless = rns->getNode(i)->isLimitless();
902 
903  if (not jointLimitData[i].isLimitless)
904  {
905  jointLimitData[i].qLimLow = rns->getNode(i)->getJointLimitLow();
906  jointLimitData[i].qLimHigh = rns->getNode(i)->getJointLimitHigh();
907  }
908  }
909 
910  // std::vector<std::string> jointInfo;
911  for (auto& joint : jointLimitData)
912  {
913  if (not joint.isLimitless)
914  {
915  /// set joint limit avoidance threshold parameters for not limiteless joints
916  float qRange = joint.qLimHigh - joint.qLimLow;
917  joint.thresholdRange = c.jointRangeBufferZone * qRange;
918  joint.qposThresholdLow = joint.qLimLow + joint.thresholdRange;
919  joint.qposThresholdHigh = joint.qLimHigh - joint.thresholdRange;
920  float qposZ1 = c.jointRangeBufferZoneZ1 * qRange;
921  float qposZ2 = c.jointRangeBufferZoneZ2 * qRange;
922  joint.qposZ1Low = joint.qLimLow + qposZ1;
923  joint.qposZ2Low = joint.qLimLow + qposZ2;
924  joint.qposZ1High = joint.qLimHigh - qposZ1;
925  joint.qposZ2High = joint.qLimHigh - qposZ2;
926  joint.jointLimitNullSpaceWeightsLow =
927  helper::calculateTransitionWeights(joint.qposZ1Low, joint.qposZ2Low);
928  joint.jointLimitNullSpaceWeightsHigh =
929  helper::calculateTransitionWeights(joint.qposZ1High, joint.qposZ2High);
930 
931  /// from here: only the parameter output information is prepared
932  // std::vector<float> params = {joint.qLimLow,
933  // joint.qLimHigh,
934  // joint.qposThresholdLow,
935  // joint.qposThresholdHigh,
936  // joint.qposZ1Low,
937  // joint.qposZ2Low,
938  // joint.qposZ1High,
939  // joint.qposZ2High,
940  // joint.jointLimitNullSpaceWeightsLow(0),
941  // joint.jointLimitNullSpaceWeightsLow(1),
942  // joint.jointLimitNullSpaceWeightsLow(2),
943  // joint.jointLimitNullSpaceWeightsLow(3),
944  // joint.jointLimitNullSpaceWeightsHigh(0),
945  // joint.jointLimitNullSpaceWeightsHigh(1),
946  // joint.jointLimitNullSpaceWeightsHigh(2),
947  // joint.jointLimitNullSpaceWeightsHigh(3)};
948  // std::vector<std::string> stringParams;
949  // // convert float values to string rounded to two digits
950  // for (auto& param : params)
951  // {
952  // std::stringstream ss;
953  // ss << std::fixed << std::setprecision(2) << param;
954  // stringParams.push_back(ss.str());
955  // }
956  // std::string info = joint.jointName + ": LimLo: " + stringParams[0] +
957  // " - LimHi: " + stringParams[1] + " - q0L: " + stringParams[2] +
958  // " - q0H: " + stringParams[3] + " - z1L: " + stringParams[4] +
959  // " z2L: " + stringParams[5] + " z1H: " + stringParams[6] +
960  // " z2H: " + stringParams[7] + " k1L: " + stringParams[8] +
961  // " k2L: " + stringParams[9] + " k3L: " + stringParams[10] +
962  // " k4L: " + stringParams[11] + " k1H: " + stringParams[12] +
963  // " k2H: " + stringParams[13] + " k3H: " + stringParams[14] +
964  // " k4H: " + stringParams[15];
965  // jointInfo.push_back(info);
966  }
967  }
968 
969  // std::ostringstream oss;
970  // for (const auto& str : jointInfo)
971  // {
972  // oss << str << "\n";
973  // }
974  // std::string initMessage = oss.str();
975  // ARMARX_INFO << "joint limit avoidance parameters initialized for: \n" << initMessage;
976  }
977 
978  void
979  collision::validateConfigData(const arondto::CollisionAvoidanceConfig& cfg)
980  {
981  /// self-collision config parameters
982  ARMARX_CHECK_GREATER(cfg.dampingFactor, 0.0); // damping can't be exactly zero
983  ARMARX_CHECK_LESS_EQUAL(cfg.dampingFactor, 1.0); // for scaling of the damping
984  ARMARX_CHECK_GREATER(cfg.maxRepulsiveForce, 0.0);
985  ARMARX_CHECK_NONNEGATIVE(cfg.maxRepulsiveForce);
986  ARMARX_CHECK_NONNEGATIVE(cfg.distanceThreshold);
987  ARMARX_CHECK_NONNEGATIVE(cfg.selfCollActivatorZ1);
988  ARMARX_CHECK_NONNEGATIVE(cfg.selfCollActivatorZ2);
989  ARMARX_CHECK_GREATER(cfg.selfCollActivatorZ2, cfg.selfCollActivatorZ1);
990 
991  /// joint limit avoidance config parameters
992  ARMARX_CHECK_NONNEGATIVE(cfg.maxRepulsiveTorque);
993  ARMARX_CHECK_NONNEGATIVE(cfg.jointRangeBufferZone);
994  ARMARX_CHECK_LESS_EQUAL(cfg.jointRangeBufferZone, 1.0); // percentage value
995  ARMARX_CHECK_NONNEGATIVE(cfg.jointRangeBufferZoneZ1);
996  ARMARX_CHECK_LESS_EQUAL(cfg.jointRangeBufferZoneZ1, 1.0); // percentage value
997  ARMARX_CHECK_NONNEGATIVE(cfg.jointRangeBufferZoneZ2);
998  ARMARX_CHECK_GREATER(cfg.jointRangeBufferZoneZ2, cfg.jointRangeBufferZoneZ1);
999  ARMARX_CHECK_LESS_EQUAL(cfg.jointRangeBufferZoneZ2, 1.0); // percentage value
1000  ARMARX_CHECK_NONNEGATIVE(cfg.safetyValFilter);
1001  ARMARX_CHECK_LESS_EQUAL(cfg.safetyValFilter, 1.0);
1002 
1003  /// check hierarchy flags for valid config
1004  if (cfg.topPrioritySelfCollisionAvoidance)
1005  {
1006  ARMARX_CHECK_EXPRESSION(cfg.enableSelfCollisionAvoidance);
1007  ARMARX_CHECK_EXPRESSION(
1008  (cfg.topPrioritySelfCollisionAvoidance == !cfg.topPriorityJointLimitAvoidance));
1009  }
1010  if (cfg.topPriorityJointLimitAvoidance)
1011  {
1012  ARMARX_CHECK_EXPRESSION(cfg.enableJointLimitAvoidance);
1013  ARMARX_CHECK_EXPRESSION(
1014  (cfg.topPrioritySelfCollisionAvoidance == !cfg.topPriorityJointLimitAvoidance));
1015  }
1016  }
1017 
1018 } // namespace armarx::control::common::control_law
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::inertia
Eigen::MatrixXd inertia
others
Definition: CollisionAvoidance.h:210
armarx::control::common::control_law::CollisionAvoidanceController::numNodes
std::size_t numNodes() const
Definition: CollisionAvoidance.h:251
Eigen
Definition: Elements.h:32
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::CollisionAvoidanceController::CollisionRobotIndices
std::unordered_map< unsigned int, const simox::control::robot::NodeInterface * > CollisionRobotIndices
Definition: CollisionAvoidance.h:61
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::jointLimitJointTorque
Eigen::VectorXf jointLimitJointTorque
Definition: CollisionAvoidance.h:162
armarx::control::common::control_law::CollisionAvoidanceController::DistanceResults
std::vector< DistanceResult > DistanceResults
Definition: CollisionAvoidance.h:57
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::projImpedanceJointTorque
Eigen::VectorXf projImpedanceJointTorque
intermediate projected torques via null space matrices
Definition: CollisionAvoidance.h:168
armarx::control::common::control_law::collision::validateConfigData
void validateConfigData(const arondto::CollisionAvoidanceConfig &cfg)
Definition: CollisionAvoidance.cpp:979
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
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::desiredJointTorques
Eigen::VectorXf desiredJointTorques
targets
Definition: CollisionAvoidance.h:149
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::projJointLimJointTorque
Eigen::VectorXf projJointLimJointTorque
Definition: CollisionAvoidance.h:170
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::selfCollisionJointTorque
Eigen::VectorXf selfCollisionJointTorque
Definition: CollisionAvoidance.h:161
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::selfCollNullSpace
Eigen::MatrixXf selfCollNullSpace
intermediate null space matrices (self-collision and joint limit avoidance)
Definition: CollisionAvoidance.h:176
ARMARX_TRACE_LITE
#define ARMARX_TRACE_LITE
Definition: trace.h:98
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
clamp
double clamp(double x, double a, double b)
Definition: point.hpp:136
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::projSelfCollJointTorque
Eigen::VectorXf projSelfCollJointTorque
Definition: CollisionAvoidance.h:169
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::kdImpedanceTorque
Eigen::VectorXf kdImpedanceTorque
Definition: CollisionAvoidance.h:160
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::control::common::control_law::CollisionAvoidanceController::~CollisionAvoidanceController
~CollisionAvoidanceController()
Definition: CollisionAvoidance.cpp:30
armarx::control::common::control_law::CollisionAvoidanceController::CollisionAvoidanceController
CollisionAvoidanceController(const simox::control::robot::NodeSetInterface *nodeSet)
Definition: CollisionAvoidance.cpp:21
armarx::control::common::control_law::CollisionAvoidanceController::DynamicsModel
simox::control::dynamics::RBDLModel DynamicsModel
Definition: CollisionAvoidance.h:58
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::jointLimitNullspaceTime
double jointLimitNullspaceTime
Definition: CollisionAvoidance.h:219
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::impedanceJointTorque
Eigen::VectorXf impedanceJointTorque
intermediate torque results
Definition: CollisionAvoidance.h:159
armarx::control::common::control_law
This file is part of ArmarX.
Definition: aron_conversions.cpp:68
ControlTarget1DoFActuator.h
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy
internal status of the controller, containing intermediate variables, mutable targets
Definition: CollisionAvoidance.h:143
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::collisionTorqueTime
double collisionTorqueTime
Definition: CollisionAvoidance.h:216
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::CollisionAvoidanceController::RtStatusForSafetyStrategy::jointLimitTorqueTime
double jointLimitTorqueTime
Definition: CollisionAvoidance.h:217
ExpressionException.h
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::selfCollisionTorqueFiltered
Eigen::VectorXf selfCollisionTorqueFiltered
Definition: CollisionAvoidance.h:164
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:704
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
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::selfCollNullspaceTime
double selfCollNullspaceTime
Definition: CollisionAvoidance.h:218
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::jointLimNullSpaceFiltered
Eigen::MatrixXf jointLimNullSpaceFiltered
Definition: CollisionAvoidance.h:181
armarx::control::common::control_law::CollisionAvoidanceController::Config
common::control_law::arondto::CollisionAvoidanceConfig Config
Definition: CollisionAvoidance.h:60
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::jointLimitTorqueFiltered
Eigen::VectorXf jointLimitTorqueFiltered
Definition: CollisionAvoidance.h:165
CollisionAvoidance.h
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::inertiaInverse
Eigen::MatrixXf inertiaInverse
Definition: CollisionAvoidance.h:211
ControlThreadOutputBuffer.h
armarx::navigation::core::Position
Eigen::Vector3f Position
Definition: basic_types.h:36
armarx::control::common::control_law::CollisionAvoidanceController::SelfCollisionData
Definition: CollisionAvoidance.h:63
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::control::common::control_law::helper::calculateTransitionWeights
Eigen::Vector4f calculateTransitionWeights(float z1, float z2)
helper functions
Definition: CollisionAvoidance.cpp:471
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::selfCollNullSpaceFiltered
Eigen::MatrixXf selfCollNullSpaceFiltered
Definition: CollisionAvoidance.h:180
armarx::control::common::control_law::CollisionAvoidanceController::RtStatusForSafetyStrategy::jointLimNullSpace
Eigen::MatrixXf jointLimNullSpace
Definition: CollisionAvoidance.h:177
armarx::control::common::control_law::CollisionAvoidanceController::DistanceResult
::simox::control::environment::DistanceResult DistanceResult
Definition: CollisionAvoidance.h:56
SensorValue1DoFActuator.h
norm
double norm(const Point &a)
Definition: point.hpp:102