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