Template.h
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 2025
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22
23#pragma once
24
25#include <ArmarXCore/core/time/Clock.h>
26
27#include <armarx/control/common/mp/JSMP.h>
28#include <armarx/control/common/mp/MPPool.h>
29#include <armarx/control/common/mp/TSMP.h>
30#include <armarx/control/common/utils.h>
31#include <armarx/control/njoint_controller/task_space/Base.h>
32#include <armarx/control/njoint_mp_controller/task_space/ControllerInterface.h>
33
34namespace armarx::control::njoint_mp_controller::task_space
35{
36 // using namespace armarx::control::njoint_controller::task_space;
37 // using namespace armarx::control::common;
38 namespace common = armarx::control::common;
39
40 template <typename TSControllerType, typename MPInterfaceType>
41 class TSMPController :
42 public TSControllerType,
43 public common::mp::MPPool,
44 virtual public MPInterfaceType
45 {
46 public:
47 using MPListConfig = common::mp::arondto::MPListConfig;
48
49 TSMPController(const RobotUnitPtr& robotUnit,
50 const NJointControllerConfigPtr& config,
51 const VirtualRobot::RobotPtr& /*robot*/);
52
53 void updateMPConfig(const ::armarx::aron::data::dto::DictPtr& dto,
54 const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
55 void additionalTask() override;
56 void onPublish(const SensorAndControl& /*sac*/,
57 const DebugDrawerInterfacePrx& /*interface*/,
58 const DebugObserverInterfacePrx& /*debugObs*/) override;
59
60 void rtPreActivateController() override;
61
62 struct MPDebugData
63 {
64 float canonicalValue;
65 std::map<std::string, Eigen::Matrix4f> desiredPose;
66 };
67
68 TripleBuffer<MPDebugData> bufferMPToOnPublish;
69
70 private:
71 std::map<common::mp::arondto::MPGroup, bool> groupDeviationFlag_;
72 void updateNonRTTiming();
73 std::map<std::string, bool> manageMPStopsAndGuards();
74 void updateMPInputs(common::mp::MPInputOutput& mpData);
75 void processGroupSafety(common::mp::MPInputOutput& mpData, MPDebugData& buffer);
76 void
77 processMPOutput(common::mp::MPInputOutput& mpData, MPDebugData& buffer, bool rtTargetSafe);
78 };
79
80 ///// -------------------------------- Implementation --------------------------------
81 template <typename TSControllerType, typename MPInterfaceType>
82 TSMPController<TSControllerType, MPInterfaceType>::TSMPController(
83 const RobotUnitPtr& robotUnit,
84 const NJointControllerConfigPtr& config,
85 const VirtualRobot::RobotPtr& robot) :
86 TSControllerType{robotUnit, config, robot}, common::mp::MPPool{}
87 {
88 auto data = MPDebugData();
89 data.canonicalValue = 1.0;
90 bufferMPToOnPublish.reinitAllBuffers(data);
91 }
92
93 template <typename TSControllerType, typename MPInterfaceType>
94 void
95 TSMPController<TSControllerType, MPInterfaceType>::rtPreActivateController()
96 {
97 TSControllerType::rtPreActivateController();
98
99 if (not isFinished("all") and (not this->rtGetDesiredPoseSafeStatusOnActivation() or
100 not this->rtGetDesiredPoseSafeStatus()))
101 {
102 ARMARX_WARNING << "Your MPs are not terminated before controller de-/re-activation, "
103 "and the controller is not in a safe status regarding desried pose. "
104 "either your MP has an improper desired pose, or "
105 "you interruped the controller with a different controller. "
106 "No worry, this is just a warning. I will terminate all running MPs.";
107 requestStop("all");
108 }
109 }
110
111 template <typename TSControllerType, typename MPInterfaceType>
112 void
113 TSMPController<TSControllerType, MPInterfaceType>::onPublish(
114 const SensorAndControl& sac,
115 const DebugDrawerInterfacePrx& interface,
116 const DebugObserverInterfacePrx& debugObs)
117 {
118 TSControllerType::onPublish(sac, interface, debugObs);
119
120 StringVariantBaseMap datafields;
121 auto& data = bufferMPToOnPublish.getUpToDateReadBuffer();
122 datafields["canonicalValue"] = new Variant(data.canonicalValue);
123
124 auto& desiredPoses = data.desiredPose;
125 for (const auto& [name, pose] : desiredPoses)
126 {
127 common::debugEigenPose(datafields, "mp.desired_pose_" + name, pose);
128 }
129 debugObs->setDebugChannel("MP", datafields);
130 }
131
132 template <typename TSControllerType, typename MPInterfaceType>
133 void
134 TSMPController<TSControllerType, MPInterfaceType>::updateMPConfig(
135 const ::armarx::aron::data::dto::DictPtr& dto,
136 const Ice::Current& /*iceCurrent*/)
137 {
138 if (not isFinished("all"))
139 {
140 ARMARX_WARNING
141 << "All running MPs will be terminated. This is only a temporal solution, most "
142 "likely your previous mp get into a un-safe status and in your high-level "
143 "application, you are not checking and using these status. A proper way to "
144 "terminate the mps is to get the ctrl proxy and call requestStop('all'). In "
145 "theory, controller should not handle this, but to save your time now, I will "
146 "force to remove the existing mp regardless and create new MPs on your request";
147 requestStop("all");
148 while (not isFinished("all"))
149 {
150 armarx::Clock::WaitFor(armarx::Duration::SecondsDouble(0.01));
151 }
152 ARMARX_INFO << "-- all mps stoped";
153 }
154 if (resetMPsV1(dto, this->controllableNodeSets, this->coordinator))
155 {
156 for (auto& [_, arm] : this->limb)
157 {
158 arm->rtFirstRun.store(true);
159 }
160 }
161 {
162 const std::scoped_lock lock(mtx_mps);
163 auto& buffer = bufferMPToOnPublish.getWriteBuffer();
164 buffer.desiredPose.clear();
165 ARMARX_INFO << " -- Debug message: reset MP debug buffer " << mps.size();
166 for (const auto& mp : mps)
167 {
168 if (mp.second.mp->getRole() == "taskspace")
169 {
170 buffer.desiredPose.emplace(mp.second.mp->getNodeSetName(),
171 Eigen::Matrix4f::Identity());
172 ARMARX_INFO << "reset mp debug message: " << mp.second.mp->getNodeSetName()
173 << " to identity";
174 }
175 }
176 bufferMPToOnPublish.reinitAllBuffers(buffer);
177 }
178 }
179
180 // template <typename TSControllerType, typename MPInterfaceType>
181 // void
182 // TSMPController<TSControllerType, MPInterfaceType>::additionalTask()
183 // {
184 // const std::lock_guard<std::recursive_mutex> lock(mtx_mps);
185 // auto [rtTargetSafe, ftSafe] = this->additionalTaskUpdateStatus();
186 // if (this->coordinator)
187 // {
188 // this->coordinator->updateNonRt();
189 // }
190
191 // /// get MP debug buffer
192 // auto& buffer = bufferMPToOnPublish.getWriteBuffer();
193
194 // /// when ft guard is enabled and rt is not safe due to ft trigger, then we stop the corresponding mp
195 // std::map<std::string, bool> flagMPToStop;
196 // for (auto& [name, arm] : this->limb)
197 // {
198 // flagMPToStop[name] = not this->limb.at(name)->controller.ftsensor.ftSafe.load();
199 // arm->nonRTDeltaT = arm->rtStatusInNonRT.accumulateTime - arm->nonRTAccumulateTime;
200 // arm->nonRTAccumulateTime = arm->rtStatusInNonRT.accumulateTime;
201 // }
202 // if (this->hands)
203 // {
204 // for (auto& [_, hand] : this->hands->hands)
205 // {
206 // hand->nonRTDeltaT = hand->rtsInNonRT.accumulateTime - hand->nonRTAccumulateTime;
207 // hand->nonRTAccumulateTime = hand->rtsInNonRT.accumulateTime;
208 // }
209 // }
210
211 // std::map<std::string, bool> mpRunning;
212 // for (auto& mp : mps)
213 // {
214 // const auto mpNodeSet = mp.second.mp->getNodeSetName();
215 // mpRunning[mp.second.mp->getMPName()] = mp.second.mp->isRunning();
216
217 // bool forceGuardEnabled = false;
218 // bool torqueGuardEnabled = false;
219 // for (auto& ftGuard : mpConfig.ftGuard)
220 // {
221 // if (ftGuard.mpName == mp.second.mp->getMPName())
222 // {
223 // forceGuardEnabled =
224 // (ftGuard.force.has_value() and
225 // ftGuard.force.value() >= mp.second.mp->getCanonicalValue());
226 // torqueGuardEnabled =
227 // (ftGuard.torque.has_value() and
228 // ftGuard.torque.value() >= mp.second.mp->getCanonicalValue());
229 // }
230 // }
231 // /// --------------------------------------------------------------------------------///
232 // /// check if MP need to be stopped
233 // /// --------------------------------------------------------------------------------///
234 // /// TODO add loop the check if MP for the sync mode coordinator need to stop
235 // auto search = flagMPToStop.find(mpNodeSet);
236 // if (mp.second.mp->isStopRequested())
237 // {
238 // mp.second.mp->stop();
239 // ARMARX_INFO << "user requested to stop mp execution at canonical value: "
240 // << mp.second.mp->getCanonicalValue();
241 // }
242 // if (search != flagMPToStop.end() and flagMPToStop.at(mpNodeSet) and
243 // mp.second.mp->isRunning() and not mp.second.mp->isFirstRun() and
244 // ((forceGuardEnabled and
245 // this->limb.at(mpNodeSet)->controller.ftsensor.isForceGuardEnabled()) or
246 // (torqueGuardEnabled and
247 // this->limb.at(mpNodeSet)->controller.ftsensor.isTorqueGuardEnabled())))
248 // {
249 // mp.second.mp->stop();
250 // ARMARX_INFO << "Due to force/torque safe guard, MP " << mp.second.mp->getMPName()
251 // << " is stopped at " << mp.second.mp->getCanonicalValue();
252 // }
253
254 // if (mp.second.mp->isFinished())
255 // {
256 // continue;
257 // }
258
259 // /// --------------------------------------------------------------------------------///
260 // /// MPs are not stoped, continue execution
261 // /// --------------------------------------------------------------------------------///
262 // if (mp.second.mp->getRole() == "taskspace")
263 // {
264 // auto& arm = this->limb.at(mp.second.mp->getNodeSetName());
265 // if (mp.second.mp->isFirstRun())
266 // {
267 // ARMARX_INFO << "checking TSMP initial status ...";
268 // /// Here we check if the TSMP start pose is too far away from the current pose, if so, we would
269 // /// prefer to reset the mp start from the current pose.
270 // const std::vector<double> mpStart = mp.second.mp->getStartVec();
271 // const auto mpStartPose = common::dVecToMat4(mpStart);
272 // if (mp.second.mp->getStartFromPrevTarget() or
273 // common::detectAndReportPoseDeviationWarning(
274 // arm->rtStatusInNonRT.currentPose,
275 // mpStartPose,
276 // "current pose",
277 // "initial start pose",
278 // arm->nonRtConfig.safeDistanceMMToGoal,
279 // arm->nonRtConfig.safeRotAngleDegreeToGoal,
280 // "Additional task for task space MPs: " + mp.second.mp->getMPName() +
281 // " detects that the "))
282 // {
283 // if (mp.second.mp->getStartFromPrevTarget())
284 // {
285 // ARMARX_INFO << "User requested to start the current TS MP from the "
286 // "previous target pose";
287 // }
288 // else
289 // {
290 // ARMARX_INFO
291 // << "deviation from current pose too large, reset MP start pose";
292 // }
293 // mp.second.mp->validateInitialState(
294 // common::mat4ToDVec(arm->rtStatusInNonRT.desiredPose));
295 // }
296 // ARMARX_INFO << "done";
297 // }
298 // common::mp::TSMPInputPtr in =
299 // std::dynamic_pointer_cast<common::mp::TSMPInput>(mp.second.input);
300 // in->pose = arm->rtStatusInNonRT.currentPose;
301 // in->vel = arm->rtStatusInNonRT.currentTwist;
302 // // in->deltaT = arm->rtStatusInNonRT.deltaT;
303 // in->deltaT = arm->nonRTDeltaT;
304 // }
305 // else if (mp.second.mp->getRole() == "nullspace")
306 // {
307 // auto& arm = this->limb.at(mp.second.mp->getNodeSetName());
308 // common::mp::JSMPInputPtr in =
309 // std::dynamic_pointer_cast<common::mp::JSMPInput>(mp.second.input);
310 // ARMARX_CHECK_EQUAL(arm->rtStatusInNonRT.nDoF,
311 // arm->rtStatusInNonRT.jointPosition.size());
312 // in->angleRadian = arm->rtStatusInNonRT.jointPosition;
313 // in->angularVel = arm->rtStatusInNonRT.qvelFiltered;
314 // in->deltaT = arm->nonRTDeltaT;
315 // }
316 // else if (mp.second.mp->getRole() == "hand")
317 // {
318 // auto& hand = this->hands->hands.at(mp.second.mp->getNodeSetName());
319 // common::mp::JSMPInputPtr in =
320 // std::dynamic_pointer_cast<common::mp::JSMPInput>(mp.second.input);
321 // in->angleRadian = hand->rtsInNonRT.jointPosition.value();
322 // in->deltaT = hand->nonRTDeltaT;
323 // }
324 // else if (mp.second.mp->getRole() == "object")
325 // {
326 // if (not this->coordinator)
327 // {
328 // ARMARX_WARNING << "when using the object for MP, the coordinator must be first "
329 // "instantiated";
330 // return;
331 // }
332 // // auto& nodesetList = coord->getNodesetList();
333 // // auto& rtsL = limb.at(coord->getLeftNodesetName())->rtStatusInNonRT;
334 // // auto& rtsR = limb.at(coord->getRightNodesetName())->rtStatusInNonRT;
335
336 // // setPos(currentPose, 0.5f * (getPos(rtsL.currentPose) + getPos(rtsR.currentPose)));
337 // // setOri(currentPose, getOri(rtsL.currentPose));
338 // const std::scoped_lock lock_vo_non_rt(this->coordinator->mtx_data_non_rt);
339 // common::mp::TSMPInputPtr in =
340 // std::dynamic_pointer_cast<common::mp::TSMPInput>(mp.second.input);
341 // in->pose = this->coordinator->dataNonRt.currentPose;
342 // in->vel = this->coordinator->dataNonRt.currentVel;
343 // in->deltaT = this->coordinator->dataNonRt.deltaT;
344 // }
345 // }
346
347
348 // /// --------------------------------------------------------------------------------///
349 // /// Handle RT unsafe logging due to descripancy in desired and current pose
350 // /// --------------------------------------------------------------------------------///
351 // if (not rtTargetSafe)
352 // {
353 // this->handleRTNotSafeInNonRT();
354
355 // // return;
356 // // Make sure that you do not call additionalTaskSetTarget before return;
357 // // since we overwrite the arm->nonRtConfig in additionalTaskUpdateStatus() with user config, which does not
358 // // align with the up-to-date mp status. userConfig is only updated to align with MP status everytime a MP
359 // // finishes execution. Only then, user can use this controller as if no mp is there.
360 // }
361
362 // /// -----------------------------------------------------------------------------------///
363 // /// The MPs will envolve one step forward if it is the first run or rtTargetSafe is true.
364 // /// -----------------------------------------------------------------------------------///
365 // runMPs(rtTargetSafe);
366
367 // /// --------------------------------------------------------------------------------///
368 // /// set mp target to nonRT config data structure
369 // /// --------------------------------------------------------------------------------///
370 // for (auto& mp : mps)
371 // {
372
373 // /// --------------------------------------------------------------------------------///
374 // /// MP stops running, then skip
375 // /// --------------------------------------------------------------------------------///
376 // if (not mpRunning.at(mp.second.mp->getMPName()))
377 // {
378 // continue;
379 // }
380
381 // /// --------------------------------------------------------------------------------///
382 // /// Check if force/torque guard should be activated
383 // /// --------------------------------------------------------------------------------///
384 // for (auto& ftGuard : mpConfig.ftGuard)
385 // {
386 // if (ftGuard.mpName == mp.second.mp->getMPName())
387 // {
388 // bool const forceGuard =
389 // (ftGuard.force.has_value() and
390 // ftGuard.force.value() >= mp.second.mp->getCanonicalValue());
391 // bool const torqueGuard =
392 // (ftGuard.torque.has_value() and
393 // ftGuard.torque.value() >= mp.second.mp->getCanonicalValue());
394
395 // auto& arm = this->limb.at(mp.second.mp->getNodeSetName());
396 // bool const resetForce =
397 // not arm->controller.ftsensor.isForceGuardEnabled() and forceGuard;
398 // bool const resetTorque =
399 // not arm->controller.ftsensor.isTorqueGuardEnabled() and torqueGuard;
400 // if (resetForce or resetTorque)
401 // {
402 // ARMARX_INFO << "Triggering force torque safety guard for "
403 // << arm->kinematicChainName << " at can value "
404 // << mp.second.mp->getCanonicalValue();
405 // }
406
407 // arm->nonRtConfig.ftConfig.enableSafeGuardForce = forceGuard;
408 // arm->nonRtConfig.ftConfig.enableSafeGuardTorque = torqueGuard;
409 // arm->controller.ftsensor.enableSafeGuard(resetForce, resetTorque);
410 // }
411 // }
412
413 // /// --------------------------------------------------------------------------------///
414 // /// Fetch MP desired pose/values and set to the buffer going to RT thread
415 // /// --------------------------------------------------------------------------------///
416 // bool desiredValueSafe = true;
417 // if (mp.second.mp->getRole() == "taskspace")
418 // {
419 // buffer.canonicalValue = mp.second.mp->getCanonicalValue();
420
421 // auto& arm = this->limb.at(mp.second.mp->getNodeSetName());
422 // common::mp::TSMPOutputPtr out =
423 // std::dynamic_pointer_cast<common::mp::TSMPOutput>(mp.second.output);
424
425 // /// Note that here we only report warning but still set target to RT, we rely on MP phase stop and
426 // /// RT safety mechanism to stop or pause the action if needed
427 // desiredValueSafe = common::detectAndReportPoseDeviationWarning(
428 // arm->rtStatusInNonRT.currentPose,
429 // out->pose,
430 // "current pose",
431 // "desired pose",
432 // arm->nonRtConfig.safeDistanceMMToGoal,
433 // arm->nonRtConfig.safeRotAngleDegreeToGoal,
434 // "Additional task for task space MPs: " + mp.second.mp->getMPName() +
435 // " detects that the ");
436 // arm->nonRtConfig.desiredPose = out->pose;
437 // arm->nonRtConfig.desiredTwist = out->vel;
438
439 // /// add mp desired pose for debug plot
440 // buffer.desiredPose.at(mp.second.mp->getNodeSetName()) = out->pose;
441 // }
442 // else if (mp.second.mp->getRole() == "object")
443 // {
444 // buffer.canonicalValue = mp.second.mp->getCanonicalValue();
445 // if (not this->coordinator)
446 // {
447 // ARMARX_WARNING << "when using the object for MP, the coordinator must be first "
448 // "instantiated";
449 // return;
450 // }
451 // common::mp::TSMPOutputPtr out =
452 // std::dynamic_pointer_cast<common::mp::TSMPOutput>(mp.second.output);
453 // this->coordinator->cfgInNonRt.desiredPose = out->pose;
454 // }
455 // else if (mp.second.mp->getRole() == "nullspace")
456 // {
457 // auto& arm = this->limb.at(mp.second.mp->getNodeSetName());
458 // common::mp::JSMPOutputPtr out =
459 // std::dynamic_pointer_cast<common::mp::JSMPOutput>(mp.second.output);
460 // ARMARX_CHECK_EQUAL(arm->rtStatusInNonRT.nDoF, out->angleRadian.size());
461 // arm->nonRtConfig.desiredNullspaceJointAngles.value() = out->angleRadian;
462 // }
463 // else if (mp.second.mp->getRole() == "hand")
464 // {
465 // common::mp::JSMPOutputPtr out =
466 // std::dynamic_pointer_cast<common::mp::JSMPOutput>(mp.second.output);
467 // auto& hand = this->hands->hands.at(mp.second.mp->getNodeSetName());
468 // common::mp::JSMPInputPtr in =
469 // std::dynamic_pointer_cast<common::mp::JSMPInput>(mp.second.input);
470 // ARMARX_CHECK_EQUAL(hand->targetNonRT.jointPosition.value().size(),
471 // out->angleRadian.size());
472 // hand->targetNonRT.jointPosition.value() = out->angleRadian;
473 // }
474
475 // /// --------------------------------------------------------------------------------///
476 // /// Upon MP stop, reset the nonRTConfig and User buffer, so that controller behaves
477 // /// Like a normal NJointController without MP.
478 // /// This is only a temporal solution, it has to be re-considered, e.g.,
479 // /// - 1) whether to stop the whole group (task space/nullspace/hand) at the same time
480 // /// - 2) all allow user to stop only partially;
481 // /// E.g., In dishwasher skill, you want to trigger the stop at last 10%, but still
482 // /// want to execute the rest of the finger motions to close and grasp the handle.
483 // /// Otherwise, you will need to add a grasp motion after the skill for 1)
484 // /// --------------------------------------------------------------------------------///
485 // if (mpRunning.at(mp.second.mp->getMPName()) and mp.second.mp->isFinished())
486 // {
487 // ARMARX_INFO << "-- reset buffer for nonRtConfig of " << mp.second.mp->getMPName();
488 // for (auto& [_, arm] : this->limb)
489 // {
490 // if (not rtTargetSafe or desiredValueSafe)
491 // {
492 // ARMARX_INFO << "-- discard the last MP desired pose, use the "
493 // "existing RT desried pose for "
494 // << arm->kinematicChainName << ": \n"
495 // << arm->rtStatusInNonRT.desiredPose;
496 // arm->nonRtConfig.desiredPose = arm->rtStatusInNonRT.desiredPose;
497 // }
498 // arm->bufferConfigUserToNonRt.reinitAllBuffers(arm->nonRtConfig);
499 // }
500 // for (auto& [name, arm] : this->limb)
501 // {
502 // this->userConfig.limbs.at(name) = arm->nonRtConfig;
503 // }
504 // if (this->hands)
505 // {
506 // this->hands->reinitBuffer(this->userConfig.hands);
507 // }
508 // if (this->coordinator)
509 // {
510 // this->coordinator->resetBufferUserCfgAfterMP();
511 // }
512 // }
513 // }
514
515 // /// -----------------------------------------------------------------------------------///
516 // /// Handle coordinator and set/commit non RT targets/buffer
517 // /// -----------------------------------------------------------------------------------///
518 // // sync mode is handled directly by "object" type of mp
519 // if (this->coordinator)
520 // {
521 // this->coordinator->commitNonRt();
522 // }
523
524 // this->additionalTaskSetTarget();
525
526 // bufferMPToOnPublish.commitWrite();
527 // }
528
529 template <typename TSControllerType, typename MPInterfaceType>
530 void
531 TSMPController<TSControllerType, MPInterfaceType>::additionalTask()
532 {
533 const std::lock_guard<std::recursive_mutex> lock(mtx_mps);
534
535 /// 1. Update Status from RT and user configuration update
536 auto [rtTargetSafe, ftSafe] = this->additionalTaskUpdateStatus();
537 if (this->coordinator)
538 {
539 this->coordinator->updateNonRt();
540 }
541
542 ARMARX_TRACE;
543 /// 2. Update Timing calculations
544 this->updateNonRTTiming();
545
546 /// 3. Check Stop Conditions
547 std::map<std::string, bool> mpRunningStatus = this->manageMPStopsAndGuards();
548
549 /// 4. Prepare Inputs
550 for (auto& mp : mps)
551 {
552 if (mpRunningStatus[mp.second.mp->getMPName()] && !mp.second.mp->isFinished())
553 {
554 this->updateMPInputs(mp.second);
555 }
556 }
557
558 /// 5. Handle RT unsafe logging due to descripancy in desired and current pose
559 if (!rtTargetSafe)
560 {
561 this->handleRTNotSafeInNonRT();
562 }
563
564 /// 6. The MPs will envolve one step forward if it is the first run or rtTargetSafe is true
565 runMPs(rtTargetSafe);
566
567 /// 7. Set mp target to nonRT config data structure
568 auto& buffer = bufferMPToOnPublish.getWriteBuffer();
569
570 groupDeviationFlag_.clear();
571 for (auto& mpWrapper : mps)
572 {
573 auto& mpData = mpWrapper.second;
574
575 // Only process if the MP is marked as running
576 if (mpRunningStatus[mpData.mp->getMPName()])
577 {
578 this->processGroupSafety(mpData, buffer);
579 }
580 }
581
582 for (auto& mpWrapper : mps)
583 {
584 auto& mpData = mpWrapper.second;
585
586 // Only process if the MP is marked as running
587 if (mpRunningStatus[mpData.mp->getMPName()])
588 {
589 this->processMPOutput(mpData, buffer, rtTargetSafe);
590 }
591 }
592
593 /// 8. Handle coordinator and set/commit non RT targets/buffer
594 if (this->coordinator)
595 {
596 this->coordinator->commitNonRt();
597 }
598
599 this->additionalTaskSetTarget();
600 bufferMPToOnPublish.commitWrite();
601 }
602
603 template <typename TSControllerType, typename MPInterfaceType>
604 void
605 TSMPController<TSControllerType, MPInterfaceType>::updateNonRTTiming()
606 {
607 for (auto& [name, arm] : this->limb)
608 {
609 arm->nonRTDeltaT = arm->rtStatusInNonRT.accumulateTime - arm->nonRTAccumulateTime;
610 arm->nonRTAccumulateTime = arm->rtStatusInNonRT.accumulateTime;
611 }
612 if (this->hands)
613 {
614 for (auto& [_, hand] : this->hands->hands)
615 {
616 hand->nonRTDeltaT = hand->rtsInNonRT.accumulateTime - hand->nonRTAccumulateTime;
617 hand->nonRTAccumulateTime = hand->rtsInNonRT.accumulateTime;
618 }
619 }
620 }
621
622 template <typename TSControllerType, typename MPInterfaceType>
623 std::map<std::string, bool>
624 TSMPController<TSControllerType, MPInterfaceType>::manageMPStopsAndGuards()
625 {
626 std::map<std::string, bool> mpRunning;
627
628 // Check global FT safety per limb
629 std::map<std::string, bool> limbStopFlags;
630 for (auto& [name, arm] : this->limb)
631 {
632 // limbStopFlags[name] = not arm->controller.isForceTorqueSafe();
633 limbStopFlags[name] = not arm->controller.ftsensor.ftSafe.load();
634 }
635
636 for (auto& mpWrapper : mps)
637 {
638 auto& mp = mpWrapper.second.mp;
639 const auto mpName = mp->getMPName();
640 const auto nodeSet = mp->getNodeSetName();
641
642 mpRunning[mpName] = mp->isRunning();
643
644 /// 1. Check User Stop Request
645 if (mp->isStopRequested())
646 {
647 mp->stop();
648 ARMARX_INFO << "user requested to stop mp execution at canonical value: "
649 << mp->getCanonicalValue();
650 continue;
651 }
652
653 /// 2. Check Force/Torque Guards configuration
654 bool forceGuardEnabled = false;
655 bool torqueGuardEnabled = false;
656 for (auto& ftGuard : mpConfig.ftGuard)
657 {
658 if (ftGuard.mpName == mpName)
659 {
660 if (ftGuard.force.has_value() &&
661 ftGuard.force.value() >= mp->getCanonicalValue())
662 {
663 forceGuardEnabled = true;
664 }
665 if (ftGuard.torque.has_value() &&
666 ftGuard.torque.value() >= mp->getCanonicalValue())
667 {
668 torqueGuardEnabled = true;
669 }
670 }
671 }
672
673 /// 3. Stop if Global FT Unsafe OR Specific Guard Triggered
674 bool globalFtUnsafe = (limbStopFlags.count(nodeSet) && limbStopFlags.at(nodeSet));
675
676 if (globalFtUnsafe && mp->isRunning() && !mp->isFirstRun() &&
677 ((forceGuardEnabled &&
678 this->limb.at(nodeSet)->controller.ftsensor.isForceGuardEnabled()) ||
679 (torqueGuardEnabled &&
680 this->limb.at(nodeSet)->controller.ftsensor.isTorqueGuardEnabled())))
681 {
682 mp->stop();
683 ARMARX_INFO << "Due to force/torque safe guard, MP " << mpName << " is stopped at "
684 << mp->getCanonicalValue();
685
686 if (this->coordinatorEnabled)
687 {
688 for (auto& mpWrapper2 : mps)
689 {
690 auto& mp2 = mpWrapper2.second.mp;
691 ARMARX_INFO << "Stopped MP " << mp2->getMPName() << " as coordination is enabled";
692 }
693 }
694 else
695 {
696 for (auto& mpWrapper2 : mps)
697 {
698 auto& mp2 = mpWrapper2.second.mp;
699 if (mp->getGroup() == mp2->getGroup())
700 {
701 mp2->stop();
702 ARMARX_INFO << "Stopped MP " << mp2->getMPName() << " part of MP group " << mp->getGroupName();
703 }
704 }
705 }
706 }
707 }
708 return mpRunning;
709 }
710
711 template <typename TSControllerType, typename MPInterfaceType>
712 void
713 TSMPController<TSControllerType, MPInterfaceType>::updateMPInputs(
714 common::mp::MPInputOutput& mpData)
715 {
716 auto& mp = mpData.mp;
717 const std::string& role = mp->getRole();
718 const std::string& nodeSet = mp->getNodeSetName();
719
720 ARMARX_TRACE;
721 if (role == "taskspace")
722 {
723 auto& arm = this->limb.at(nodeSet);
724
725 // Handle First Run Logic (Deviation Checks)
726 if (mp->isFirstRun())
727 {
728 ARMARX_INFO << "checking TSMP initial status ...";
729 const std::vector<double> mpStart = mp->getStartVec();
730 const auto mpStartPose = common::dVecToMat4(mpStart);
731
732 bool deviation = common::detectAndReportPoseDeviationWarning(
733 arm->rtStatusInNonRT.currentPose,
734 mpStartPose,
735 "current pose",
736 "initial start pose",
737 arm->nonRtConfig.safeDistanceMMToGoal,
738 arm->nonRtConfig.safeRotAngleDegreeToGoal,
739 "Additional task for task space MPs: " + mp->getMPName() +
740 " detects that the ");
741
742 if (mp->getStartFromPrevTarget() or deviation)
743 {
744 if (mp->getStartFromPrevTarget())
745 {
746 ARMARX_INFO << "User requested to start the current TS MP from the "
747 "previous target pose";
748 }
749 else
750 {
751 ARMARX_INFO << "deviation from current pose too large, reset MP start pose";
752 }
753
754 mp->validateInitialState(common::mat4ToDVec(arm->rtStatusInNonRT.desiredPose));
755 }
756 ARMARX_INFO << "done";
757 }
758
759 common::mp::TSMPInputPtr in =
760 std::dynamic_pointer_cast<common::mp::TSMPInput>(mpData.input);
761 in->pose = arm->rtStatusInNonRT.currentPose;
762 in->vel = arm->rtStatusInNonRT.currentTwist;
763 in->deltaT = arm->nonRTDeltaT;
764 return;
765 }
766
767 if (role == "nullspace")
768 {
769 auto& arm = this->limb.at(nodeSet);
770 common::mp::JSMPInputPtr in =
771 std::dynamic_pointer_cast<common::mp::JSMPInput>(mpData.input);
772 ARMARX_CHECK_EQUAL(arm->rtStatusInNonRT.nDoF,
773 arm->rtStatusInNonRT.jointPosition.size());
774 in->angleRadian = arm->rtStatusInNonRT.jointPosition;
775 in->angularVel = arm->rtStatusInNonRT.qvelFiltered;
776 in->deltaT = arm->nonRTDeltaT;
777 return;
778 }
779
780 if (role == "hand")
781 {
782 auto& hand = this->hands->hands.at(nodeSet);
783 common::mp::JSMPInputPtr in =
784 std::dynamic_pointer_cast<common::mp::JSMPInput>(mpData.input);
785 if (hand->targetNonRT.jointPosition.has_value())
786 {
787 in->angleRadian = hand->targetNonRT.jointPosition.value();
788 in->deltaT = hand->nonRTDeltaT;
789 // ARMARX_INFO << "__ " << VAROUT(in->angleRadian) << VAROUT(in->deltaT);
790 }
791 return;
792 }
793
794 if (role == "object")
795 {
796 if (not this->coordinator)
797 {
798 ARMARX_WARNING
799 << "when using the object for MP, the coordinator must be first instantiated";
800 return;
801 }
802 const std::scoped_lock lock_vo_non_rt(this->coordinator->mtx_data_non_rt);
803 common::mp::TSMPInputPtr in =
804 std::dynamic_pointer_cast<common::mp::TSMPInput>(mpData.input);
805 in->pose = this->coordinator->dataNonRt.currentPose;
806 in->vel = this->coordinator->dataNonRt.currentVel;
807 in->deltaT = this->coordinator->dataNonRt.deltaT;
808 return;
809 }
810
811 ARMARX_WARNING << "Your mp role " << role << " is not supported";
812 }
813
814 template <typename TSControllerType, typename MPInterfaceType>
815 void
816 TSMPController<TSControllerType, MPInterfaceType>::processGroupSafety(
817 common::mp::MPInputOutput& mpData, // Pass the specific MP data structure
818 MPDebugData& buffer)
819 {
820 auto& mp = mpData.mp;
821 /// --------------------------------------------------------------------------------
822 /// A. Check/Trigger Force-Torque Safety Guards for this MP
823 /// --------------------------------------------------------------------------------
824 for (auto& ftGuard : mpConfig.ftGuard)
825 {
826 if (ftGuard.mpName == mp->getMPName())
827 {
828 bool const forceGuard =
829 (ftGuard.force.has_value() && ftGuard.force.value() >= mp->getCanonicalValue());
830 bool const torqueGuard = (ftGuard.torque.has_value() &&
831 ftGuard.torque.value() >= mp->getCanonicalValue());
832
833 auto& arm = this->limb.at(mp->getNodeSetName());
834
835 bool const resetForce =
836 not arm->controller.ftsensor.isForceGuardEnabled() and forceGuard;
837 bool const resetTorque =
838 not arm->controller.ftsensor.isTorqueGuardEnabled() and torqueGuard;
839 if (resetForce or resetTorque)
840 {
841 ARMARX_INFO << "Triggering force torque safety guard for "
842 << arm->kinematicChainName << " at can value "
843 << mp->getCanonicalValue();
844 }
845
846 arm->nonRtConfig.ftConfig.enableSafeGuardForce = forceGuard;
847 arm->nonRtConfig.ftConfig.enableSafeGuardTorque = torqueGuard;
848 arm->controller.ftsensor.enableSafeGuard(resetForce, resetTorque);
849 }
850 }
851
852 /// --------------------------------------------------------------------------------
853 /// B. Fetch MP desired pose/values and set to the buffer going to RT thread
854 /// --------------------------------------------------------------------------------
855 const std::string& role = mp->getRole();
856 const std::string& nodeSetName = mp->getNodeSetName();
857
858 if (role == "taskspace")
859 {
860 buffer.canonicalValue = mp->getCanonicalValue();
861 auto& arm = this->limb.at(nodeSetName);
862 auto out = std::dynamic_pointer_cast<common::mp::TSMPOutput>(mpData.output);
863
864 bool flag = common::detectAndReportPoseDeviationWarning(
865 arm->rtStatusInNonRT.currentPose,
866 out->pose,
867 "current pose",
868 "desired pose",
869 arm->nonRtConfig.safeDistanceMMToGoal,
870 arm->nonRtConfig.safeRotAngleDegreeToGoal,
871 "Additional task for task space MPs: " + mp->getMPName() + " detects that the ");
872 groupDeviationFlag_.emplace(mp->getGroup(), flag);
873
874 arm->nonRtConfig.desiredPose = out->pose;
875 arm->nonRtConfig.desiredTwist = out->vel;
876 buffer.desiredPose.at(nodeSetName) = out->pose;
877 }
878 }
879
880 template <typename TSControllerType, typename MPInterfaceType>
881 void
882 TSMPController<TSControllerType, MPInterfaceType>::processMPOutput(
883 common::mp::MPInputOutput& mpData, // Pass the specific MP data structure
884 MPDebugData& buffer,
885 bool rtTargetSafe)
886 {
887 auto& mp = mpData.mp;
888 bool deviationFlag = true;
889 if (groupDeviationFlag_.count(mp->getGroup()))
890 {
891 deviationFlag = groupDeviationFlag_.at(mp->getGroup());
892 }
893 const std::string& role = mp->getRole();
894 const std::string& nodeSetName = mp->getNodeSetName();
895
896 if (role == "object")
897 {
898 buffer.canonicalValue = mp->getCanonicalValue();
899 if (this->coordinator)
900 {
901 auto out = std::dynamic_pointer_cast<common::mp::TSMPOutput>(mpData.output);
902 this->coordinator->cfgInNonRt.desiredPose = out->pose;
903 }
904 }
905 else if (role == "nullspace")
906 {
907 auto& arm = this->limb.at(nodeSetName);
908 auto out = std::dynamic_pointer_cast<common::mp::JSMPOutput>(mpData.output);
909 ARMARX_CHECK_EQUAL(arm->rtStatusInNonRT.nDoF, out->angleRadian.size());
910 if (not deviationFlag)
911 {
912 arm->nonRtConfig.desiredNullspaceJointAngles.value() = out->angleRadian;
913 }
914 }
915 else if (role == "hand")
916 {
917 auto& hand = this->hands->hands.at(nodeSetName);
918 auto out = std::dynamic_pointer_cast<common::mp::JSMPOutput>(mpData.output);
919 ARMARX_CHECK_EQUAL(hand->targetNonRT.jointPosition.value().size(),
920 out->angleRadian.size());
921 if (not deviationFlag)
922 {
923 hand->targetNonRT.jointPosition.value() = out->angleRadian;
924 }
925 // ARMARX_INFO << "__ mp set target " << nodeSetName << ": "
926 // << VAROUT(mp->getCanonicalValue()) << VAROUT(out->angleRadian);
927 }
928
929 /// ----------------------------------------------------------------------------------
930 /// C. Upon MP stop, reset the nonRTConfig and User buffer, so that controller behaves
931 /// Like a normal NJointController without MP.
932 /// This is only a temporal solution, it has to be re-considered, e.g.,
933 /// - 1) whether to stop the whole group (task space/nullspace/hand) at the same time
934 /// - 2) all allow user to stop only partially;
935 /// E.g., In dishwasher skill, you want to trigger the stop at last 10%, but still
936 /// want to execute the rest of the finger motions to close and grasp the handle.
937 /// Otherwise, you will need to add a grasp motion after the skill for 1)
938 /// ----------------------------------------------------------------------------------
939 if (mp->isFinished())
940 {
941 ARMARX_INFO << "-- reset buffer for nonRtConfig of " << mp->getMPName();
942
943 // Reset all limbs to safe state or existing RT state
944 for (auto& [name, arm] : this->limb)
945 {
946 // If RT was unsafe OR the MP calculated an unsafe deviation
947 if (!rtTargetSafe || deviationFlag) // Note: Logic preserved from your update
948 {
949 ARMARX_INFO << "-- discard the last MP desired pose, use the existing RT "
950 "desired pose for "
951 << arm->kinematicChainName << ": \n"
952 << arm->rtStatusInNonRT.desiredPose;
953 arm->nonRtConfig.desiredPose = arm->rtStatusInNonRT.desiredPose;
954 }
955 arm->bufferConfigUserToNonRt.reinitAllBuffers(arm->nonRtConfig);
956 }
957
958 // Sync user config
959 for (auto& [name, arm] : this->limb)
960 {
961 this->userConfig.limbs.at(name) = arm->nonRtConfig;
962 }
963
964 // Sync hands
965 if (this->hands)
966 {
967 this->hands->reinitBuffer(this->userConfig.hands);
968 }
969
970 // Sync coordinator
971 if (this->coordinator)
972 {
973 this->coordinator->resetBufferUserCfgAfterMP();
974 }
975 }
976 }
977} // namespace armarx::control::njoint_mp_controller::task_space
978
979//#include "Template_impl.h"
armarx::Clock::WaitFor
static void WaitFor(const Duration &duration)
Wait for a certain duration on the virtual clock.
Definition Clock.cpp:99
armarx::Duration::SecondsDouble
static Duration SecondsDouble(double seconds)
Constructs a duration in seconds.
Definition Duration.cpp:78
armarx::TripleBuffer
A simple triple buffer for lockfree comunication between a single writer and a single reader.
Definition TripleBuffer.h:72
armarx::Variant
The Variant class is described here: Variants.
Definition Variant.h:224
armarx::control::common::mp::MPPool::resetMPsV1
bool resetMPsV1(const ::armarx::aron::data::dto::DictPtr &dto, const std::map< std::string, VirtualRobot::RobotNodeSetPtr > &rnsMap, const std::shared_ptr< common::coordination::Coordination > &coordinator)
Definition MPPool.cpp:604
armarx::control::common::mp::MPPool::mtx_mps
std::recursive_mutex mtx_mps
Definition MPPool.h:147
armarx::control::common::mp::MPPool::isFinished
bool isFinished(const std::string &mpName="all", const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition MPPool.cpp:386
armarx::control::common::mp::MPPool::mps
std::map< std::string, MPInputOutput > mps
Definition MPPool.h:146
armarx::control::common::mp::MPPool::requestStop
void requestStop(const std::string &mpName="all", const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition MPPool.cpp:237
armarx::control::njoint_controller::task_space::NJointTaskspaceController< law::TSAdmController >::robotUnit
RobotUnitPtr robotUnit
Definition Base.h:217
armarx::control::njoint_mp_controller::task_space::TSMPController::additionalTask
void additionalTask() override
Definition Template.h:531
armarx::control::njoint_mp_controller::task_space::TSMPController::onPublish
void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition Template.h:113
armarx::control::njoint_mp_controller::task_space::TSMPController::TSMPController
TSMPController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition Template.h:82
armarx::control::njoint_mp_controller::task_space::TSMPController::updateMPConfig
void updateMPConfig(const ::armarx::aron::data::dto::DictPtr &dto, const Ice::Current &iceCurrent=Ice::emptyCurrent) override
Definition Template.h:134
armarx::control::njoint_mp_controller::task_space::TSMPController::MPListConfig
common::mp::arondto::MPListConfig MPListConfig
Definition Template.h:47
armarx::control::njoint_mp_controller::task_space::TSMPController::bufferMPToOnPublish
TripleBuffer< MPDebugData > bufferMPToOnPublish
Definition Template.h:68
armarx::control::njoint_mp_controller::task_space::TSMPController::rtPreActivateController
void rtPreActivateController() override
Definition Template.h:95
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition ExpressionException.h:130
ARMARX_INFO
#define ARMARX_INFO
The normal logging level.
Definition Logging.h:181
ARMARX_WARNING
#define ARMARX_WARNING
The logging level for unexpected behaviour, but not a serious problem.
Definition Logging.h:193
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition Bus.h:19
armarx::control::common::mp
This file is part of ArmarX.
Definition aron_conversions.cpp:332
armarx::control::common::mp::TSMPInputPtr
std::shared_ptr< TSMPInput > TSMPInputPtr
Definition TSMP.h:46
armarx::control::common::mp::JSMPInputPtr
std::shared_ptr< JSMPInput > JSMPInputPtr
Definition JSMP.h:47
armarx::control::common
This file is part of ArmarX.
Definition aron_conversions.cpp:30
armarx::control::common::detectAndReportPoseDeviationWarning
bool detectAndReportPoseDeviationWarning(const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2, const std::string &namePose1, const std::string &namePose2, float positionThrMM, float angleThrDeg, const std::string &who)
Definition utils.cpp:773
armarx::control::common::dVecToMat4
Eigen::Matrix4f dVecToMat4(const DVec &dvec)
create Eigen:Matrix4f from 7D double vector.
Definition utils.cpp:325
armarx::control::common::debugEigenPose
void debugEigenPose(StringVariantBaseMap &datafields, const std::string &name, Eigen::Matrix4f pose)
Definition utils.cpp:140
armarx::control::common::mat4ToDVec
DVec mat4ToDVec(const Eigen::Matrix4f &mat)
convert Eigen:Matrix4f to 7D double vector.
Definition utils.cpp:359
armarx::control::njoint_mp_controller::task_space
This file is part of ArmarX.
Definition AdmittanceController.cpp:30
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::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
Base.h
armarx::control::njoint_mp_controller::task_space::TSMPController::MPDebugData
Definition Template.h:63
armarx::control::njoint_mp_controller::task_space::TSMPController::MPDebugData::desiredPose
std::map< std::string, Eigen::Matrix4f > desiredPose
Definition Template.h:65
armarx::control::njoint_mp_controller::task_space::TSMPController::MPDebugData::canonicalValue
float canonicalValue
Definition Template.h:64
ARMARX_TRACE
#define ARMARX_TRACE
Definition trace.h:77