25 #include <VirtualRobot/MathTools.h>
36 NJointControllerRegistration<NJointTaskspaceMixedImpedanceVelocityController>
38 "NJointTaskspaceMixedImpedanceVelocityController");
46 arm->kinematicChainName = nodeSetName;
47 VirtualRobot::RobotNodeSetPtr rtRns =
48 rtGetRobot()->getRobotNodeSet(arm->kinematicChainName);
49 VirtualRobot::RobotNodeSetPtr nonRtRns =
50 nonRtRobotPtr->getRobotNodeSet(arm->kinematicChainName);
53 ARMARX_IMPORTANT <<
"Creating Taskspace Impedance controller with kinematic chain: "
54 << arm->kinematicChainName <<
" with num of joints: (RT robot) "
55 << rtRns->getSize() <<
", and (non-RT robot) " << nonRtRns->getSize();
57 std::vector<size_t> jointIDTorqueMode;
58 std::vector<size_t> jointIDVelocityMode;
59 arm->jointNames.clear();
60 for (
size_t i = 0; i < rtRns->getSize(); ++i)
62 std::string jointName = rtRns->getNode(i)->getName();
63 arm->jointNames.push_back(jointName);
65 bool foundControlDevice =
false;
67 cfg.jointNameListTorque.begin(), cfg.jointNameListTorque.end(), jointName);
68 if (it != cfg.jointNameListTorque.end())
72 auto* casted_ct = ct->
asA<ControlTarget1DoFActuatorTorque>();
74 arm->targetsTorque.push_back(casted_ct);
75 jointIDTorqueMode.push_back(i);
76 foundControlDevice =
true;
80 cfg.jointNameListVelocity.begin(), cfg.jointNameListVelocity.end(), jointName);
81 if (it != cfg.jointNameListVelocity.end())
85 auto* casted_ct = ct->
asA<ControlTarget1DoFActuatorVelocity>();
87 arm->targetsVel.push_back(casted_ct);
88 jointIDVelocityMode.push_back(i);
89 foundControlDevice =
true;
91 if (not foundControlDevice)
96 ARMARX_ERROR <<
"Does not found valid control device for joint name: " << jointName
97 <<
"Please check torque controlled joints: [" << namesTorque
98 <<
"] and velocity controlled joints: [" << namesVelocity <<
"].";
103 const auto* torqueSensor = sv->
asA<SensorValue1DoFActuatorTorque>();
104 const auto* velocitySensor = sv->
asA<SensorValue1DoFActuatorVelocity>();
105 const auto* positionSensor = sv->
asA<SensorValue1DoFActuatorPosition>();
107 if (torqueSensor ==
nullptr)
111 if (velocitySensor ==
nullptr)
113 ARMARX_WARNING <<
"No velocity sensor available for " << jointName;
115 if (positionSensor ==
nullptr)
117 ARMARX_WARNING <<
"No position sensor available for " << jointName;
120 arm->sensorDevices.torqueSensors.push_back(torqueSensor);
121 arm->sensorDevices.velocitySensors.push_back(velocitySensor);
122 arm->sensorDevices.positionSensors.push_back(positionSensor);
124 ARMARX_DEBUG <<
"Number of torque controlled joints " << jointIDTorqueMode.size();
125 ARMARX_DEBUG <<
"Number of velocity controlled joints " << jointIDVelocityMode.size();
127 jointIDTorqueMode.size() + jointIDVelocityMode.size());
129 arm->controller.initialize(nonRtRns, rtRns, jointIDTorqueMode, jointIDVelocityMode);
130 arm->controller.ftsensor.initialize(rtRns,
robotUnit, cfg.ftConfig);
134 arm->nonRtConfig = cfg;
139 const NJointControllerConfigPtr& config,
145 ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
155 limb.emplace(pair.first, std::make_unique<ArmData>());
163 return "NJointTaskspaceMixedImpedanceVelocityController";
169 std::string taskName =
getClassName() +
"AdditionalTask";
181 while (
getState() == eManagedIceObjectStarted)
187 c.waitForCycleDuration();
195 arm->nonRtConfig = arm->bufferConfigUserToNonRt.getUpToDateReadBuffer();
196 arm->bufferConfigNonRtToRt.getWriteBuffer() = arm->nonRtConfig;
197 arm->bufferConfigNonRtToRt.commitWrite();
205 for (
auto& pair :
limb)
208 pair.second->rtStatusInNonRT =
209 pair.second->bufferRtStatusToNonRt.getUpToDateReadBuffer();
210 rtSafe = rtSafe and pair.second->rtStatusInNonRT.rtSafe;
214 for (
auto& pair :
limb)
217 "new target \n\n [%.2f, %.2f, %.2f]\n\nis too far away from the "
218 "current pose\n\n [%.2f, %.2f, %.2f]",
219 pair.second->rtStatusInNonRT.desiredPose(0, 3),
220 pair.second->rtStatusInNonRT.desiredPose(1, 3),
221 pair.second->rtStatusInNonRT.desiredPose(2, 3),
222 pair.second->rtStatusInNonRT.currentPose(0, 3),
223 pair.second->rtStatusInNonRT.currentPose(1, 3),
224 pair.second->rtStatusInNonRT.currentPose(2, 3))
225 .deactivateSpam(1.0);
233 double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
234 arm->rtConfig = arm->bufferConfigNonRtToRt.getUpToDateReadBuffer();
235 double time_update_non_rt_buffer =
236 IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
238 if (arm->rtFirstRun.load())
240 arm->controller.ftsensor.reset();
244 arm->controller.updateFT(arm->rtConfig.ftConfig, arm->rtStatus);
246 double time_update_ft = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
248 size_t nDoF = arm->sensorDevices.positionSensors.size();
249 double time_update_size = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
250 for (
size_t i = 0; i < nDoF; ++i)
252 arm->rtStatus.jointPosition[i] = arm->sensorDevices.positionSensors[i]->position;
253 arm->rtStatus.jointVelocity[i] = arm->sensorDevices.velocitySensors[i]->velocity;
254 arm->rtStatus.jointTorque[i] = arm->sensorDevices.torqueSensors[i]->torque;
257 double time_update_js = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
258 arm->rtStatus.deltaT = deltaT;
259 double time_update_rt_status = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
261 arm->bufferRtStatusToNonRt.getWriteBuffer() = arm->rtStatus;
262 arm->bufferRtStatusToNonRt.commitWrite();
263 arm->bufferRtStatusToUser.getWriteBuffer() = arm->rtStatus;
264 arm->bufferRtStatusToUser.commitWrite();
265 double time_write_rt_buffer = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
267 arm->controller.run(arm->rtConfig, arm->rtStatus);
268 double time_run_rt = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
270 for (
unsigned int i = 0; i < arm->rtStatus.nDoFTorque; i++)
272 auto jointIdx = arm->controller.jointIDTorqueMode[i];
273 arm->targetsTorque.at(i)->torque = arm->rtStatus.desiredJointTorques(jointIdx);
274 if (!arm->targetsTorque.at(i)->isValid())
276 arm->targetsTorque.at(i)->torque = 0;
279 for (
unsigned int i = 0; i < arm->rtStatus.nDoFVelocity; i++)
281 auto jointIdx = arm->controller.jointIDVelocityMode[i];
282 arm->targetsVel.at(i)->velocity = arm->rtStatus.desiredJointVelocity(jointIdx);
284 if (!arm->targetsVel.at(i)->isValid())
286 arm->targetsVel.at(i)->velocity = 0;
289 arm->bufferRtStatusToOnPublish.getWriteBuffer() = arm->rtStatus;
290 arm->bufferRtStatusToOnPublish.commitWrite();
292 arm->bufferConfigRtToOnPublish.getWriteBuffer() = arm->rtConfig;
293 arm->bufferConfigRtToOnPublish.commitWrite();
295 arm->bufferConfigRtToUser.getWriteBuffer() = arm->rtConfig;
296 arm->bufferConfigRtToUser.commitWrite();
297 double time_rt_status_buffer = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
298 timeMeasure = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
300 if (timeMeasure > 100)
303 "update_non_rt_buffer: %.2f\n"
305 "update_size: %.2f\n"
307 "update_rt_status: %.2f\n"
308 "write_rt_buffer: %.2f\n"
310 "rt_status_buffer: %.2f\n"
312 time_update_non_rt_buffer,
313 time_update_ft - time_update_non_rt_buffer,
314 time_update_size - time_update_ft,
315 time_update_js - time_update_size,
316 time_update_rt_status - time_update_non_rt_buffer,
317 time_write_rt_buffer - time_update_rt_status,
318 time_run_rt - time_write_rt_buffer,
319 time_rt_status_buffer - time_run_rt,
323 if (arm->rtFirstRun.load())
325 arm->rtFirstRun.store(
false);
326 arm->rtReady.store(
true);
335 double deltaT = timeSinceLastIteration.toSecondsDouble();
336 for (
auto& pair :
limb)
338 limbRT(pair.second, deltaT);
344 const Ice::Current& iceCurrent)
346 std::vector<float> tcpVel;
347 auto& arm =
limb.at(rns);
348 auto s = arm->bufferRtStatusToUser.getUpToDateReadBuffer();
349 for (
int i = 0; i <
s.currentTwist.size(); i++)
351 tcpVel.push_back(
s.currentTwist[i]);
359 const Ice::Current& iceCurrent)
365 limb.at(pair.first)->bufferConfigUserToNonRt.getWriteBuffer() = pair.second;
366 limb.at(pair.first)->bufferConfigUserToNonRt.commitWrite();
372 const std::string& nodeSetName,
373 const bool forceGuard,
374 const bool torqueGuard,
375 const Ice::Current& iceCurrent)
380 it->second.ftConfig.enableSafeGuardForce = forceGuard;
381 it->second.ftConfig.enableSafeGuardTorque = torqueGuard;
382 limb.at(nodeSetName)->controller.ftsensor.resetSafeGuardOffset();
384 <<
"reset safe guard with force torque sensors: safe? "
385 <<
limb.at(nodeSetName)->controller.ftsensor.isSafe(it->second.ftConfig);
386 limb.at(nodeSetName)->bufferConfigUserToNonRt.getWriteBuffer() = it->second;
387 limb.at(nodeSetName)->bufferConfigUserToNonRt.commitWrite();
392 <<
" found in the controllers.";
398 const std::string& nodeSetName,
399 const Ice::Current& iceCurrent)
404 return limb.at(nodeSetName)->controller.ftsensor.ftSafe.load();
409 <<
" found in the controllers.";
418 const Ice::Current& iceCurrent)
422 for (
size_t i = 0; i < 4; ++i)
424 for (
int j = 0; j < 4; ++j)
426 pair.second.desiredPose(i, j) = targetPoseMap.at(pair.first)[i][j];
429 if (targetNullspaceMap.at(pair.first).size() > 0)
431 const size_t nDoF = pair.second.desiredNullspaceJointAngles.value().size();
433 <<
"the joint numbers does not match";
434 for (
size_t i = 0; i < nDoF; ++i)
436 pair.second.desiredNullspaceJointAngles.value()(i) =
437 targetNullspaceMap.at(pair.first)[i];
440 limb.at(pair.first)->bufferConfigUserToNonRt.getWriteBuffer() = pair.second;
441 limb.at(pair.first)->bufferConfigUserToNonRt.commitWrite();
449 for (
auto& pair :
limb)
452 pair.second->bufferConfigRtToUser.getUpToDateReadBuffer();
461 const auto nDoF = arm->controller.numOfJoints;
464 const auto checkNonNegative = [](
const auto&
v) {
ARMARX_CHECK((
v.array() >= 0).all()); };
466 if (!configData.desiredNullspaceJointAngles.has_value())
470 ARMARX_INFO <<
"No user defined nullspace target, reset to zero with "
472 configData.desiredNullspaceJointAngles = Eigen::VectorXf::Zero(nDoF);
473 arm->reInitPreActivate.store(
true);
478 arm->bufferConfigRtToUser.getUpToDateReadBuffer().desiredNullspaceJointAngles;
480 <<
"Controller is active, but no user defined nullspace target. \n"
481 "You should first get up-to-date config of this controller and then update "
483 " auto cfg = ctrl->getConfig(); \n"
484 " cfg.desiredPose = xxx;\n"
485 " ctrl->updateConfig(cfg);\n"
486 "Now, I decide by myself to use the existing values of nullspace target\n"
487 << currentValue.value();
488 configData.desiredNullspaceJointAngles = currentValue;
492 checkSize(configData.desiredNullspaceJointAngles.value());
493 checkSize(configData.kdNullspaceTorque);
494 checkSize(configData.kpNullspaceTorque);
495 checkSize(configData.kdNullspaceVel);
496 checkSize(configData.kpNullspaceVel);
498 checkNonNegative(configData.kdNullspaceTorque);
499 checkNonNegative(configData.kpNullspaceTorque);
500 checkNonNegative(configData.kpNullspaceVel);
501 checkNonNegative(configData.kdNullspaceVel);
502 checkNonNegative(configData.kdImpedance);
503 checkNonNegative(configData.kpImpedance);
512 auto rtData = arm->bufferConfigRtToOnPublish.getUpToDateReadBuffer();
513 auto rtStatus = arm->bufferRtStatusToOnPublish.getUpToDateReadBuffer();
527 debugObs->setDebugChannel(
getClassName() +
"_" + arm->kinematicChainName, datafields);
536 for (
auto& pair :
limb)
545 for (
auto& pair :
limb)
547 pair.second->rtReady.store(
false);
554 VirtualRobot::RobotNodeSetPtr rns =
rtGetRobot()->getRobotNodeSet(arm->kinematicChainName);
556 ARMARX_IMPORTANT <<
"rt preactivate controller with target pose\n\n" << currentPose;
558 if (arm->reInitPreActivate.load())
560 arm->rtConfig.desiredNullspaceJointAngles.value() = rns->getJointValuesEigen();
561 arm->rtConfig.desiredPose = currentPose;
563 arm->bufferConfigUserToNonRt.reinitAllBuffers(arm->rtConfig);
564 arm->reInitPreActivate.store(
false);
567 arm->nonRtConfig = arm->rtConfig;
568 arm->bufferConfigNonRtToRt.reinitAllBuffers(arm->rtConfig);
569 arm->bufferConfigRtToOnPublish.reinitAllBuffers(arm->rtConfig);
570 arm->bufferConfigRtToUser.reinitAllBuffers(arm->rtConfig);
573 const auto nDoF = rns->getSize();
574 const auto nDoFTorque = arm->controller.jointIDTorqueMode.size();
575 const auto nDoFVelocity = arm->controller.jointIDVelocityMode.size();
577 arm->rtStatus.nDoFTorque = nDoFTorque;
578 arm->rtStatus.nDoFVelocity = nDoFVelocity;
579 arm->rtStatus.numJoints = nDoF;
580 arm->rtStatus.desiredJointTorques.setZero(nDoF);
581 arm->rtStatus.desiredJointVelocity.setZero(nDoF);
582 arm->rtStatus.forceImpedance.setZero();
583 arm->rtStatus.cartesianVelTarget.setZero();
584 arm->rtStatus.nullspaceTorque.setZero(nDoF);
585 arm->rtStatus.nullspaceVelocity.setZero(nDoF);
587 arm->rtStatus.jointPosition.resize(nDoF, 0.);
588 arm->rtStatus.jointVelocity.resize(nDoF, 0.);
589 arm->rtStatus.jointTorque.resize(nDoF, 0.);
590 for (
size_t i = 0; i < nDoF; ++i)
592 arm->rtStatus.jointPosition[i] = arm->sensorDevices.positionSensors[i]->position;
593 arm->rtStatus.jointVelocity[i] = arm->sensorDevices.velocitySensors[i]->velocity;
594 arm->rtStatus.jointTorque[i] = arm->sensorDevices.torqueSensors[i]->torque;
597 arm->rtStatus.qpos = rns->getJointValuesEigen();
598 arm->rtStatus.qvel.setZero(nDoF);
599 arm->rtStatus.qvelFiltered.setZero(nDoF);
601 arm->rtStatus.currentPose = currentPose;
602 arm->rtStatus.desiredPose = currentPose;
603 arm->rtStatus.currentTwist.setZero();
604 arm->rtStatus.poseDiffMatImp.setZero();
605 arm->rtStatus.poseErrorImp.setZero();
606 arm->rtStatus.jacobi.setZero(6, nDoF);
607 arm->rtStatus.jtpinv.setZero(6, nDoF);
609 arm->rtStatus.deltaT = 0.0;
610 arm->rtStatus.rtSafe =
false;
612 arm->rtStatus.currentForceTorque.setZero();
614 arm->rtStatusInNonRT = arm->rtStatus;
615 arm->bufferRtStatusToOnPublish.reinitAllBuffers(arm->rtStatus);
616 arm->bufferRtStatusToNonRt.reinitAllBuffers(arm->rtStatus);
617 arm->bufferRtStatusToUser.reinitAllBuffers(arm->rtStatus);
620 arm->controller.preactivateInit(rns);
626 for (
auto& pair :
limb)
628 ARMARX_INFO <<
"rtPreActivateController for " << pair.first;
630 userConfig.cfg.at(pair.first) = pair.second->rtConfig;