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