28 NJointControllerRegistration<NJointTSMixedImpedanceVelocityMPController>
30 "NJointTSMixedImpedanceVelocityMPController");
33 const RobotUnitPtr& robotUnit,
34 const NJointControllerConfigPtr& config,
38 mpConfig = MPListConfig();
45 return "NJointTSMixedImpedanceVelocityMPController";
51 const Ice::Current& iceCurrent)
55 auto configData = MPListConfig::FromAron(dto);
57 while (additionalTaskRunning.load())
61 for (
const auto& _mp : configData.mpList)
63 if (
limb.find(_mp.nodeSetName) ==
limb.end())
65 ARMARX_WARNING <<
"You have to make sure the RobotNodeSet " << _mp.nodeSetName
66 <<
" in your MP config "
67 <<
"corresponds to one of the RobotNodeSet in the controllers";
76 ARMARX_INFO <<
"-- successfully updated MP config.";
81 "If you want to force reset, please call \n\n"
82 " ctrl->stop('all') \n\n"
83 "before you continue";
101 additionalTaskRunning.store(
true);
103 for (
auto& _mp :
mps)
105 auto& arm =
limb.at(_mp.second.mp->getNodeSetName());
106 arm->nonRtConfig = arm->bufferConfigUserToNonRt.getUpToDateReadBuffer();
107 arm->rtStatusInNonRT = arm->bufferRtStatusToNonRt.getUpToDateReadBuffer();
108 rtSafe = rtSafe and arm->rtStatusInNonRT.rtSafe;
110 if (_mp.second.mp->getClassName() ==
"TSMP")
113 std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input);
114 in->pose = arm->rtStatusInNonRT.currentPose;
115 in->vel = arm->rtStatusInNonRT.currentTwist;
116 in->deltaT = arm->rtStatusInNonRT.deltaT;
118 else if (_mp.second.mp->getClassName() ==
"JSMP")
121 std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input);
123 arm->rtStatusInNonRT.qpos.size());
124 in->angleRadian = arm->rtStatusInNonRT.qpos;
125 in->angularVel = arm->rtStatusInNonRT.qvelFiltered;
126 in->deltaT = arm->rtStatusInNonRT.deltaT;
135 for (
auto& _mp :
mps)
136 for (
auto& pair :
limb)
138 if (not pair.second->rtReady)
140 const auto& desiredPose =
141 pair.second->rtStatusInNonRT.desiredPose.topRightCorner<3, 1>();
142 const auto& currentPose =
143 pair.second->rtStatusInNonRT.currentPose.topRightCorner<3, 1>();
145 if ((desiredPose - currentPose).norm() >
146 pair.second->nonRtConfig.safeDistanceToGoal)
149 "new target \n\n [%.2f, %.2f, %.2f]\n\nis too far away from the "
150 "current pose\n\n [%.2f, %.2f, %.2f]",
151 pair.second->rtStatusInNonRT.desiredPose(0, 3),
152 pair.second->rtStatusInNonRT.desiredPose(1, 3),
153 pair.second->rtStatusInNonRT.desiredPose(2, 3),
154 pair.second->rtStatusInNonRT.currentPose(0, 3),
155 pair.second->rtStatusInNonRT.currentPose(1, 3),
156 pair.second->rtStatusInNonRT.currentPose(2, 3))
157 .deactivateSpam(1.0);
162 for (
auto& _mp :
mps)
164 auto& arm =
limb.at(_mp.second.mp->getNodeSetName());
165 if (_mp.second.mp->getClassName() ==
"TSMP")
168 std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output);
169 arm->nonRtConfig.desiredPose = out->pose;
170 arm->nonRtConfig.desiredTwist = out->vel;
172 else if (_mp.second.mp->getClassName() ==
"JSMP")
175 std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output);
177 arm->nonRtConfig.desiredNullspaceJointAngles.value() = out->angleRadian;
180 for (
auto& pair :
limb)
187 additionalTaskRunning.store(
false);
194 arm->bufferConfigNonRtToRt.getWriteBuffer() = arm->nonRtConfig;
195 arm->bufferConfigNonRtToRt.commitWrite();
201 ARMARX_INFO <<
"-- reconfigure MP: reinitialize the mp input output, as well as the rt "
202 "related buffer values";
204 for (
auto& _mp :
mps)
206 const auto& nodeSetName = _mp.second.mp->getNodeSetName();
208 VirtualRobot::RobotNodeSetPtr rns =
rtGetRobot()->getRobotNodeSet(nodeSetName);
211 if (_mp.second.mp->getClassName() ==
"TSMP")
214 << _mp.second.mp->getMPName();
215 std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input)->pose = currentPose;
216 std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input)->vel.setZero();
217 std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output)->pose = currentPose;
218 std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output)->vel.setZero();
220 else if (_mp.second.mp->getClassName() ==
"JSMP")
223 << _mp.second.mp->getMPName() <<
" with "
224 << rns->getJointValues().size() <<
" joints";
225 std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angleRadian =
226 rns->getJointValuesEigen();
227 std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angularVel =
228 Eigen::VectorXf::Zero(rns->getJointValues().size());
229 std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angleRadian =
230 rns->getJointValuesEigen();
231 std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angularVel =
232 Eigen::VectorXf::Zero(rns->getJointValues().size());