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