33 const RobotUnitPtr& robotUnit,
34 const NJointControllerConfigPtr& config,
39 ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
40 auto configData = ::armarx::fromAron<AronDTO, BO>(cfg->config);
41 validateConfigData(configData);
42 bufferUserToAdditionalTask.reinitAllBuffers(configData);
43 bufferUserToRt.reinitAllBuffers(configData);
45 createMPs(configData.mpConfig);
46 isMPReady.store(
true);
53 return "NJointTSImpedanceMPController";
62 isMPReady.store(
false);
65 isMPReady.store(
true);
67 ARMARX_IMPORTANT <<
"reconfigure MP: reinitialize the mp input output, as well as the rt related buffer values";
72 if (_mp.second.mp->getClassName() ==
"TSMP")
74 ARMARX_IMPORTANT <<
"init input output data for " << _mp.second.mp->getMPName();
75 std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input)->pose = currentPose;
76 std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output)->pose = currentPose;
78 else if (_mp.second.mp->getClassName() ==
"JSMP") {
79 ARMARX_IMPORTANT <<
"init input output data for " << _mp.second.mp->getMPName();
80 std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angleRadian = rns->getJointValuesEigen();
101 if (_mp.second.mp->getClassName() ==
"TSMP")
103 mp::TSMPInputPtr in = std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input);
104 in->pose =
controller.tcp->getPoseInRootFrame();
105 in->vel =
controller.bufferNonRtToOnPublish.getWriteBuffer().currentTwist;
106 in->deltaT = rt2Add.deltaT;
108 else if (_mp.second.mp->getClassName() ==
"JSMP")
110 mp::JSMPInputPtr in = std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input);
111 for (
size_t i = 0; i < rt2Add.jointPosition.size(); i++)
113 in->angleRadian(i) = rt2Add.jointPosition[i];
114 in->angularVel(i) = rt2Add.jointVelocity[i];
116 in->deltaT = rt2Add.deltaT;
123 if (_mp.second.mp->getClassName() ==
"TSMP")
125 mp::TSMPOutputPtr out = std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output);
126 u2RtConfig.desiredPose = out->pose;
127 u2RtConfig.desiredTwist = out->vel;
128 u2AddConfig.desiredPose = out->pose;
129 u2AddConfig.desiredTwist = out->vel;
131 else if (_mp.second.mp->getClassName() ==
"JSMP")
133 mp::JSMPOutputPtr out = std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output);
134 u2RtConfig.desiredNullspaceJointAngles.value() = out->angleRadian;
135 u2AddConfig.desiredNullspaceJointAngles.value() = out->angleRadian;
141 controller.updateControlStatus(u2AddConfig, rt2Add);
146 ARMARX_IMPORTANT <<
"rt pre activate: reinitialize the mp input output, as well as the rt related buffer values";
152 if (_mp.second.mp->getClassName() ==
"TSMP")
154 ARMARX_IMPORTANT <<
"init input output data for " << _mp.second.mp->getMPName();
155 std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input)->pose = currentPose;
156 std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output)->pose = currentPose;
158 else if (_mp.second.mp->getClassName() ==
"JSMP")
160 ARMARX_IMPORTANT <<
"init input output data for " << _mp.second.mp->getMPName();
161 std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angleRadian = rns->getJointValuesEigen();
162 std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angularVel =
163 Eigen::VectorXf::Zero(rns->getJointValues().size());
164 std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angleRadian =
165 rns->getJointValuesEigen();
166 std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angularVel =
167 Eigen::VectorXf::Zero(rns->getJointValues().size());