32 #include <VirtualRobot/MathTools.h>
33 #include <VirtualRobot/Nodes/RobotNode.h>
34 #include <VirtualRobot/RobotNodeSet.h>
35 #include <VirtualRobot/math/Helpers.h>
43 const std::string& controllerName,
44 NJointCartesianNaturalPositionControllerConfigPtr config) :
45 _ik(ik), _robotUnit(robotUnit), _controllerName(controllerName), _config(config), _arm(arm)
47 _runCfg = _config->runCfg;
48 _velocityBaseSettings.
basePosVel = _runCfg.maxTcpPosVel;
49 _velocityBaseSettings.
baseOriVel = _runCfg.maxTcpOriVel;
52 _userNullspaceTargets = std::vector<float>(arm.
rns->getSize(), std::nanf(
""));
53 _naturalNullspaceTargets = std::vector<float>(arm.
rns->getSize(), std::nanf(
""));
55 VirtualRobot::RobotNodeSetPtr rns = arm.
rns;
56 for (
size_t i = 0; i < rns->getSize(); i++)
58 VirtualRobot::RobotNodePtr rn = rns->getNode(i);
63 _rnsToElb.emplace_back(rn);
70 _defaultWaypointConfigs[
"default"] = wpc;
72 _ftOffset.force = Eigen::Vector3f::Zero();
73 _ftOffset.torque = Eigen::Vector3f::Zero();
76 NJointCartesianNaturalPositionControllerConfigPtr
78 const std::string& elbowNode)
80 NJointCartesianNaturalPositionControllerConfigPtr cfg =
81 new NJointCartesianNaturalPositionControllerConfig();
83 cfg->elbowNode = elbowNode;
90 NJointControllerInterfacePrx ctrl = _robotUnit->getNJointController(_controllerName);
93 _controllerCreated =
false;
94 _controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
95 _controller->setConfig(_runCfg);
99 ctrl = _robotUnit->createNJointController(
100 "NJointCartesianNaturalPositionController", _controllerName, _config);
101 _controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
102 _controllerCreated =
true;
104 if (_activateControllerOnInit)
106 _controller->activateController();
113 std::optional<float> minElbowHeight)
116 _tcpTarget = tcpTarget;
124 ARMARX_ERROR <<
"could not solve natural IK for target: " << tcpTarget;
127 _elbTarget = _arm.
elbow->getPoseInRootFrame();
128 _controller->setTarget(
130 if (_setJointNullspaceFromNaturalIK)
132 for (
size_t i = 0; i < _rnsToElb.size(); i++)
134 _naturalNullspaceTargets.at(i) = ikResult.
jointValues(i);
137 updateNullspaceTargets();
144 const std::vector<float>& nullspaceTargets)
147 _userNullspaceTargets = nullspaceTargets;
148 updateNullspaceTargets();
194 _ftOffset = _controller->getAverageFTValue();
195 _controller->setFTOffset(_ftOffset);
201 bool useCurrentFTasOffset)
207 _controller->setFTLimit(force, torque);
213 _controller->clearFTLimit();
220 FTSensorValue ft = _controller->getCurrentFTValue();
223 ft.force = ft.force - _ftOffset.force;
224 ft.torque = ft.torque - _ftOffset.torque;
229 armarx::FTSensorValue
232 FTSensorValue ft = _controller->getAverageFTValue();
235 ft.force = ft.force - _ftOffset.force;
236 ft.torque = ft.torque - _ftOffset.torque;
244 _controller->stopMovement();
245 _controller->clearFTLimit();
246 _controller->setNullspaceControlEnabled(
true);
251 CartesianNaturalPositionControllerProxy::updateDynamicKp()
258 _dynamicKp.
calculate(error, KpElb, KpJla);
260 _runCfg.elbowKp = KpElb;
261 _runCfg.jointLimitAvoidanceKp = KpJla;
262 _controller->setConfig(_runCfg);
268 CartesianNaturalPositionControllerProxy::updateNullspaceTargets()
270 std::vector<float> nsTargets = _userNullspaceTargets;
271 for (
size_t i = 0; i < _naturalNullspaceTargets.size(); i++)
273 if (std::isnan(nsTargets.at(i)))
275 nsTargets.at(i) = _naturalNullspaceTargets.at(i);
278 _controller->setNullspaceTarget(nsTargets);
286 float f = std::exp(-0.5f * (error * error) / (
sigmaMM *
sigmaMM));
288 KpJla = (1 - f) *
maxKp;
293 CartesianNaturalPositionControllerConfig runCfg)
295 _controller->setConfig(runCfg);
296 this->_runCfg = runCfg;
299 CartesianNaturalPositionControllerConfig
309 if (_waypoints.size() == 0)
311 _waypointChanged =
true;
313 _waypoints.emplace_back(waypoint);
318 const Eigen::Vector3f& tcpTarget,
319 const std::vector<float>& userNullspaceTargets,
320 std::optional<float> minElbowHeight)
327 std::optional<float> minElbowHeight)
330 w.
config = _defaultWaypointConfigs[
"default"];
340 std::optional<float> minElbowHeight)
343 w.
config = _defaultWaypointConfigs[
"default"];
355 _currentWaypointIndex = 0;
362 _defaultWaypointConfigs[
"default"] = config;
368 std::stringstream ss;
370 ss << std::fixed <<
"Waypoint: " << (_currentWaypointIndex + 1) <<
"/" << _waypoints.size()
378 const VirtualRobot::RobotNodeSetPtr& rns,
381 size_t len = rns->getSize();
382 std::vector<Eigen::Vector3f> positions;
383 for (
size_t i = 0; i < len; i++)
385 positions.push_back(rns->getNode(i)->getPositionInRootFrame());
387 positions.push_back(rns->getTCP()->getPositionInRootFrame());
389 std::vector<float> dists;
390 for (
size_t i = 0; i < len; i++)
392 dists.push_back((positions.at(i) - positions.at(i + 1)).norm());
395 std::vector<float> result(len, 0);
397 for (
int i = len - 1; i >= 0; i--)
400 result.at(i) = maxPosVel / dist;
408 std::vector<float> result(vec.size(), 0);
409 for (
size_t i = 0; i < vec.size(); i++)
411 result.at(i) = vec.at(i) * scale;
419 _activateControllerOnInit =
value;
427 float scale = referencePosVel /
v.basePosVel;
428 _runCfg.maxTcpPosVel =
v.basePosVel *
v.scaleTcpPosVel * scale;
429 _runCfg.maxTcpOriVel =
v.baseOriVel *
v.scaleTcpOriVel * scale;
430 _runCfg.maxElbPosVel =
v.basePosVel *
v.scaleElbPosVel * scale;
431 _runCfg.maxJointVelocity =
ScaleVec(
v.baseJointVelocity,
v.scaleJointVelocities * scale);
432 _runCfg.maxNullspaceVelocity =
433 ScaleVec(
v.baseJointVelocity,
v.scaleNullspaceVelocities * scale);
437 _runCfg.jointLimitAvoidanceKp = k.
baseKpJla;
438 _runCfg.nullspaceTargetKp = k.
baseKpNs;
443 _controller->setConfig(_runCfg);
448 const CartesianNaturalPositionControllerConfig& runCfg)
453 _kpBaseSettings.
baseKpJla = _runCfg.jointLimitAvoidanceKp;
454 _kpBaseSettings.
baseKpNs = _runCfg.nullspaceTargetKp;
463 if (_controllerCreated)
466 _controller->deactivateAndDeleteController();
471 _controller->deactivateController();
473 _controllerCreated =
false;
479 if (_waypoints.size() == 0)
487 _currentWaypointIndex++;
488 _waypointChanged =
true;
490 if (_waypointChanged)
492 _waypointChanged =
false;
494 return onWaypointChanged();
500 CartesianNaturalPositionControllerProxy::onWaypointChanged()
504 _userNullspaceTargets = w.targets.userNullspaceTargets;
506 << math::Helpers::GetPosition(w.targets.tcpTarget).transpose();
507 return setTarget(w.targets.tcpTarget, w.targets.setOri, w.targets.minElbowHeight);
514 return _waypoints.at(_currentWaypointIndex);
520 return _waypoints.size() == 0 || _currentWaypointIndex == _waypoints.size() - 1;
523 NJointCartesianNaturalPositionControllerInterfacePrx
532 _dynamicKp = dynamicKp;
548 config.thresholdTcpPosReached,
549 config.thresholdTcpPosReached /
550 config.rad2mmFactor);
557 this->config = config;
563 const std::vector<float>& userNullspaceTargets)
565 ARMARX_CHECK(this->targets.userNullspaceTargets.size() == userNullspaceTargets.size());
566 this->targets.userNullspaceTargets = userNullspaceTargets;
607 const VirtualRobot::RobotNodeSetPtr& rns) :
608 jointValues(rns->getJointValues()), rns(rns)
615 rns->setJointValues(jointValues);