31 NJointControllerRegistration<NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController>
33 "NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController");
38 const NJointControllerConfigPtr& config,
48 const Ice::Current&)
const
50 return "NJointTSCollisionAvoidanceMixedImpedanceVelocityMPController";
56 const Ice::Current& iceCurrent)
60 for (
auto& pair :
limb)
62 pair.second->rtFirstRun.store(
true);
70 std::lock_guard<std::recursive_mutex> lock(
mtx_mps);
75 std::map<std::string, bool> flagMPToStop;
76 for (
auto& pair :
limb)
78 flagMPToStop[pair.first] = not
limb.at(pair.first)->controller.ftsensor.ftSafe.load();
79 if (flagMPToStop[pair.first])
81 ARMARX_INFO <<
" --- force triggered at " << pair.first;
83 pair.second->nonRTDeltaT =
84 pair.second->rtStatusInNonRT.accumulateTime - pair.second->nonRTAccumulateTime;
85 pair.second->nonRTAccumulateTime = pair.second->rtStatusInNonRT.accumulateTime;
89 for (
auto& pair :
hands->hands)
91 pair.second->nonRTDeltaT =
92 pair.second->rtsInNonRT.accumulateTime - pair.second->nonRTAccumulateTime;
93 pair.second->nonRTAccumulateTime = pair.second->rtsInNonRT.accumulateTime;
97 std::map<std::string, bool> mpRunning;
100 const auto mpNodeSet = _mp.second.mp->getNodeSetName();
101 mpRunning[_mp.second.mp->getMPName()] = _mp.second.mp->isRunning();
103 auto search = flagMPToStop.find(mpNodeSet);
104 if (_mp.second.mp->isStopRequested())
106 _mp.second.mp->stop();
107 ARMARX_INFO <<
"user requested to stop mp execution at canonical value: "
108 << _mp.second.mp->getCanonicalValue();
110 if (search != flagMPToStop.end() and flagMPToStop.at(mpNodeSet) and
111 _mp.second.mp->isRunning() and
112 (
limb.at(mpNodeSet)->controller.ftsensor.isForceGuardEnabled() or
113 limb.at(mpNodeSet)->controller.ftsensor.isTorqueGuardEnabled()))
115 _mp.second.mp->stop();
116 auto& force =
limb.at(mpNodeSet)->rtStatusInNonRT.currentForceTorque;
117 ARMARX_INFO <<
"Due to force/torque safe guard, MP " << _mp.second.mp->getMPName()
118 <<
" is stopped at " << _mp.second.mp->getCanonicalValue()
119 <<
" with current ft: " << force;
122 if (_mp.second.mp->isFinished())
126 if (_mp.second.mp->getRole() ==
"taskspace")
128 auto& arm =
limb.at(_mp.second.mp->getNodeSetName());
129 if (_mp.second.mp->isFirstRun())
134 const std::vector<double> mpStart = _mp.second.mp->getStartVec();
136 if (_mp.second.mp->getStartFromPrevTarget() or
138 arm->rtStatusInNonRT.currentPose,
141 "TSMP initial start pose",
142 arm->nonRtConfig.safeDistanceMMToGoal,
143 arm->nonRtConfig.safeRotAngleDegreeToGoal,
144 _mp.second.mp->getMPName() +
"mp_set_target"))
146 if (_mp.second.mp->getStartFromPrevTarget())
148 ARMARX_INFO <<
"User requested to start the current TS MP from the "
149 "previous target pose";
154 <<
"deviation from current pose too large, reset MP start pose";
156 _mp.second.mp->validateInitialState(
161 mp::TSMPInputPtr in = std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input);
162 in->pose = arm->rtStatusInNonRT.currentPose;
163 in->vel = arm->rtStatusInNonRT.currentTwist;
165 in->deltaT = arm->nonRTDeltaT;
167 else if (_mp.second.mp->getRole() ==
"nullspace")
169 auto& arm =
limb.at(_mp.second.mp->getNodeSetName());
170 mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input);
172 arm->rtStatusInNonRT.jointPos.size());
173 in->angleRadian = arm->rtStatusInNonRT.jointPos;
174 in->angularVel = arm->rtStatusInNonRT.qvelFiltered;
175 in->deltaT = arm->nonRTDeltaT;
177 else if (_mp.second.mp->getRole() ==
"hand")
179 auto&
hand =
hands->hands.at(_mp.second.mp->getNodeSetName());
180 mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input);
181 in->angleRadian =
hand->rtsInNonRT.jointPosition.value();
182 in->deltaT =
hand->nonRTDeltaT;
185 if (not rtTargetSafe)
198 for (
auto& _mp :
mps)
200 if (not mpRunning.at(_mp.second.mp->getMPName()))
204 for (
auto& ftGuard :
mpConfig.ftGuard)
206 if (ftGuard.mpName == _mp.second.mp->getMPName())
208 bool const forceGuard =
209 (ftGuard.force.has_value() and
210 ftGuard.force.value() >= _mp.second.mp->getCanonicalValue());
211 bool const torqueGuard =
212 (ftGuard.torque.has_value() and
213 ftGuard.torque.value() >= _mp.second.mp->getCanonicalValue());
215 auto& arm =
limb.at(_mp.second.mp->getNodeSetName());
216 bool const resetForce =
217 not arm->controller.ftsensor.isForceGuardEnabled() and forceGuard;
218 bool const resetTorque =
219 not arm->controller.ftsensor.isTorqueGuardEnabled() and torqueGuard;
220 if (resetForce or resetTorque)
222 ARMARX_INFO <<
"Triggering force torque safety guard for "
223 << arm->kinematicChainName <<
" at can value "
224 << _mp.second.mp->getCanonicalValue();
227 arm->nonRtConfig.ftConfig.enableSafeGuardForce = forceGuard;
228 arm->nonRtConfig.ftConfig.enableSafeGuardTorque = torqueGuard;
229 arm->controller.ftsensor.enableSafeGuard(resetForce, resetTorque);
233 if (_mp.second.mp->getRole() ==
"taskspace")
235 auto& arm =
limb.at(_mp.second.mp->getNodeSetName());
237 std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output);
242 arm->rtStatusInNonRT.currentPose,
246 arm->nonRtConfig.safeDistanceMMToGoal,
247 arm->nonRtConfig.safeRotAngleDegreeToGoal,
248 _mp.second.mp->getMPName() +
"mp_set_target");
249 arm->nonRtConfig.desiredPose = out->pose;
250 arm->nonRtConfig.desiredTwist = out->vel;
252 else if (_mp.second.mp->getRole() ==
"nullspace")
254 auto& arm =
limb.at(_mp.second.mp->getNodeSetName());
256 std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output);
258 arm->nonRtConfig.desiredNullspaceJointAngles.value() = out->angleRadian;
260 else if (_mp.second.mp->getRole() ==
"hand")
263 std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output);
264 auto&
hand =
hands->hands.at(_mp.second.mp->getNodeSetName());
265 mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input);
267 out->angleRadian.size());
268 hand->targetNonRT.jointPosition.value() = out->angleRadian;
270 if (mpRunning.at(_mp.second.mp->getMPName()) and _mp.second.mp->isFinished())
272 ARMARX_INFO <<
"reset buffer for nonRtConfig of " << _mp.second.mp->getMPName();
273 for (
auto& pair :
limb)
275 auto& arm = pair.second;
276 arm->bufferConfigUserToNonRt.reinitAllBuffers(arm->nonRtConfig);
277 arm->controller.ftsensor.enableSafeGuard(
true,
true);
279 for (
auto& pair :
limb)
281 userConfig.limbs.at(pair.first) = pair.second->nonRtConfig;