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