25 #include <VirtualRobot/MathTools.h>
26 #include <VirtualRobot/Nodes/RobotNode.h>
27 #include <VirtualRobot/RobotNodeSet.h>
47 NJointControllerRegistration<NJointTaskspaceMixedImpedanceVelocityController>
49 "NJointTaskspaceMixedImpedanceVelocityController");
57 arm->kinematicChainName = nodeSetName;
58 VirtualRobot::RobotNodeSetPtr rtRns =
59 rtGetRobot()->getRobotNodeSet(arm->kinematicChainName);
60 VirtualRobot::RobotNodeSetPtr nonRtRns =
61 nonRtRobotPtr->getRobotNodeSet(arm->kinematicChainName);
64 ARMARX_IMPORTANT <<
"Creating Taskspace Impedance controller with kinematic chain: "
65 << arm->kinematicChainName <<
" with num of joints: (RT robot) "
66 << rtRns->getSize() <<
", and (non-RT robot) " << nonRtRns->getSize();
68 std::vector<size_t> jointIDTorqueMode;
69 std::vector<size_t> jointIDVelocityMode;
70 arm->jointNames.clear();
71 for (
size_t i = 0; i < rtRns->getSize(); ++i)
73 std::string jointName = rtRns->getNode(i)->getName();
74 arm->jointNames.push_back(jointName);
76 bool foundControlDevice =
false;
78 cfg.jointNameListTorque.begin(), cfg.jointNameListTorque.end(), jointName);
79 if (it != cfg.jointNameListTorque.end())
83 auto* casted_ct = ct->
asA<ControlTarget1DoFActuatorTorque>();
85 arm->targetsTorque.push_back(casted_ct);
86 jointIDTorqueMode.push_back(i);
87 foundControlDevice =
true;
91 cfg.jointNameListVelocity.begin(), cfg.jointNameListVelocity.end(), jointName);
92 if (it != cfg.jointNameListVelocity.end())
96 auto* casted_ct = ct->
asA<ControlTarget1DoFActuatorVelocity>();
98 arm->targetsVel.push_back(casted_ct);
99 jointIDVelocityMode.push_back(i);
100 foundControlDevice =
true;
102 if (not foundControlDevice)
107 ARMARX_ERROR <<
"Does not found valid control device for joint name: " << jointName
108 <<
"Please check torque controlled joints: [" << namesTorque
109 <<
"] and velocity controlled joints: [" << namesVelocity <<
"].";
114 arm->sensorDevices.append(sv, jointName);
116 ARMARX_DEBUG <<
"Number of torque controlled joints " << jointIDTorqueMode.size();
117 ARMARX_DEBUG <<
"Number of velocity controlled joints " << jointIDVelocityMode.size();
119 jointIDTorqueMode.size() + jointIDVelocityMode.size());
121 arm->controller.initialize(nonRtRns, rtRns, jointIDTorqueMode, jointIDVelocityMode);
122 arm->controller.ftsensor.initialize(rtRns,
robotUnit, cfg.ftConfig);
126 arm->nonRtConfig = cfg;
127 arm->rtStatus.reset(rtRns->getSize(), jointIDTorqueMode.size(), jointIDVelocityMode.size());
132 const NJointControllerConfigPtr& config,
138 ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
148 limb.emplace(pair.first, std::make_unique<ArmData>());
158 for (
auto& jointName :
hands->hands.at(pair.first)->jointNames)
160 hands->appendDevice(pair.first,
166 <<
hands->hands.at(pair.first)->kinematicChainName;
175 return "NJointTaskspaceMixedImpedanceVelocityController";
181 std::string taskName =
getClassName() +
"AdditionalTask";
193 while (
getState() == eManagedIceObjectStarted)
199 c.waitForCycleDuration();
207 arm->bufferConfigNonRtToRt.getWriteBuffer() = arm->nonRtConfig;
208 arm->bufferConfigNonRtToRt.commitWrite();
217 if (not rtTargetSafe)
223 std::tuple<bool, bool>
228 bool rtTargetSafe =
true;
229 bool forceTorqueSafe =
true;
230 for (
auto& pair :
limb)
232 auto& arm = pair.second;
233 arm->nonRtConfig = arm->bufferConfigUserToNonRt.getUpToDateReadBuffer();
234 arm->rtStatusInNonRT = arm->bufferRtStatusToNonRt.getUpToDateReadBuffer();
235 rtSafe = rtSafe and pair.second->rtStatusInNonRT.rtSafe;
236 rtTargetSafe = rtTargetSafe and pair.second->rtStatusInNonRT.rtTargetSafe;
237 forceTorqueSafe = forceTorqueSafe and pair.second->rtStatusInNonRT.forceTorqueSafe;
241 hands->nonRTUpdateStatus();
243 return std::make_tuple(rtTargetSafe, forceTorqueSafe);
249 for (
auto& pair :
limb)
255 hands->nonRTSetTarget();
262 for (
auto& pair :
limb)
264 if (not pair.second->rtReady)
267 if (not pair.second->rtStatusInNonRT.rtTargetSafe)
272 pair.second->rtStatusInNonRT.currentPose,
273 pair.second->rtStatusInNonRT.desiredPoseUnsafe,
285 arm->rtConfig = arm->bufferConfigNonRtToRt.getUpToDateReadBuffer();
286 arm->rtStatus.deltaT = deltaT;
287 arm->rtStatus.accumulateTime += deltaT;
289 arm->controller.ftsensor.updateStatus(arm->rtConfig.ftConfig,
290 arm->rtStatus.currentForceTorque,
291 arm->rtStatus.deltaT,
292 arm->rtFirstRun.load());
293 arm->rtStatus.safeFTGuardOffset.head<3>() =
294 arm->controller.ftsensor.getSafeGuardForceOffset();
296 arm->sensorDevices.updateJointValues(
297 arm->rtStatus.jointPos, arm->rtStatus.jointVel, arm->rtStatus.jointTor);
301 arm->bufferRtStatusToNonRt.getWriteBuffer() = arm->rtStatus;
302 arm->bufferRtStatusToNonRt.commitWrite();
303 arm->bufferRtStatusToUser.getWriteBuffer() = arm->rtStatus;
304 arm->bufferRtStatusToUser.commitWrite();
310 const size_t nDoFTorque,
311 const size_t nDoFVelocity,
312 const Eigen::VectorXf& targetTorque,
313 const Eigen::VectorXf& targetVelocity)
315 for (
size_t i = 0; i < nDoFTorque; i++)
317 auto jointIdx = arm->controller.jointIDTorqueMode[i];
318 arm->targetsTorque.at(i)->torque = targetTorque(jointIdx);
319 if (!arm->targetsTorque.at(i)->isValid())
321 arm->targetsTorque.at(i)->torque = 0;
324 for (
size_t i = 0; i < nDoFVelocity; i++)
326 auto jointIdx = arm->controller.jointIDVelocityMode[i];
327 arm->targetsVel.at(i)->velocity = targetVelocity(jointIdx);
329 if (!arm->targetsVel.at(i)->isValid())
331 arm->targetsVel.at(i)->velocity = 0;
334 arm->bufferRtStatusToOnPublish.getWriteBuffer() = arm->rtStatus;
335 arm->bufferRtStatusToOnPublish.commitWrite();
337 arm->bufferConfigRtToOnPublish.getWriteBuffer() = arm->rtConfig;
338 arm->bufferConfigRtToOnPublish.commitWrite();
340 arm->bufferConfigRtToUser.getWriteBuffer() = arm->rtConfig;
341 arm->bufferConfigRtToUser.commitWrite();
343 if (arm->rtFirstRun.load())
345 arm->rtFirstRun.store(
false);
346 arm->rtReady.store(
true);
353 double time_measure = IceUtil::Time::now().toMicroSecondsDouble();
355 double time_update_status = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
357 arm->controller.run(arm->rtConfig, arm->rtStatus);
358 double time_run_rt = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
361 arm->rtStatus.nDoFTorque,
362 arm->rtStatus.nDoFVelocity,
363 arm->rtStatus.desiredJointTorques,
364 arm->rtStatus.desiredJointVelocity);
366 double time_set_target = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
367 time_measure = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
369 if (time_measure > 200)
372 "time_update_status: %.2f\n"
373 "run_rt_limb: %.2f\n"
374 "set_target_limb: %.2f\n"
377 time_run_rt - time_update_status,
378 time_set_target - time_run_rt,
380 .deactivateSpam(1.0f);
389 double deltaT = timeSinceLastIteration.toSecondsDouble();
390 for (
auto& pair :
limb)
392 limbRT(pair.second, deltaT);
396 hands->updateRTStatus(deltaT);
405 const Ice::Current& iceCurrent)
414 pair.second.desiredPose,
415 prevCfg.limbs.at(pair.first).desiredPose,
417 "previous desired pose",
418 pair.second.safeDistanceMMToGoal,
419 pair.second.safeRotAngleDegreeToGoal,
420 "updateConfig_" + pair.first))
423 pair.second.desiredPose = prevCfg.limbs.at(pair.first).desiredPose;
425 limb.at(pair.first)->bufferConfigUserToNonRt.getWriteBuffer() = pair.second;
426 limb.at(pair.first)->bufferConfigUserToNonRt.commitWrite();
437 for (
auto& pair :
limb)
440 pair.second->bufferConfigRtToUser.getUpToDateReadBuffer();
452 for (
auto& pair :
limb)
455 pair.second->bufferConfigRtToUser.getUpToDateReadBuffer();
466 const std::string& nodeSetName,
467 const bool forceGuard,
468 const bool torqueGuard,
469 const Ice::Current& iceCurrent)
474 it->second.ftConfig.enableSafeGuardForce = forceGuard;
475 it->second.ftConfig.enableSafeGuardTorque = torqueGuard;
476 limb.at(nodeSetName)->controller.ftsensor.enableSafeGuard(forceGuard, torqueGuard);
478 <<
"reset safe guard with force torque sensors: safe? "
479 <<
limb.at(nodeSetName)->controller.ftsensor.isSafe(it->second.ftConfig);
480 limb.at(nodeSetName)->bufferConfigUserToNonRt.getWriteBuffer() = it->second;
481 limb.at(nodeSetName)->bufferConfigUserToNonRt.commitWrite();
486 <<
" found in the controllers.";
493 const std::string& nodeSetName,
494 const Ice::Current& iceCurrent)
499 return limb.at(nodeSetName)->controller.ftsensor.ftSafe.load();
503 <<
" found in the controllers.";
509 const Ice::Current& iceCurrent)
511 std::vector<float> tcpVel;
512 auto& arm =
limb.at(rns);
513 auto s = arm->bufferRtStatusToUser.getUpToDateReadBuffer();
514 for (
int i = 0; i <
s.currentTwist.size(); i++)
516 tcpVel.push_back(
s.currentTwist[i]);
523 const std::string& nodeSetName,
524 const Ice::Current& iceCurrent)
526 auto search =
limb.find(nodeSetName);
527 if (search ==
limb.end())
530 <<
"does not exist in the controller";
531 return std::vector<double>{};
534 auto& arm =
limb.at(nodeSetName);
535 auto currentPose = arm->bufferRtStatusToOnPublish.getUpToDateReadBuffer().currentPose;
544 const Ice::Current& iceCurrent)
548 for (
size_t i = 0; i < 4; ++i)
550 for (
int j = 0; j < 4; ++j)
552 pair.second.desiredPose(i, j) = targetPoseMap.at(pair.first)[i][j];
555 if (targetNullspaceMap.at(pair.first).size() > 0)
557 const size_t nDoF = pair.second.desiredNullspaceJointAngles.value().size();
559 <<
"the joint numbers does not match";
560 for (
size_t i = 0; i < nDoF; ++i)
562 pair.second.desiredNullspaceJointAngles.value()(i) =
563 targetNullspaceMap.at(pair.first)[i];
566 limb.at(pair.first)->bufferConfigUserToNonRt.getWriteBuffer() = pair.second;
567 limb.at(pair.first)->bufferConfigUserToNonRt.commitWrite();
576 const auto nDoF = arm->controller.numOfJoints;
579 const auto checkNonNegative = [](
const auto&
v) {
ARMARX_CHECK((
v.array() >= 0).all()); };
581 if (!configData.desiredNullspaceJointAngles.has_value())
585 ARMARX_INFO <<
"No user defined nullspace target, reset to zero with "
587 configData.desiredNullspaceJointAngles = Eigen::VectorXf::Zero(nDoF);
588 arm->reInitPreActivate.store(
true);
593 arm->bufferConfigRtToUser.getUpToDateReadBuffer().desiredNullspaceJointAngles;
595 <<
"Controller is active, but no user defined nullspace target. \n"
596 "You should first get up-to-date config of this controller and then update "
598 " auto cfg = ctrl->getConfig(); \n"
599 " cfg.desiredPose = xxx;\n"
600 " ctrl->updateConfig(cfg);\n"
601 "Now, I decide by myself to use the existing values of nullspace target\n"
602 << currentValue.value();
603 configData.desiredNullspaceJointAngles = currentValue;
607 checkSize(configData.desiredNullspaceJointAngles.value());
608 checkSize(configData.kdNullspaceTorque);
609 checkSize(configData.kpNullspaceTorque);
610 checkSize(configData.kdNullspaceVel);
611 checkSize(configData.kpNullspaceVel);
613 checkNonNegative(configData.kdNullspaceTorque);
614 checkNonNegative(configData.kpNullspaceTorque);
615 checkNonNegative(configData.kpNullspaceVel);
616 checkNonNegative(configData.kdNullspaceVel);
617 checkNonNegative(configData.kdImpedance);
618 checkNonNegative(configData.kpImpedance);
627 auto rtData = arm->bufferConfigRtToOnPublish.getUpToDateReadBuffer();
628 auto rtStatus = arm->bufferRtStatusToOnPublish.getUpToDateReadBuffer();
632 if (rtData.desiredNullspaceJointAngles.has_value())
635 "desiredNullspaceJointAngles",
636 rtData.desiredNullspaceJointAngles.value());
645 float positionError =
649 datafields[
"poseError_position"] =
new Variant(positionError);
650 datafields[
"poseError_angle"] =
new Variant(angleError);
651 datafields[
"poseError_threshold_position"] =
new Variant(rtData.safeDistanceMMToGoal);
652 datafields[
"poseError_threshold_angle"] =
new Variant(rtData.safeRotAngleDegreeToGoal);
660 float currentForceNorm = rtStatus.currentForceTorque.head<3>().
norm();
661 float currentTorqueNorm = rtStatus.currentForceTorque.tail<3>().
norm();
662 float safeForceGuardOffsetNorm = rtStatus.safeFTGuardOffset.head<3>().
norm();
663 float safeTorqueGuardOffsetNorm = rtStatus.safeFTGuardOffset.tail<3>().
norm();
664 datafields[
"currentForceNorm"] =
new Variant(currentForceNorm);
665 datafields[
"currentTorqueNorm"] =
new Variant(currentTorqueNorm);
666 datafields[
"safeForceGuardOffsetNorm"] =
new Variant(safeForceGuardOffsetNorm);
667 datafields[
"safeTorqueGuardOffsetNorm"] =
new Variant(safeTorqueGuardOffsetNorm);
668 datafields[
"safeForceGuardDifference"] =
669 new Variant(currentForceNorm - safeForceGuardOffsetNorm);
670 datafields[
"safeTorqueGuardDifference"] =
671 new Variant(currentTorqueNorm - safeTorqueGuardOffsetNorm);
672 datafields[
"safeForceGuardThreshold"] =
673 new Variant(rtData.ftConfig.safeGuardForceThreshold);
674 datafields[
"safeTorqueGuardThreshold"] =
675 new Variant(rtData.ftConfig.safeGuardTorqueThreshold);
677 datafields[
"rtSafe"] =
new Variant(rtStatus.rtSafe * 1.0);
678 datafields[
"rtTargetSafe"] =
new Variant(rtStatus.rtTargetSafe * 1.0);
679 bool forceTrigger = (not rtStatus.rtSafe) and rtStatus.rtTargetSafe;
680 datafields[
"forceTrigger"] =
new Variant(forceTrigger * 1.0);
681 datafields[
"forceTorqueSafe"] =
new Variant(rtStatus.forceTorqueSafe * 1.0);
690 debugObs->setDebugChannel(
getClassName() +
"_" + arm->kinematicChainName, datafields);
696 rtStatus.forceImpedance.head<3>() * 5.0)
707 for (
auto& pair :
limb)
709 if (not pair.second->rtReady.load())
718 for (
auto& pair :
limb)
720 pair.second->rtReady.store(
false);
727 VirtualRobot::RobotNodeSetPtr rns =
rtGetRobot()->getRobotNodeSet(arm->kinematicChainName);
729 auto quat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
731 "[ %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f ].",
740 if (arm->reInitPreActivate.load())
742 arm->rtConfig.desiredNullspaceJointAngles.value() = rns->getJointValuesEigen();
743 arm->rtConfig.desiredPose = currentPose;
745 arm->bufferConfigUserToNonRt.reinitAllBuffers(arm->rtConfig);
746 arm->reInitPreActivate.store(
false);
749 arm->nonRtConfig = arm->rtConfig;
750 arm->bufferConfigNonRtToRt.reinitAllBuffers(arm->rtConfig);
751 arm->bufferConfigRtToOnPublish.reinitAllBuffers(arm->rtConfig);
752 arm->bufferConfigRtToUser.reinitAllBuffers(arm->rtConfig);
755 arm->sensorDevices.updateJointValues(
756 arm->rtStatus.jointPos, arm->rtStatus.jointVel, arm->rtStatus.jointTor);
757 arm->rtStatus.rtPreActivate(currentPose);
759 arm->rtStatusInNonRT = arm->rtStatus;
760 arm->nonRTDeltaT = 0.0;
761 arm->nonRTAccumulateTime = 0.0;
762 arm->bufferRtStatusToOnPublish.reinitAllBuffers(arm->rtStatus);
763 arm->bufferRtStatusToNonRt.reinitAllBuffers(arm->rtStatus);
764 arm->bufferRtStatusToUser.reinitAllBuffers(arm->rtStatus);
767 arm->controller.preactivateInit(rns);
773 for (
auto& pair :
limb)
777 userConfig.limbs.at(pair.first) = pair.second->rtConfig;
781 hands->rtPreActivate();
798 const std::map<std::string, ConstControlDevicePtr>& controlDevices,
799 const std::map<std::string, ConstSensorDevicePtr>&)
805 ::armarx::WidgetDescription::WidgetSeq widgets;
808 LabelPtr label =
new Label;
809 label->text =
"select a controller config";
811 StringComboBoxPtr cfgBox =
new StringComboBox;
812 cfgBox->name =
"config_box";
813 cfgBox->defaultIndex = 0;
814 cfgBox->multiSelect =
false;
816 cfgBox->options = std::vector<std::string>{
"default",
"default_right",
"default_right_a7"};
819 layout->children.emplace_back(label);
820 layout->children.emplace_back(cfgBox);
823 layout->children.insert(layout->children.end(), widgets.begin(), widgets.end());
832 auto cfgName =
values.at(
"config_box")->getString();
835 "controller_config/NJointTaskspaceMixedImpedanceVelocityController/" + cfgName +
840 auto cfgDTO = armarx::readFromJson<ConfigDict>(configPath.
toSystemPath());
843 return new ConfigurableNJointControllerConfig{cfgDTO.toAronDTO()};