CollisionAvoidanceImpedanceController.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package ...
17  * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
18  * @date 2021
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "CollisionAvoidanceImpedanceController.h"
24 
25 #include <VirtualRobot/MathTools.h>
26 
27 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
28 #include <ArmarXCore/core/PackagePath.h> // for GUI
29 #include <ArmarXCore/core/time/CycleUtil.h>
30 #include <ArmarXCore/core/time/TimeUtil.h>
31 #include <ArmarXCore/observers/variant/SingleTypeVariantList.h> // for GUI
32 
33 #include <armarx/control/common/aron_conversions.h> // for GUI
34 #include <armarx/control/common/utils.h>
35 #include <armarx/control/ethercat/RTUtility.h>
36 
37 #include <simox/control/geodesics/util.h>
38 #include <simox/control/robot/NodeInterface.h>
39 
40 namespace armarx::control::njoint_controller::task_space
41 {
42  NJointControllerRegistration<NJointTaskspaceCollisionAvoidanceImpedanceController>
43  registrationControllerNJointTaskspaceCollisionAvoidanceImpedanceController(
44  "NJointTaskspaceCollisionAvoidanceImpedanceController");
45 
46  NJointTaskspaceCollisionAvoidanceImpedanceController::
47  NJointTaskspaceCollisionAvoidanceImpedanceController(
48  const RobotUnitPtr& robotUnit,
49  const NJointControllerConfigPtr& config,
50  const VirtualRobot::RobotPtr& robot) :
51  NJointTaskspaceImpedanceController{robotUnit, config, robot}
52  {
53  ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
54  ARMARX_CHECK_EXPRESSION(cfg);
55  userCfgWithColl = CollisionCtrlCfg::FromAron(cfg->config);
56 
57  coll = std::make_shared<core::CollisionAvoidanceBase>(rtGetRobot(), userCfgWithColl.coll);
58  collReady.store(true);
59  ARMARX_INFO << "-- " << getClassName() << " initialized --";
60  }
61 
62  std::string
63  NJointTaskspaceCollisionAvoidanceImpedanceController::getClassName(const Ice::Current&) const
64  {
65  return "NJointTaskspaceCollisionAvoidanceImpedanceController";
66  }
67 
68  void
69  NJointTaskspaceCollisionAvoidanceImpedanceController::limbRT(ArmPtr& arm, const double deltaT)
70  {
71  double time_measure = IceUtil::Time::now().toMicroSecondsDouble();
72  limbRTUpdateStatus(arm, deltaT);
73  double time_update_status = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
74 
75  arm->controller.run(arm->rtConfig, arm->rtStatus);
76  double time_run_rt = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
77 
78  if (collReady.load())
79  {
80  ARMARX_CHECK_EXPRESSION(coll);
81  coll->rtLimbControllerRun(arm->kinematicChainName,
82  arm->rtStatus.jointPos,
83  arm->rtStatus.qvelFiltered,
84  arm->rtConfig.torqueLimit,
85  arm->rtStatus.desiredJointTorques);
86  }
87  double time_coll_run = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
88 
89  if (collReady.load())
90  {
91  limbRTSetTarget(arm,
92  coll->collLimb.at(arm->kinematicChainName)->rts.desiredJointTorques);
93  }
94  else
95  {
96  limbRTSetTarget(arm, arm->rtStatus.desiredJointTorques);
97  }
98  double time_set_target = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
99 
100  time_measure = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
101 
102  if (time_measure > 200)
103  {
104  ARMARX_RT_LOGF_WARN("---- rt too slow: "
105  "time_update_status: %.2f\n"
106  "run_rt_limb: %.2f\n"
107  "run_coll_rt_limb: %.2f\n"
108  "set_target_limb: %.2f\n"
109  "time all: %.2f\n",
110  time_update_status, // 0-1 us
111  time_run_rt - time_update_status, //
112  time_coll_run - time_run_rt, //
113  time_set_target - time_coll_run, //
114  time_measure)
115  .deactivateSpam(1.0f); // 0-1 us
116  }
117  }
118 
119  void
120  NJointTaskspaceCollisionAvoidanceImpedanceController::rtRun(
121  const IceUtil::Time& sensorValuesTimestamp,
122  const IceUtil::Time& timeSinceLastIteration)
123  {
124  double deltaT = timeSinceLastIteration.toSecondsDouble();
125 
126  if (collReady.load())
127  {
128  ARMARX_CHECK_EXPRESSION(coll);
129  coll->updateRtConfigFromUser();
130  coll->rtCollisionChecking();
131  }
132  for (auto& pair : limb)
133  {
134  this->limbRT(pair.second, deltaT);
135  }
136  if (hands)
137  {
138  hands->updateRTStatus(deltaT);
139  hands->setTargets();
140  }
141  }
142 
143  void
144  NJointTaskspaceCollisionAvoidanceImpedanceController::updateCollisionAvoidanceConfig(
145  const ::armarx::aron::data::dto::DictPtr& dto,
146  const Ice::Current& iceCurrent)
147  {
148  ARMARX_CHECK_EXPRESSION(coll);
149  auto cfg = common::control_law::arondto::CollisionAvoidanceConfigDict::FromAron(dto);
150  coll->setUserCfg(cfg);
151  }
152 
153  ::armarx::aron::data::dto::DictPtr
154  NJointTaskspaceCollisionAvoidanceImpedanceController::getCollisionAvoidanceConfig(
155  const Ice::Current& iceCurrent)
156  {
157  if (not collReady.load())
158  return nullptr;
159 
160  ARMARX_CHECK_EXPRESSION(coll);
161  return coll->userCfg.toAronDTO();
162  }
163 
164  void
165  NJointTaskspaceCollisionAvoidanceImpedanceController::updateConfig(
166  const ::armarx::aron::data::dto::DictPtr& dto,
167  const Ice::Current& iceCurrent)
168  {
169  NJointTaskspaceImpedanceController::updateConfig(dto);
170  userCfgWithColl = CollisionCtrlCfg::FromAron(dto);
171  ARMARX_CHECK_EXPRESSION(coll);
172  coll->setUserCfg(userCfgWithColl.coll);
173  }
174 
175  ::armarx::aron::data::dto::DictPtr
176  NJointTaskspaceCollisionAvoidanceImpedanceController::getConfig(const Ice::Current& iceCurrent)
177  {
178  NJointTaskspaceImpedanceController::getConfig();
179  userCfgWithColl.limbs = userConfig.limbs;
180  userCfgWithColl.hands = userConfig.hands;
181  return userCfgWithColl.toAronDTO();
182  }
183 
184  void
185  NJointTaskspaceCollisionAvoidanceImpedanceController::collLimbPublish(
186  core::CollisionAvoidanceBase::CollArmPtr& arm,
187  const DebugObserverInterfacePrx& debugObs)
188  {
189  // double limbTimeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
190 
191  StringVariantBaseMap datafields;
192  auto rtStatus = arm->bufferRTtoPublish.getUpToDateReadBuffer();
193 
194  datafields["trackingError"] = new Variant(rtStatus.trackingError);
195  common::debugEigenVec(datafields, "desiredJointTorques", rtStatus.desiredJointTorques);
196  common::debugEigenVec(datafields, "selfCollisionTorque", rtStatus.selfCollisionJointTorque);
197  common::debugEigenVec(datafields, "jointLimitTorque", rtStatus.jointLimitJointTorque);
198 
199  common::debugEigenVec(
200  datafields, "projectedImpedanceTorque", rtStatus.projImpedanceJointTorque);
201  // common::debugEigenVec(datafields, "projectedJointLimitTorque", rtStatus.projJointLimTorque);
202  // common::debugEigenVec(datafields, "projectedForceImpedance", rtStatus.projForceImpedance);
203  // common::debugEigenVec(
204  // datafields, "projectedSelfCollisionTorque", rtStatus.projSelfCollTorque);
205 
206  Eigen::VectorXf selfCollNullspace = rtStatus.selfCollNullSpace.diagonal();
207  Eigen::VectorXf jointLimNullspace = rtStatus.jointLimNullSpace.diagonal();
208  common::debugEigenVec(datafields, "selfCollNullspaceDiagonal", selfCollNullspace);
209  common::debugEigenVec(datafields, "jointLimNullspaceDiagonal", jointLimNullspace);
210 
211  // if (rtData.enableJointLimitAvoidance)
212  // {
213  // for (size_t i = 0; i < rtStatus.jointLimitData.size(); ++i)
214  // {
215  // if (not rtStatus.jointLimitData[i].isLimitless)
216  // {
217  // datafields["n_des(q)_" + std::to_string(i) + "_" +
218  // rtStatus.jointLimitData[i].jointName] =
219  // new Variant(rtStatus.jointLimitData[i].desiredNSjointLim);
220  // datafields["q_damping" + std::to_string(i) + "_" +
221  // rtStatus.jointLimitData[i].jointName] =
222  // new Variant(rtStatus.jointLimitData[i].totalDamping);
223  // datafields["q_repTorque" + std::to_string(i) + "_" +
224  // rtStatus.jointLimitData[i].jointName] =
225  // new Variant(rtStatus.jointLimitData[i].repulsiveTorque);
226  // }
227  // }
228  // }
229 
230  // datafields["activeCollPairsNum"] = new Variant(rtStatus.activeCollPairsNum);
231  // datafields["collisionPairsNum"] = new Variant(rtStatus.collisionPairsNum);
232  // datafields["jointLimitNullspaceTime"] = new Variant(rtStatus.jointLimitNullspaceTime);
233  // datafields["jointLimitTorqueTime"] = new Variant(rtStatus.jointLimitTorqueTime);
234  // datafields["collisionTorqueTime"] = new Variant(rtStatus.collisionTorqueTime);
235  // datafields["selfCollNullspaceTime"] = new Variant(rtStatus.selfCollNullspaceTime);
236  // datafields["collisionPairTime"] = new Variant(rtStatus.collisionPairTime);
237  // datafields["impForceRatio"] = new Variant(rtStatus.impForceRatio);
238  // datafields["impTorqueRatio"] = new Variant(rtStatus.impTorqueRatio);
239 
240  // double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
241  viz::Layer layer = arviz.layer(getName() + "_" + arm->nodeSetName);
242  for (int i = 0; i < rtStatus.activeCollPairsNum; ++i)
243  {
244  // visualize impedance force
245 
246  // layer.add(viz::Arrow("projImpForce_" + std::to_string(i))
247  // .fromTo(rtStatus.selfCollDataVec[i].point * 1000.0,
248  // rtStatus.selfCollDataVec[i].point * 1000.0 +
249  // rtStatus.selfCollDataVec[i].projectedImpedanceForce *
250  // rtStatus.selfCollDataVec[i].direction * 5.0)
251  // .color(simox::Color::purple()));
252 
253  // layer.add(viz::Arrow("impForce_" + std::to_string(i))
254  // .fromTo(rtStatus.selfCollDataVec[i].point * 1000.0,
255  // rtStatus.selfCollDataVec[i].point * 1000.0 +
256  // rtStatus.forceImpedance.head(3).dot(
257  // rtStatus.selfCollDataVec[i].direction) *
258  // rtStatus.selfCollDataVec[i].direction * 5.0)
259  // .color(simox::Color::yellow())); // project forceImp in dir of collision
260  layer.add(viz::Arrow("force_" + std::to_string(i))
261  .fromTo(rtStatus.selfCollDataVec[i].point * 1000.0,
262  rtStatus.selfCollDataVec[i].point * 1000.0 +
263  rtStatus.selfCollDataVec[i].direction * 50.0 *
264  rtStatus.selfCollDataVec[i].repulsiveForce)
265  .color(simox::Color::blue()));
266 
267  //
268 
269  // //size_t index = rtStatus.distanceIndexPairs[i].second;
270  // datafields[std::to_string(i) + "_minDistance"] =
271  // new Variant(rtStatus.selfCollDataVec[i].minDistance);
272  // datafields[std::to_string(i) + "_repForce"] =
273  // new Variant(rtStatus.selfCollDataVec[i].repulsiveForce);
274  // datafields[std::to_string(i) + "_dampingForce"] =
275  // new Variant(-1.0f * rtStatus.selfCollDataVec[i].damping *
276  // rtStatus.selfCollDataVec[i].distanceVelocity);
277  // datafields[std::to_string(i) + "_n_des(d)"] =
278  // new Variant(rtStatus.selfCollDataVec[i].desiredNSColl);
279  // common::debugEigenVec(
280  // datafields, std::to_string(i) + "_point", rtStatus.selfCollDataVec[i].point);
281  // common::debugEigenVec(
282  // datafields, std::to_string(i) + "_dir", rtStatus.selfCollDataVec[i].direction);
283  //
284  }
285 
286 
287  // /// prepare test case config
288  // const float tolerance = 1e-9f;
289  // if (rtData.testConfig == 1)
290  // {
291  // if (rtStatus.evalData[0].nodeName == "ArmL8_Wri2")
292  // {
293  // // set distance
294  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_dist"] =
295  // new Variant(rtStatus.evalData[0].minDistance);
296  // // set f_rep, damping, desired NS space
297 
298 
299  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_repF"] =
300  // new Variant(rtStatus.evalData[0].repulsiveForce);
301  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_projMass"] =
302  // new Variant(rtStatus.evalData[0].projectedMass);
303  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_locStiffness"] =
304  // new Variant(rtStatus.evalData[0].localStiffness);
305  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_dampFactor"] =
306  // new Variant(rtStatus.evalData[0].damping);
307  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_distVel"] =
308  // new Variant(rtStatus.evalData[0].distanceVelocity);
309  // if (fabs(rtStatus.evalData[0].damping + 1.0f) < tolerance)
310  // {
311  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_dampForce"] =
312  // new Variant(0.0);
313  // }
314  // else
315  // {
316  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_dampForce"] =
317  // new Variant(-1.0 * rtStatus.evalData[0].damping *
318  // rtStatus.evalData[0].distanceVelocity);
319  // }
320 
321  // datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_n_des(d)"] =
322  // new Variant(rtStatus.evalData[0].desiredNSColl);
323  // }
324  // if (rtStatus.evalData[1].nodeName == "ArmL8_Wri2")
325  // {
326  // // set distance
327  // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_dist"] =
328  // new Variant(rtStatus.evalData[1].minDistance);
329  // // set f_rep, damping, desired NS space
330 
331  // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_repF"] =
332  // new Variant(rtStatus.evalData[1].repulsiveForce);
333  // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_projMass"] =
334  // new Variant(rtStatus.evalData[1].projectedMass);
335  // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_locStiffness"] =
336  // new Variant(rtStatus.evalData[1].localStiffness);
337  // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_dampFactor"] =
338  // new Variant(rtStatus.evalData[1].damping);
339  // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_distVel"] =
340  // new Variant(rtStatus.evalData[1].distanceVelocity);
341  // if (fabs(rtStatus.evalData[1].damping + 1.0f) < tolerance)
342  // {
343  // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_dampForce"] =
344  // new Variant(0.0);
345  // }
346  // else
347  // {
348  // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_dampForce"] =
349  // new Variant(-1.0 * rtStatus.evalData[1].damping *
350  // rtStatus.evalData[1].distanceVelocity);
351  // }
352 
353  // datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_n_des(d)"] =
354  // new Variant(rtStatus.evalData[1].desiredNSColl);
355  // }
356  // if (rtStatus.evalData[2].nodeName == "ArmL7_Wri1")
357  // {
358  // // set distance
359  // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_dist"] =
360  // new Variant(rtStatus.evalData[2].minDistance);
361  // // set f_rep, damping, desired NS space
362 
363  // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_repF"] =
364  // new Variant(rtStatus.evalData[2].repulsiveForce);
365 
366  // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_projMass"] =
367  // new Variant(rtStatus.evalData[2].projectedMass);
368  // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_locStiffness"] =
369  // new Variant(rtStatus.evalData[2].localStiffness);
370  // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_dampFactor"] =
371  // new Variant(rtStatus.evalData[2].damping);
372  // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_distVel"] =
373  // new Variant(rtStatus.evalData[2].distanceVelocity);
374  // if (fabs(rtStatus.evalData[2].damping + 1.0f) < tolerance)
375  // {
376  // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_dampForce"] =
377  // new Variant(0.0);
378  // }
379  // else
380  // {
381  // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_dampForce"] =
382  // new Variant(-1.0 * rtStatus.evalData[2].damping *
383  // rtStatus.evalData[2].distanceVelocity);
384  // }
385 
386 
387  // datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_n_des(d)"] =
388  // new Variant(rtStatus.evalData[2].desiredNSColl);
389  // }
390  // if (rtStatus.evalData[3].nodeName == "ArmL8_Wri2")
391  // {
392  // // set distance
393  // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_dist"] =
394  // new Variant(rtStatus.evalData[3].minDistance);
395  // // set f_rep, damping, desired NS space
396 
397  // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_repF"] =
398  // new Variant(rtStatus.evalData[3].repulsiveForce);
399 
400  // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_projMass"] =
401  // new Variant(rtStatus.evalData[3].projectedMass);
402  // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_locStiffness"] =
403  // new Variant(rtStatus.evalData[3].localStiffness);
404  // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_dampFactor"] =
405  // new Variant(rtStatus.evalData[3].damping);
406  // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_distVel"] =
407  // new Variant(rtStatus.evalData[3].distanceVelocity);
408  // if (fabs(rtStatus.evalData[3].damping + 1.0f) < tolerance)
409  // {
410  // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_dampForce"] =
411  // new Variant(0.0);
412  // }
413  // else
414  // {
415  // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_dampForce"] =
416  // new Variant(-1.0 * rtStatus.evalData[3].damping *
417  // rtStatus.evalData[3].distanceVelocity);
418  // }
419 
420 
421  // datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_n_des(d)"] =
422  // new Variant(rtStatus.evalData[3].desiredNSColl);
423  // }
424 
425  // if (rtStatus.evalData[4].nodeName == "ArmL8_Wri2")
426  // {
427  // // set distance
428  // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_dist"] =
429  // new Variant(rtStatus.evalData[4].minDistance);
430  // // set f_rep, damping, desired NS space
431 
432  // // values have been calculated
433  // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_repF"] =
434  // new Variant(rtStatus.evalData[4].repulsiveForce);
435 
436  // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_projMass"] =
437  // new Variant(rtStatus.evalData[4].projectedMass);
438  // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_locStiffness"] =
439  // new Variant(rtStatus.evalData[4].localStiffness);
440  // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_dampFactor"] =
441  // new Variant(rtStatus.evalData[4].damping);
442  // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_distVel"] =
443  // new Variant(rtStatus.evalData[4].distanceVelocity);
444  // if (fabs(rtStatus.evalData[4].damping + 1.0f) < tolerance)
445  // {
446  // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_dampForce"] =
447  // new Variant(0.0);
448  // }
449  // else
450  // {
451  // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_dampForce"] =
452  // new Variant(-1.0 * rtStatus.evalData[4].damping *
453  // rtStatus.evalData[4].distanceVelocity);
454  // }
455 
456 
457  // datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_n_des(d)"] =
458  // new Variant(rtStatus.evalData[4].desiredNSColl);
459  // }
460  // if (rtStatus.evalData[5].nodeName == "ArmL8_Wri2")
461  // {
462  // // set distance
463  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_dist"] =
464  // new Variant(rtStatus.evalData[5].minDistance);
465 
466  // // values have been calculated
467  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_repF"] =
468  // new Variant(rtStatus.evalData[5].repulsiveForce);
469 
470  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_projMass"] =
471  // new Variant(rtStatus.evalData[5].projectedMass);
472  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_locStiffness"] =
473  // new Variant(rtStatus.evalData[5].localStiffness);
474  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_dampFactor"] =
475  // new Variant(rtStatus.evalData[5].damping);
476  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_distVel"] =
477  // new Variant(rtStatus.evalData[5].distanceVelocity);
478  // if (fabs(rtStatus.evalData[5].damping + 1.0f) < tolerance)
479  // {
480  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_dampForce"] =
481  // new Variant(0.0);
482  // }
483  // else
484  // {
485  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_dampForce"] =
486  // new Variant(-1.0 * rtStatus.evalData[5].damping *
487  // rtStatus.evalData[5].distanceVelocity);
488  // }
489 
490 
491  // datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_n_des(d)"] =
492  // new Variant(rtStatus.evalData[5].desiredNSColl);
493  // }
494  // if (rtStatus.evalData[6].nodeName == "ArmL5_Elb1")
495  // {
496  // // set distance
497  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_dist"] =
498  // new Variant(rtStatus.evalData[6].minDistance);
499 
500  // // values have been calculated
501  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_repF"] =
502  // new Variant(rtStatus.evalData[6].repulsiveForce);
503 
504  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_projMass"] =
505  // new Variant(rtStatus.evalData[6].projectedMass);
506  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_locStiffness"] =
507  // new Variant(rtStatus.evalData[6].localStiffness);
508  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_dampFactor"] =
509  // new Variant(rtStatus.evalData[6].damping);
510  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_distVel"] =
511  // new Variant(rtStatus.evalData[6].distanceVelocity);
512  // if (fabs(rtStatus.evalData[6].damping + 1.0f) < tolerance)
513  // {
514  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_dampForce"] =
515  // new Variant(0.0);
516  // }
517  // else
518  // {
519  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_dampForce"] =
520  // new Variant(-1.0 * rtStatus.evalData[6].damping *
521  // rtStatus.evalData[6].distanceVelocity);
522  // }
523 
524 
525  // datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_n_des(d)"] =
526  // new Variant(rtStatus.evalData[6].desiredNSColl);
527  // }
528  // }
529 
530  // visualize impedance force
531 
532  // layer.add(viz::Arrow("impForce")
533  // .fromTo(rtStatus.currentPose.block<3, 1>(0, 3),
534  // rtStatus.currentPose.block<3, 1>(0, 3) +
535  // rtStatus.forceImpedance.head(3) * 1000.0)
536  // .color(simox::Color::yellow()));
537  // layer.add(viz::Arrow("projImpForce")
538  // .fromTo(rtStatus.currentPose.block<3, 1>(0, 3),
539  // rtStatus.currentPose.block<3, 1>(0, 3) +
540  // rtStatus.projForceImpedance.head(3) * 1000.0)
541  // .color(simox::Color::purple()));
542 
543  arviz.commit(layer);
544 
545 
546  //double selfCollDebugTime = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
547  //datafields["selfCollDebugTime"] = new Variant(selfCollDebugTime);
548 
549  // debug value assignment torso, neck joints
550  // datafields["torsoJointValue_float"] = new Variant(rtStatus.torsoJointValuef);
551  // datafields["neck1JointValue_float"] = new Variant(rtStatus.neck1JointValuef);
552  // datafields["neck2JointValue_float"] = new Variant(rtStatus.neck2JointValuef);
553 
554  // datafields["torsoJointValue_double"] = new Variant(rtStatus.torsoJointValued);
555  // datafields["neck1JointValue_double"] = new Variant(rtStatus.neck1JointValued);
556  // datafields["neck2JointValue_double"] = new Variant(rtStatus.neck2JointValued);
557 
558  // common::debugEigenVec(
559  // datafields, "set_ActuatedJointValues", rtStatus.setActuatedJointValues.cast<float>());
560  // common::debugEigenVec(
561  // datafields, "get_ActuatedJointValues", rtStatus.getActuatedJointValues.cast<float>());
562 
563  // double limbTime = IceUtil::Time::now().toMicroSecondsDouble() - limbTimeMeasure;
564  // datafields["limbOnPublishTime"] = new Variant(limbTime);
565 
566  debugObs->setDebugChannel("CollAvoid_ImpCtrl_" + arm->nodeSetName, datafields);
567  }
568 
569  void
570  NJointTaskspaceCollisionAvoidanceImpedanceController::onPublish(
571  const SensorAndControl& sc,
572  const DebugDrawerInterfacePrx& drawer,
573  const DebugObserverInterfacePrx& debugObs)
574  {
575  NJointTaskspaceImpedanceController::onPublish(sc, drawer, debugObs);
576  if (coll)
577  {
578  for (auto& pair : coll->collLimb)
579  {
580  collLimbPublish(pair.second, debugObs);
581  }
582  }
583  }
584 
585  void
586  NJointTaskspaceCollisionAvoidanceImpedanceController::rtPreActivateController()
587  {
588  ARMARX_RT_LOGF_INFO(
589  "rt Preactivate controller NJointTaskspaceCollisionAvoidanceImpedanceController");
590  NJointTaskspaceImpedanceController::rtPreActivateController();
591  ARMARX_TRACE;
592  if (collReady.load())
593  {
594  ARMARX_CHECK_EXPRESSION(coll);
595  coll->rtPreActivate();
596  }
597  }
598 
599  void
600  NJointTaskspaceCollisionAvoidanceImpedanceController::rtPostDeactivateController()
601  {
602  NJointTaskspaceImpedanceController::rtPostDeactivateController();
603  if (collReady.load())
604  {
605  ARMARX_CHECK_EXPRESSION(coll);
606  coll->rtPostDeactivate();
607  }
608  ARMARX_RT_LOGF_INFO(
609  "post deactivate: NJointTaskspaceCollisionAvoidanceImpedanceController");
610  }
611 
612  WidgetDescription::WidgetPtr
613  NJointTaskspaceCollisionAvoidanceImpedanceController::GenerateConfigDescription(
614  const VirtualRobot::RobotPtr& robot,
615  const std::map<std::string, ConstControlDevicePtr>& controlDevices,
616  const std::map<std::string, ConstSensorDevicePtr>&)
617  {
618  using namespace armarx::WidgetDescription;
619  HBoxLayoutPtr layout = new HBoxLayout;
620 
621 
622  ::armarx::WidgetDescription::WidgetSeq widgets;
623 
624  /// select default config
625  LabelPtr label = new Label;
626  label->text = "select a controller config";
627 
628  StringComboBoxPtr cfgBox = new StringComboBox;
629  cfgBox->name = "config_box";
630  cfgBox->defaultIndex = 0;
631  cfgBox->multiSelect = false;
632 
633  cfgBox->options = std::vector<std::string>{
634  "default", "default_a7_right", "default_a7_right_zero_torque"};
635  ARMARX_TRACE;
636 
637  layout->children.emplace_back(label);
638  layout->children.emplace_back(cfgBox);
639  ARMARX_TRACE;
640 
641  layout->children.insert(layout->children.end(), widgets.begin(), widgets.end());
642  ARMARX_INFO_S << "Layout done";
643  return layout;
644  }
645 
646  NJointTaskspaceCollisionAvoidanceImpedanceController::ConfigPtrT
647  NJointTaskspaceCollisionAvoidanceImpedanceController::GenerateConfigFromVariants(
648  const StringVariantBaseMap& values)
649  {
650  auto cfgName = values.at("config_box")->getString();
651  const armarx::PackagePath configPath(
652  "armarx_control",
653  "controller_config/NJointTaskspaceCollisionAvoidanceImpedanceController/" + cfgName +
654  ".json");
655  ARMARX_INFO_S << "Loading config from " << configPath.toSystemPath();
656  ARMARX_CHECK(std::filesystem::exists(configPath.toSystemPath()));
657 
658  auto cfgDTO = armarx::readFromJson<CollisionCtrlCfg>(configPath.toSystemPath());
659 
660  ARMARX_TRACE;
661  return new ConfigurableNJointControllerConfig{cfgDTO.toAronDTO()};
662  }
663 
664 } // namespace armarx::control::njoint_controller::task_space
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition: Client.cpp:80
ARMARX_RT_LOGF_INFO
#define ARMARX_RT_LOGF_INFO(...)
Definition: ControlThreadOutputBuffer.h:339
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::GenerateConfigFromVariants
static ConfigPtrT GenerateConfigFromVariants(const StringVariantBaseMap &values)
Definition: CollisionAvoidanceImpedanceController.cpp:647
SingleTypeVariantList.h
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController
Brief description of class NJointTaskspaceImpedanceController.
Definition: ImpedanceController.h:51
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::ConfigPtrT
ConfigurableNJointControllerConfigPtr ConfigPtrT
Definition: CollisionAvoidanceImpedanceController.h:64
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController::hands
core::HandControlPtr hands
Definition: ImpedanceController.h:164
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::onPublish
void onPublish(const SensorAndControl &sc, const DebugDrawerInterfacePrx &drawer, const DebugObserverInterfacePrx &) override
Definition: CollisionAvoidanceImpedanceController.cpp:570
armarx::control::njoint_controller::task_space
Definition: AdmittanceController.cpp:37
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::updateCollisionAvoidanceConfig
void updateCollisionAvoidanceConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
NJointController interface for collision avoidance.
Definition: CollisionAvoidanceImpedanceController.cpp:144
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::getClassName
std::string getClassName(const Ice::Current &=Ice::emptyCurrent) const override
Definition: CollisionAvoidanceImpedanceController.cpp:63
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::viz::Arrow
Definition: Elements.h:198
aron_conversions.h
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::getConfig
::armarx::aron::data::dto::DictPtr getConfig(const Ice::Current &iceCurrent) override
Definition: CollisionAvoidanceImpedanceController.cpp:176
armarx::viz::Layer::add
void add(ElementT const &element)
Definition: Layer.h:29
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController::rtPostDeactivateController
void rtPostDeactivateController() override
This function is called after the controller is deactivated.
Definition: ImpedanceController.cpp:607
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::collLimbPublish
void collLimbPublish(core::CollisionAvoidanceBase::CollArmPtr &arm, const DebugObserverInterfacePrx &debugObs)
Definition: CollisionAvoidanceImpedanceController.cpp:185
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::limbRT
void limbRT(ArmPtr &arm, const double deltaT)
Definition: CollisionAvoidanceImpedanceController.cpp:69
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController::updateConfig
void updateConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
NJointController interface.
Definition: ImpedanceController.cpp:325
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:177
visionx::voxelgrid::Label
uint32_t Label
Type of an object label.
Definition: types.h:7
armarx::WidgetDescription
Definition: DefaultWidgetDescriptions.cpp:27
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController::ArmPtr
std::unique_ptr< ArmData > ArmPtr
Definition: ImpedanceController.h:97
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController::onPublish
void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition: ImpedanceController.cpp:526
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::PackagePath::toSystemPath
static std::filesystem::path toSystemPath(const data::PackagePath &pp)
Definition: PackagePath.cpp:54
armarx::control::CollisionAvoidanceControllerInterface::getCollisionAvoidanceConfig
armarx::aron::data::dto::Dict getCollisionAvoidanceConfig()
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController::limbRTSetTarget
void limbRTSetTarget(ArmPtr &arm, const Eigen::VectorXf &targetTorque)
Definition: ImpedanceController.cpp:250
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::NJointTaskspaceCollisionAvoidanceImpedanceController
NJointTaskspaceCollisionAvoidanceImpedanceController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &robot)
Definition: CollisionAvoidanceImpedanceController.cpp:47
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::GenerateConfigDescription
static WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr &, const std::map< std::string, ConstControlDevicePtr > &, const std::map< std::string, ConstSensorDevicePtr > &)
Definition: CollisionAvoidanceImpedanceController.cpp:613
armarx::control::njoint_controller::core::CollisionAvoidanceBase::CollArmPtr
std::shared_ptr< CollisionAvoidanceArmData > CollArmPtr
Definition: CollisionAvoidanceCore.h:103
ArmarXObjectScheduler.h
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: ImpedanceController.cpp:592
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController::getConfig
::armarx::aron::data::dto::DictPtr getConfig(const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition: ImpedanceController.cpp:354
armarx::control::common::debugEigenVec
void debugEigenVec(StringVariantBaseMap &datafields, const std::string &name, Eigen::VectorXf vec)
Definition: utils.cpp:125
utils.h
CycleUtil.h
armarx::RemoteGui::Client::HBoxLayout
Definition: Widgets.h:160
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: CollisionAvoidanceImpedanceController.cpp:120
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
TimeUtil.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::ArVizComponentPluginUser::arviz
armarx::viz::Client arviz
Definition: ArVizComponentPlugin.h:43
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController::limb
std::map< std::string, ArmPtr > limb
Definition: ImpedanceController.h:160
armarx::viz::ElementOps::color
DerivedT & color(Color color)
Definition: ElementOps.h:195
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::rtPostDeactivateController
void rtPostDeactivateController() override
This function is called after the controller is deactivated.
Definition: CollisionAvoidanceImpedanceController.cpp:600
IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface >
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:107
armarx::control::njoint_controller::task_space::registrationControllerNJointTaskspaceCollisionAvoidanceImpedanceController
NJointControllerRegistration< NJointTaskspaceCollisionAvoidanceImpedanceController > registrationControllerNJointTaskspaceCollisionAvoidanceImpedanceController("NJointTaskspaceCollisionAvoidanceImpedanceController")
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController::limbRTUpdateStatus
void limbRTUpdateStatus(ArmPtr &arm, const double deltaT)
-----------------------------— Real time cotnrol --------------------------------------—
Definition: ImpedanceController.cpp:231
ARMARX_INFO_S
#define ARMARX_INFO_S
Definition: Logging.h:195
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::updateConfig
void updateConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent) override
Definition: CollisionAvoidanceImpedanceController.cpp:165
armarx::viz::Client::layer
Layer layer(std::string const &name) const
Definition: Client.cpp:73
ARMARX_RT_LOGF_WARN
#define ARMARX_RT_LOGF_WARN(...)
Definition: ControlThreadOutputBuffer.h:345
armarx::PackagePath
Definition: PackagePath.h:55
armarx::viz::Layer
Definition: Layer.h:12
armarx::control::njoint_controller::task_space::NJointTaskspaceCollisionAvoidanceImpedanceController::rtPreActivateController
void rtPreActivateController() override
NJointControllerBase interface.
Definition: CollisionAvoidanceImpedanceController.cpp:586
CollisionAvoidanceImpedanceController.h
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController::userConfig
ConfigDict userConfig
Definition: ImpedanceController.h:163
armarx::control::njoint_controller::task_space::NJointTaskspaceImpedanceController::robotUnit
RobotUnitPtr robotUnit
Definition: ImpedanceController.h:162
RTUtility.h
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
PackagePath.h