25 #include <VirtualRobot/MathTools.h>
31 #include <armarx/control/common/control_law/aron/TaskspaceImpedanceControllerConfig.aron.generated.h>
37 NJointControllerRegistration<NJointTaskspaceImpedanceController>
39 "NJointTaskspaceImpedanceController");
42 const RobotUnitPtr& robotUnit,
43 const NJointControllerConfigPtr& config,
47 ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
54 VirtualRobot::RobotNodeSetPtr rtRns =
rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
57 VirtualRobot::RobotNodeSetPtr nonRtRns =
nonRtRobot->getRobotNodeSet(cfg->nodeSetName);
66 for (
size_t i = 0; i < rtRns->getSize(); ++i)
68 std::string jointName = rtRns->getNode(i)->getName();
74 auto* casted_ct = ct->
asA<ControlTarget1DoFActuatorTorque>();
78 const auto* torqueSensor = sv->
asA<SensorValue1DoFActuatorTorque>();
79 const auto* velocitySensor = sv->
asA<SensorValue1DoFActuatorVelocity>();
80 const auto* positionSensor = sv->
asA<SensorValue1DoFActuatorPosition>();
82 if (torqueSensor ==
nullptr)
86 if (velocitySensor ==
nullptr)
88 ARMARX_WARNING <<
"No velocity sensor available for " << jointName;
90 if (positionSensor ==
nullptr)
92 ARMARX_WARNING <<
"No position sensor available for " << jointName;
101 auto configData = ::armarx::fromAron<AronDTO, BO>(cfg->config);
111 if (configData.desiredNullspaceJointAngles.has_value())
126 return "NJointTaskspaceImpedanceController";
137 runTask(
"NJointTaskspaceAdmittanceAdditionalTask",
143 <<
"Create a new thread alone NJointTaskspaceAdmittanceController";
144 while (
getState() == eManagedIceObjectStarted)
150 c.waitForCycleDuration();
170 double deltaT = timeSinceLastIteration.toSecondsDouble();
181 for (
size_t i = 0; i < nDoF; ++i)
205 std::vector<float> tcpVel;
206 auto s =
controller.bufferNonRtToUser.getUpToDateReadBuffer();
207 for (
int i = 0; i <
s.currentTwist.size(); i++)
209 tcpVel.push_back(
s.currentTwist[i]);
216 const Ice::Current& iceCurrent)
218 auto configData = ::armarx::fromAron<AronDTO, BO>(dto);
235 const auto checkNonNegative = [](
const auto&
v) {
ARMARX_CHECK((
v.array() >= 0).all()); };
239 ARMARX_INFO <<
"No user defined nullspace target, reset to zero with " <<
VAROUT(nDoF);
240 configData.desiredNullspaceJointAngles = Eigen::VectorXf::Zero(nDoF);
243 checkSize(configData.desiredNullspaceJointAngles.value());
244 checkSize(configData.kdNullspace);
245 checkSize(configData.kpNullspace);
247 checkNonNegative(configData.kdNullspace);
248 checkNonNegative(configData.kpNullspace);
249 checkNonNegative(configData.kdImpedance);
250 checkNonNegative(configData.kpImpedance);
260 auto nonRtData =
controller.bufferNonRtToOnPublish.getUpToDateReadBuffer();
261 auto rtData =
controller.bufferRtToOnPublish.getUpToDateReadBuffer();
272 debugObs->setDebugChannel(
"NJointTaskspaceImpedanceController", datafields);
281 ARMARX_IMPORTANT <<
"rt preactivate controller with target pose\n\n" << currentPose;
286 c.desiredNullspaceJointAngles.value() = rns->getJointValuesEigen();
287 c.desiredPose = currentPose;
295 const auto nDoF = rns->getSize();
302 for (
size_t i = 0; i < nDoF; ++i)