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