25 #include <VirtualRobot/MathTools.h>
31 #include <armarx/control/common/control_law/aron/KeypointControllerConfig.aron.generated.h>
37 NJointControllerRegistration<NJointKeypointsImpedanceController>
39 "NJointKeypointsImpedanceController");
42 const RobotUnitPtr& robotUnit,
43 const NJointControllerConfigPtr& config,
46 ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
54 VirtualRobot::RobotNodeSetPtr rtRns =
rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
57 VirtualRobot::RobotNodeSetPtr nonRtRns =
nonRtRobot->getRobotNodeSet(cfg->nodeSetName);
65 for (
size_t i = 0; i < rtRns->getSize(); ++i)
67 std::string jointName = rtRns->getNode(i)->getName();
73 auto casted_ct = ct->
asA<ControlTarget1DoFActuatorTorque>();
77 const SensorValue1DoFActuatorTorque* torqueSensor = sv->
asA<SensorValue1DoFActuatorTorque>();
78 const SensorValue1DoFActuatorVelocity* velocitySensor = sv->
asA<SensorValue1DoFActuatorVelocity>();
79 const SensorValue1DoFActuatorPosition* positionSensor = sv->
asA<SensorValue1DoFActuatorPosition>();
86 ARMARX_WARNING <<
"No velocity sensor available for " << jointName;
90 ARMARX_WARNING <<
"No position sensor available for " << jointName;
99 auto configData = ::armarx::fromAron<AronDTO, BO>(cfg->config);
112 return "NJointKeypointsImpedanceController";
123 runTask(
"NJointTaskspaceAdmittanceAdditionalTask",
128 ARMARX_IMPORTANT <<
"Create a new thread alone NJointKeypointsImpedanceController";
129 while (
getState() == eManagedIceObjectStarted)
135 c.waitForCycleDuration();
155 double deltaT = timeSinceLastIteration.toSecondsDouble();
167 for (
size_t i = 0; i < nDoF; ++i)
190 const Ice::Current& iceCurrent)
192 auto configData = ::armarx::fromAron<AronDTO, BO>(dto);
209 const auto checkNonNegative = [](
const auto&
v) {
ARMARX_CHECK((
v.array() >= 0).all()); };
213 ARMARX_INFO <<
"No user defined nullspace target, reset to zero with " <<
VAROUT(nDoF);
214 configData.desiredNullspaceJointAngles = Eigen::VectorXf::Zero(nDoF);
217 checkSize(configData.desiredNullspaceJointAngles.value());
218 checkSize(configData.kdNullspace);
219 checkSize(configData.kpNullspace);
221 checkNonNegative(configData.kdNullspace);
222 checkNonNegative(configData.kpNullspace);
223 checkNonNegative(configData.kdImpedance);
224 checkNonNegative(configData.kpImpedance);
242 auto nonRtData =
controller.bufferNonRtToOnPublish.getUpToDateReadBuffer();
243 auto rtData =
controller.bufferRtToOnPublish.getUpToDateReadBuffer();
265 debugObs->setDebugChannel(
"NJointKeypointsImpedanceController", datafields);
274 ARMARX_IMPORTANT <<
"rt preactivate controller with target pose\n\n" << currentPose;
280 c.desiredNullspaceJointAngles.value() = rns->getJointValuesEigen();
288 const auto nDoF = rns->getSize();
295 for (
size_t i = 0; i < nDoF; ++i)