25 #include <SimoxUtility/math/convert/mat4f_to_pos.h>
26 #include <SimoxUtility/math/convert/mat4f_to_quat.h>
27 #include <VirtualRobot/IK/DifferentialIK.h>
28 #include <VirtualRobot/Robot.h>
29 #include <VirtualRobot/RobotNodeSet.h>
40 const NJointControllerConfigPtr& config,
44 NJointTaskSpaceImpedanceControlConfigPtr cfg =
45 NJointTaskSpaceImpedanceControlConfigPtr::dynamicCast(config);
51 VirtualRobot::RobotNodeSetPtr rns =
rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
55 for (
size_t i = 0; i < rns->getSize(); ++i)
57 std::string jointName = rns->getNode(i)->getName();
58 jointNames.push_back(jointName);
63 auto casted_ct = ct->
asA<ControlTarget1DoFActuatorTorque>();
65 targets.push_back(casted_ct);
67 const SensorValue1DoFActuatorTorque* torqueSensor =
68 sv->
asA<SensorValue1DoFActuatorTorque>();
69 const SensorValue1DoFActuatorVelocity* velocitySensor =
70 sv->
asA<SensorValue1DoFActuatorVelocity>();
71 const SensorValue1DoFActuatorPosition* positionSensor =
72 sv->
asA<SensorValue1DoFActuatorPosition>();
79 torqueSensors.push_back(torqueSensor);
80 velocitySensors.push_back(velocitySensor);
81 positionSensors.push_back(positionSensor);
90 initData.
Kpos = cfg->Kpos;
91 initData.
Dpos = cfg->Dpos;
92 initData.
Kori = cfg->Kori;
93 initData.
Dori = cfg->Dori;
95 initData.
Knull = cfg->Knull;
96 initData.
Dnull = cfg->Dnull;
108 const auto& jointDesiredTorques =
112 for (
size_t i = 0; i < targets.size(); ++i)
114 targets.at(i)->torque = jointDesiredTorques(i);
115 if (!targets.at(i)->isValid())
117 targets.at(i)->torque = 0;
141 datafields[pair.first] =
new Variant(pair.second);
144 datafields[
"desiredForce_x"] =
146 datafields[
"desiredForce_y"] =
148 datafields[
"desiredForce_z"] =
151 datafields[
"tcpDesiredTorque_x"] =
153 datafields[
"tcpDesiredTorque_y"] =
155 datafields[
"tcpDesiredTorque_z"] =
161 debugObs->setDebugChannel(
"DSControllerOutput", datafields);
203 const Ice::FloatSeq& vals,
212 for (
size_t i = 0; i < 3; ++i)
218 else if (name ==
"Kori")
223 for (
size_t i = 0; i < 3; ++i)
229 else if (name ==
"Dpos")
234 for (
size_t i = 0; i < 3; ++i)
240 else if (name ==
"Dori")
245 for (
size_t i = 0; i < 3; ++i)
251 else if (name ==
"Knull")
257 for (
size_t i = 0; i < 8; ++i)
263 else if (name ==
"Dnull")
269 for (
size_t i = 0; i < 8; ++i)
280 const Eigen::VectorXf& knull,
281 const Eigen::VectorXf& dnull,
296 const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg,
302 ARMARX_CHECK_EQUAL(
static_cast<std::size_t
>(cfg.desiredJointPositions.size()), targets.size());