CartesianNaturalPositionControllerProxy.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
5  * Karlsruhe Institute of Technology (KIT), all rights reserved.
6  *
7  * ArmarX is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License version 2 as
9  * published by the Free Software Foundation.
10  *
11  * ArmarX is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program. If not, see <http://www.gnu.org/licenses/>.
18  *
19  * @author armar-user (armar-user at kit dot edu)
20  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
21  * GNU General Public License
22  */
23 
25 
28 
30 //#include <RobotAPI/libraries/aron/core/navigator/Navigator.h>
31 
32 #include <VirtualRobot/MathTools.h>
33 #include <VirtualRobot/Nodes/RobotNode.h>
34 #include <VirtualRobot/RobotNodeSet.h>
35 #include <VirtualRobot/math/Helpers.h>
36 
37 using namespace armarx;
38 
40  const NaturalIK& ik,
41  const NaturalIK::ArmJoints& arm,
42  const RobotUnitInterfacePrx& robotUnit,
43  const std::string& controllerName,
44  NJointCartesianNaturalPositionControllerConfigPtr config) :
45  _ik(ik), _robotUnit(robotUnit), _controllerName(controllerName), _config(config), _arm(arm)
46 {
47  _runCfg = _config->runCfg;
48  _velocityBaseSettings.basePosVel = _runCfg.maxTcpPosVel;
49  _velocityBaseSettings.baseOriVel = _runCfg.maxTcpOriVel;
50  _velocityBaseSettings.baseJointVelocity =
51  CalculateMaxJointVelocity(arm.rns, _runCfg.maxTcpPosVel);
52  _userNullspaceTargets = std::vector<float>(arm.rns->getSize(), std::nanf(""));
53  _naturalNullspaceTargets = std::vector<float>(arm.rns->getSize(), std::nanf(""));
54  updateBaseKpValues(_runCfg);
55  VirtualRobot::RobotNodeSetPtr rns = arm.rns;
56  for (size_t i = 0; i < rns->getSize(); i++)
57  {
58  VirtualRobot::RobotNodePtr rn = rns->getNode(i);
59  if (rn == arm.elbow)
60  {
61  break;
62  }
63  _rnsToElb.emplace_back(rn);
64  }
65 
66  {
67  WaypointConfig wpc;
68  wpc.referencePosVel = 80;
69  wpc.thresholdTcpPosReached = 5;
70  _defaultWaypointConfigs["default"] = wpc;
71  }
72  _ftOffset.force = Eigen::Vector3f::Zero();
73  _ftOffset.torque = Eigen::Vector3f::Zero();
74 }
75 
76 NJointCartesianNaturalPositionControllerConfigPtr
78  const std::string& elbowNode)
79 {
80  NJointCartesianNaturalPositionControllerConfigPtr cfg =
81  new NJointCartesianNaturalPositionControllerConfig();
82  cfg->rns = rns;
83  cfg->elbowNode = elbowNode;
84  return cfg;
85 }
86 
87 void
89 {
90  NJointControllerInterfacePrx ctrl = _robotUnit->getNJointController(_controllerName);
91  if (ctrl)
92  {
93  _controllerCreated = false;
94  _controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
95  _controller->setConfig(_runCfg);
96  }
97  else
98  {
99  ctrl = _robotUnit->createNJointController(
100  "NJointCartesianNaturalPositionController", _controllerName, _config);
101  _controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
102  _controllerCreated = true;
103  }
104  if (_activateControllerOnInit)
105  {
106  _controller->activateController();
107  }
108 }
109 
110 bool
112  NaturalDiffIK::Mode setOri,
113  std::optional<float> minElbowHeight)
114 {
115  ScopedJointValueRestore jvr(_arm.rns);
116  _tcpTarget = tcpTarget;
117  _setOri = setOri;
118  _fwd = _ik.solveSoechtingIK(math::Helpers::Position(tcpTarget), minElbowHeight);
119  //VirtualRobot::IKSolver::CartesianSelection mode = setOri ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
121  tcpTarget, _fwd.elbow, _arm.rns, _arm.tcp, _arm.elbow, setOri, _natikParams.diffIKparams);
122  if (!ikResult.reached)
123  {
124  ARMARX_ERROR << "could not solve natural IK for target: " << tcpTarget;
125  return false;
126  }
127  _elbTarget = _arm.elbow->getPoseInRootFrame();
128  _controller->setTarget(
129  _tcpTarget, _arm.elbow->getPositionInRootFrame(), setOri == NaturalDiffIK::Mode::All);
130  if (_setJointNullspaceFromNaturalIK)
131  {
132  for (size_t i = 0; i < _rnsToElb.size(); i++)
133  {
134  _naturalNullspaceTargets.at(i) = ikResult.jointValues(i);
135  }
136  }
137  updateNullspaceTargets();
138  updateDynamicKp();
139  return true;
140 }
141 
142 void
144  const std::vector<float>& nullspaceTargets)
145 {
146  ARMARX_CHECK(_arm.rns->getSize() == nullspaceTargets.size());
147  _userNullspaceTargets = nullspaceTargets;
148  updateNullspaceTargets();
149 }
150 
151 Eigen::Vector3f
153 {
154  return math::Helpers::Position(_tcpTarget);
155 }
156 
157 Eigen::Vector3f
159 {
160  return math::Helpers::Position(_elbTarget);
161 }
162 
163 float
165 {
166  return CartesianPositionController::GetPositionError(_tcpTarget, _arm.tcp);
167 }
168 
169 float
171 {
172  if (_setOri == NaturalDiffIK::Mode::Position)
173  {
174  return 0;
175  }
176  return CartesianPositionController::GetOrientationError(_tcpTarget, _arm.tcp);
177 }
178 
179 float
181 {
182  return CartesianPositionController::GetPositionError(_elbTarget, _arm.elbow);
183 }
184 
185 bool
187 {
188  return isLastWaypoint() && currentWaypoint().reached(_arm.tcp);
189 }
190 
191 void
193 {
194  _ftOffset = _controller->getAverageFTValue();
195  _controller->setFTOffset(_ftOffset);
196 }
197 
198 void
200  float torque,
201  bool useCurrentFTasOffset)
202 {
204  {
205  this->useCurrentFTasOffset();
206  }
207  _controller->setFTLimit(force, torque);
208 }
209 
210 void
212 {
213  _controller->clearFTLimit();
214  //_controller->resetFTOffset();
215 }
216 
217 FTSensorValue
219 {
220  FTSensorValue ft = _controller->getCurrentFTValue();
221  if (substactOffset)
222  {
223  ft.force = ft.force - _ftOffset.force;
224  ft.torque = ft.torque - _ftOffset.torque;
225  }
226  return ft;
227 }
228 
229 armarx::FTSensorValue
231 {
232  FTSensorValue ft = _controller->getAverageFTValue();
233  if (substactOffset)
234  {
235  ft.force = ft.force - _ftOffset.force;
236  ft.torque = ft.torque - _ftOffset.torque;
237  }
238  return ft;
239 }
240 
241 void
243 {
244  _controller->stopMovement();
245  _controller->clearFTLimit();
246  _controller->setNullspaceControlEnabled(true);
247  clearWaypoints();
248 }
249 
250 void
251 CartesianNaturalPositionControllerProxy::updateDynamicKp()
252 {
253  if (_dynamicKp.enabled)
254  {
255  float error = (math::Helpers::Position(_tcpTarget) - _fwd.wrist).norm();
256  float KpElb, KpJla;
257  ARMARX_IMPORTANT << VAROUT(error);
258  _dynamicKp.calculate(error, KpElb, KpJla);
259  //ARMARX_IMPORTANT << VAROUT()
260  _runCfg.elbowKp = KpElb;
261  _runCfg.jointLimitAvoidanceKp = KpJla;
262  _controller->setConfig(_runCfg);
263  ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
264  }
265 }
266 
267 void
268 CartesianNaturalPositionControllerProxy::updateNullspaceTargets()
269 {
270  std::vector<float> nsTargets = _userNullspaceTargets;
271  for (size_t i = 0; i < _naturalNullspaceTargets.size(); i++)
272  {
273  if (std::isnan(nsTargets.at(i)))
274  {
275  nsTargets.at(i) = _naturalNullspaceTargets.at(i);
276  }
277  }
278  _controller->setNullspaceTarget(nsTargets);
279 }
280 
281 void
283  float& KpElb,
284  float& KpJla)
285 {
286  float f = std::exp(-0.5f * (error * error) / (sigmaMM * sigmaMM));
287  KpElb = f * maxKp;
288  KpJla = (1 - f) * maxKp;
289 }
290 
291 void
293  CartesianNaturalPositionControllerConfig runCfg)
294 {
295  _controller->setConfig(runCfg);
296  this->_runCfg = runCfg;
297 }
298 
299 CartesianNaturalPositionControllerConfig
301 {
302  return _runCfg;
303 }
304 
305 void
308 {
309  if (_waypoints.size() == 0)
310  {
311  _waypointChanged = true;
312  }
313  _waypoints.emplace_back(waypoint);
314 }
315 
318  const Eigen::Vector3f& tcpTarget,
319  const std::vector<float>& userNullspaceTargets,
320  std::optional<float> minElbowHeight)
321 {
322  return createWaypoint(tcpTarget, minElbowHeight).setNullspaceTargets(userNullspaceTargets);
323 }
324 
327  std::optional<float> minElbowHeight)
328 {
329  Waypoint w;
330  w.config = _defaultWaypointConfigs["default"];
331  w.targets.tcpTarget = math::Helpers::CreatePose(tcpTarget, Eigen::Matrix3f::Identity());
333  w.targets.minElbowHeight = minElbowHeight;
334  w.targets.userNullspaceTargets = std::vector<float>(_arm.rns->getSize(), std::nanf(""));
335  return w;
336 }
337 
340  std::optional<float> minElbowHeight)
341 {
342  Waypoint w;
343  w.config = _defaultWaypointConfigs["default"];
344  w.targets.tcpTarget = tcpTarget;
346  w.targets.minElbowHeight = minElbowHeight;
347  w.targets.userNullspaceTargets = std::vector<float>(_arm.rns->getSize(), std::nanf(""));
348  return w;
349 }
350 
351 void
353 {
354  _waypoints.clear();
355  _currentWaypointIndex = 0;
356 }
357 
358 void
361 {
362  _defaultWaypointConfigs["default"] = config;
363 }
364 
365 std::string
367 {
368  std::stringstream ss;
369  ss.precision(2);
370  ss << std::fixed << "Waypoint: " << (_currentWaypointIndex + 1) << "/" << _waypoints.size()
371  << " distance: " << getTcpPositionError() << " mm "
372  << VirtualRobot::MathTools::rad2deg(getTcpOrientationError()) << " deg";
373  return ss.str();
374 }
375 
376 std::vector<float>
378  const VirtualRobot::RobotNodeSetPtr& rns,
379  float maxPosVel)
380 {
381  size_t len = rns->getSize();
382  std::vector<Eigen::Vector3f> positions;
383  for (size_t i = 0; i < len; i++)
384  {
385  positions.push_back(rns->getNode(i)->getPositionInRootFrame());
386  }
387  positions.push_back(rns->getTCP()->getPositionInRootFrame());
388 
389  std::vector<float> dists;
390  for (size_t i = 0; i < len; i++)
391  {
392  dists.push_back((positions.at(i) - positions.at(i + 1)).norm());
393  }
394 
395  std::vector<float> result(len, 0);
396  float dist = 0;
397  for (int i = len - 1; i >= 0; i--)
398  {
399  dist += dists.at(i);
400  result.at(i) = maxPosVel / dist;
401  }
402  return result;
403 }
404 
405 std::vector<float>
406 CartesianNaturalPositionControllerProxy::ScaleVec(const std::vector<float>& vec, float scale)
407 {
408  std::vector<float> result(vec.size(), 0);
409  for (size_t i = 0; i < vec.size(); i++)
410  {
411  result.at(i) = vec.at(i) * scale;
412  }
413  return result;
414 }
415 
416 void
418 {
419  _activateControllerOnInit = value;
420 }
421 
422 void
424 {
425  VelocityBaseSettings& v = _velocityBaseSettings;
426  KpBaseSettings& k = _kpBaseSettings;
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);
434  _runCfg.KpPos = k.baseKpTcpPos; // * scale;
435  _runCfg.KpOri = k.baseKpTcpOri; // * scale;
436  _runCfg.elbowKp = k.baseKpElbPos; // * scale;
437  _runCfg.jointLimitAvoidanceKp = k.baseKpJla; // * scale;
438  _runCfg.nullspaceTargetKp = k.baseKpNs; // * scale;
439  _runCfg.maxNullspaceAcceleration = k.maxNullspaceAcceleration; // * scale;
440  _runCfg.maxPositionAcceleration = k.maxPositionAcceleration; // * scale;
441  _runCfg.maxOrientationAcceleration = k.maxOrientationAcceleration; // * scale;
442 
443  _controller->setConfig(_runCfg);
444 }
445 
446 void
448  const CartesianNaturalPositionControllerConfig& runCfg)
449 {
450  _kpBaseSettings.baseKpTcpPos = _runCfg.KpPos;
451  _kpBaseSettings.baseKpTcpOri = _runCfg.KpOri;
452  _kpBaseSettings.baseKpElbPos = _runCfg.elbowKp;
453  _kpBaseSettings.baseKpJla = _runCfg.jointLimitAvoidanceKp;
454  _kpBaseSettings.baseKpNs = _runCfg.nullspaceTargetKp;
455  _kpBaseSettings.maxNullspaceAcceleration = _runCfg.maxNullspaceAcceleration;
456  _kpBaseSettings.maxPositionAcceleration = _runCfg.maxPositionAcceleration;
457  _kpBaseSettings.maxOrientationAcceleration = _runCfg.maxOrientationAcceleration;
458 }
459 
460 void
462 {
463  if (_controllerCreated)
464  {
465  // delete controller only if it was created
466  _controller->deactivateAndDeleteController();
467  }
468  else
469  {
470  // if the controller existed, only deactivate it
471  _controller->deactivateController();
472  }
473  _controllerCreated = false;
474 }
475 
476 bool
478 {
479  if (_waypoints.size() == 0)
480  {
481  return true;
482  }
483 
484  Waypoint& w = currentWaypoint();
485  if (!isLastWaypoint() && w.reached(_arm.tcp))
486  {
487  _currentWaypointIndex++;
488  _waypointChanged = true;
489  }
490  if (_waypointChanged)
491  {
492  _waypointChanged = false;
493  ARMARX_IMPORTANT << "Waypoint changed. Setting new target: ";
494  return onWaypointChanged();
495  }
496  return true;
497 }
498 
499 bool
500 CartesianNaturalPositionControllerProxy::onWaypointChanged()
501 {
502  Waypoint& w = currentWaypoint();
503  setMaxVelocities(w.config.referencePosVel);
504  _userNullspaceTargets = w.targets.userNullspaceTargets;
505  ARMARX_IMPORTANT << "Waypoint target position: "
506  << math::Helpers::GetPosition(w.targets.tcpTarget).transpose();
507  return setTarget(w.targets.tcpTarget, w.targets.setOri, w.targets.minElbowHeight);
508 }
509 
512 {
513  ARMARX_CHECK(_waypoints.size() > 0);
514  return _waypoints.at(_currentWaypointIndex);
515 }
516 
517 bool
519 {
520  return _waypoints.size() == 0 || _currentWaypointIndex == _waypoints.size() - 1;
521 }
522 
523 NJointCartesianNaturalPositionControllerInterfacePrx
525 {
526  return _controller;
527 }
528 
529 void
531 {
532  _dynamicKp = dynamicKp;
533  updateDynamicKp();
534 }
535 
538 {
539  return _dynamicKp;
540 }
541 
542 bool
544 {
545  return CartesianPositionController::Reached(targets.tcpTarget,
546  tcp,
547  targets.setOri == NaturalDiffIK::Mode::All,
548  config.thresholdTcpPosReached,
549  config.thresholdTcpPosReached /
550  config.rad2mmFactor);
551 }
552 
556 {
557  this->config = config;
558  return *this;
559 }
560 
563  const std::vector<float>& userNullspaceTargets)
564 {
565  ARMARX_CHECK(this->targets.userNullspaceTargets.size() == userNullspaceTargets.size());
566  this->targets.userNullspaceTargets = userNullspaceTargets;
567  return *this;
568 }
569 
570 /*
571 aron::AronObjectPtr CartesianNaturalPositionControllerProxy::Waypoint::toAron()
572 {
573  aron::AronObjectPtr obj = new aron::AronObject();
574  aron::Navigator nav(obj);
575  nav.appendMatrix4f("tcpTarget", tcpTarget);
576  nav.appendVector3f("elbTarget", elbTarget);
577  nav.appendVec("userNullspaceTargets", userNullspaceTargets);
578  nav.appendVec("naturalNullspaceTargets", naturalNullspaceTargets);
579  nav.appendBool("setOri", setOri == NaturalDiffIK::Mode::All);
580  nav.appendFloat("referencePosVel", referencePosVel);
581  nav.appendFloat("thresholdTcpPosReached", thresholdTcpPosReached);
582  nav.appendFloat("thresholdTcpOriReached", thresholdTcpOriReached);
583  nav.appendBool("resetFTonEnter", resetFTonEnter);
584  nav.appendBool("resetFTonExit", resetFTonExit);
585  nav.appendFloat("forceTheshold", forceTheshold);
586  return obj;
587 }
588 
589 void CartesianNaturalPositionControllerProxy::Waypoint::fromAron(aron::AronObjectPtr aron)
590 {
591  aron::Navigator nav(aron);
592  tcpTarget = nav.atKey("tcpTarget").asMatrix4f();
593  elbTarget = nav.atKey("elbTarget").asVector3f();
594  userNullspaceTargets = nav.atKey("userNullspaceTargets").asVecFloat();
595  naturalNullspaceTargets = nav.atKey("naturalNullspaceTargets").asVecFloat();
596  setOri = nav.atKey("setOri").asBool() ? NaturalDiffIK::Mode::All : NaturalDiffIK::Mode::Position;
597  referencePosVel = nav.atKey("referencePosVel").asFloat();
598  thresholdTcpPosReached = nav.atKey("thresholdTcpPosReached").asFloat();
599  thresholdTcpOriReached = nav.atKey("thresholdTcpOriReached").asFloat();
600  resetFTonEnter = nav.atKey("resetFTonEnter").asBool();
601  resetFTonExit = nav.atKey("resetFTonExit").asBool();
602  forceTheshold = nav.atKey("forceTheshold").asFloat();
603 
604 }*/
605 
607  const VirtualRobot::RobotNodeSetPtr& rns) :
608  jointValues(rns->getJointValues()), rns(rns)
609 {
610 }
611 
613 {
614  //ARMARX_IMPORTANT << "restoring joint values";
615  rns->setJointValues(jointValues);
616 }
armarx::CartesianNaturalPositionControllerProxy::KpBaseSettings::baseKpNs
float baseKpNs
Definition: CartesianNaturalPositionControllerProxy.h:56
armarx::CartesianNaturalPositionControllerProxy::setTarget
bool setTarget(const Eigen::Matrix4f &tcpTarget, NaturalDiffIK::Mode setOri, std::optional< float > minElbowHeight=std::nullopt)
Definition: CartesianNaturalPositionControllerProxy.cpp:111
armarx::CartesianNaturalPositionControllerProxy::cleanup
void cleanup()
Definition: CartesianNaturalPositionControllerProxy.cpp:461
armarx::CartesianNaturalPositionControllerProxy::KpBaseSettings::maxPositionAcceleration
float maxPositionAcceleration
Definition: CartesianNaturalPositionControllerProxy.h:57
armarx::NaturalDiffIK::Mode::All
@ All
armarx::CartesianNaturalPositionControllerProxy::getRuntimeConfig
CartesianNaturalPositionControllerConfig getRuntimeConfig()
Definition: CartesianNaturalPositionControllerProxy.cpp:300
CartesianNaturalPositionControllerProxy.h
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::CartesianNaturalPositionControllerProxy::WaypointTargets::setOri
NaturalDiffIK::Mode setOri
Definition: CartesianNaturalPositionControllerProxy.h:86
armarx::CartesianNaturalPositionControllerProxy::enableFTLimit
void enableFTLimit(float force, float torque, bool useCurrentFTasOffset)
Definition: CartesianNaturalPositionControllerProxy.cpp:199
armarx::CartesianNaturalPositionControllerProxy::WaypointConfig
Definition: CartesianNaturalPositionControllerProxy.h:75
armarx::CartesianNaturalPositionControllerProxy::WaypointTargets::minElbowHeight
std::optional< float > minElbowHeight
Definition: CartesianNaturalPositionControllerProxy.h:87
armarx::NaturalDiffIK::Mode::Position
@ Position
armarx::NaturalIK::ArmJoints::tcp
VirtualRobot::RobotNodePtr tcp
Definition: NaturalIK.h:70
armarx::NaturalIK::SoechtingForwardPositions::wrist
Eigen::Vector3f wrist
Definition: NaturalIK.h:98
armarx::CartesianNaturalPositionControllerProxy::isFinalWaypointReached
bool isFinalWaypointReached()
Definition: CartesianNaturalPositionControllerProxy.cpp:186
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::CartesianNaturalPositionControllerProxy::getInternalController
NJointCartesianNaturalPositionControllerInterfacePrx getInternalController()
Definition: CartesianNaturalPositionControllerProxy.cpp:524
armarx::CartesianNaturalPositionControllerProxy::ScopedJointValueRestore::~ScopedJointValueRestore
~ScopedJointValueRestore()
Definition: CartesianNaturalPositionControllerProxy.cpp:612
armarx::CartesianNaturalPositionControllerProxy::VelocityBaseSettings::basePosVel
float basePosVel
Definition: CartesianNaturalPositionControllerProxy.h:71
armarx::NaturalDiffIK::Mode
Mode
Definition: NaturalDiffIK.h:34
armarx::NaturalIK::ArmJoints::elbow
VirtualRobot::RobotNodePtr elbow
Definition: NaturalIK.h:68
armarx::NaturalDiffIK::Result::reached
bool reached
Definition: NaturalDiffIK.h:85
armarx::CartesianNaturalPositionControllerProxy::DynamicKp::sigmaMM
float sigmaMM
Definition: CartesianNaturalPositionControllerProxy.h:46
armarx::CartesianNaturalPositionControllerProxy::VelocityBaseSettings::baseJointVelocity
Ice::FloatSeq baseJointVelocity
Definition: CartesianNaturalPositionControllerProxy.h:70
armarx::CartesianNaturalPositionControllerProxy::DynamicKp::calculate
void calculate(float error, float &KpElb, float &KpJla)
Definition: CartesianNaturalPositionControllerProxy.cpp:282
armarx::CartesianNaturalPositionControllerProxy::getCurrentElbowTargetPosition
Eigen::Vector3f getCurrentElbowTargetPosition()
Definition: CartesianNaturalPositionControllerProxy.cpp:158
armarx::CartesianNaturalPositionControllerProxy::clearWaypoints
void clearWaypoints()
Definition: CartesianNaturalPositionControllerProxy.cpp:352
armarx::CartesianPositionController::GetOrientationError
static float GetOrientationError(const Eigen::Matrix4f &targetPose, const VirtualRobot::RobotNodePtr &tcp, const VirtualRobot::RobotNodePtr &referenceFrame=nullptr)
Definition: CartesianPositionController.cpp:129
armarx::CartesianNaturalPositionControllerProxy::KpBaseSettings
Definition: CartesianNaturalPositionControllerProxy.h:50
armarx::NaturalIK::ArmJoints
Definition: NaturalIK.h:65
armarx::CartesianNaturalPositionControllerProxy::currentWaypoint
Waypoint & currentWaypoint()
Definition: CartesianNaturalPositionControllerProxy.cpp:511
armarx::CartesianNaturalPositionControllerProxy::DynamicKp
Definition: CartesianNaturalPositionControllerProxy.h:42
armarx::CartesianNaturalPositionControllerProxy::getCurrentTargetPosition
Eigen::Vector3f getCurrentTargetPosition()
Definition: CartesianNaturalPositionControllerProxy.cpp:152
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::CartesianNaturalPositionControllerProxy::VelocityBaseSettings::baseOriVel
float baseOriVel
Definition: CartesianNaturalPositionControllerProxy.h:72
armarx::CartesianNaturalPositionControllerProxy::Waypoint::reached
bool reached(const VirtualRobot::RobotNodePtr &tcp)
Definition: CartesianNaturalPositionControllerProxy.cpp:543
armarx::CartesianNaturalPositionControllerProxy::DynamicKp::maxKp
float maxKp
Definition: CartesianNaturalPositionControllerProxy.h:45
armarx::NaturalIK
The NaturalIK class.
Definition: NaturalIK.h:52
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::CartesianNaturalPositionControllerProxy::setMaxVelocities
void setMaxVelocities(float referencePosVel)
Definition: CartesianNaturalPositionControllerProxy.cpp:423
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:855
armarx::CartesianNaturalPositionControllerProxy::addWaypoint
void addWaypoint(const Waypoint &waypoint)
Definition: CartesianNaturalPositionControllerProxy.cpp:306
armarx::NaturalIK::Parameters::diffIKparams
NaturalDiffIK::Parameters diffIKparams
Definition: NaturalIK.h:61
armarx::CartesianNaturalPositionControllerProxy::Waypoint::setConfig
Waypoint & setConfig(const WaypointConfig &config)
Definition: CartesianNaturalPositionControllerProxy.cpp:554
armarx::CartesianPositionController::GetPositionError
static float GetPositionError(const Eigen::Matrix4f &targetPose, const VirtualRobot::RobotNodePtr &tcp, const VirtualRobot::RobotNodePtr &referenceFrame=nullptr)
Definition: CartesianPositionController.cpp:116
armarx::CartesianNaturalPositionControllerProxy::WaypointConfig::referencePosVel
float referencePosVel
Definition: CartesianNaturalPositionControllerProxy.h:77
armarx::CartesianNaturalPositionControllerProxy::updateBaseKpValues
void updateBaseKpValues(const CartesianNaturalPositionControllerConfig &runCfg)
Definition: CartesianNaturalPositionControllerProxy.cpp:447
armarx::NaturalDiffIK::Result::jointValues
Eigen::VectorXf jointValues
Definition: NaturalDiffIK.h:78
armarx::CartesianNaturalPositionControllerProxy::getStatusText
std::string getStatusText()
Definition: CartesianNaturalPositionControllerProxy.cpp:366
armarx::CartesianNaturalPositionControllerProxy::VelocityBaseSettings
Definition: CartesianNaturalPositionControllerProxy.h:62
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::CartesianNaturalPositionControllerProxy::WaypointTargets::userNullspaceTargets
std::vector< float > userNullspaceTargets
Definition: CartesianNaturalPositionControllerProxy.h:88
armarx::CartesianNaturalPositionControllerProxy::disableFTLimit
void disableFTLimit()
Definition: CartesianNaturalPositionControllerProxy.cpp:211
armarx::CartesianNaturalPositionControllerProxy::getCurrentFTValue
FTSensorValue getCurrentFTValue(bool substactOffset)
Definition: CartesianNaturalPositionControllerProxy.cpp:218
armarx::CartesianNaturalPositionControllerProxy::update
bool update()
Definition: CartesianNaturalPositionControllerProxy.cpp:477
armarx::NaturalIK::ArmJoints::rns
VirtualRobot::RobotNodeSetPtr rns
Definition: NaturalIK.h:67
armarx::CartesianNaturalPositionControllerProxy::Waypoint::setNullspaceTargets
Waypoint & setNullspaceTargets(const std::vector< float > &userNullspaceTargets)
Definition: CartesianNaturalPositionControllerProxy.cpp:562
armarx::CartesianNaturalPositionControllerProxy::getDynamicKp
DynamicKp getDynamicKp()
Definition: CartesianNaturalPositionControllerProxy.cpp:537
armarx::CartesianNaturalPositionControllerProxy::KpBaseSettings::baseKpElbPos
float baseKpElbPos
Definition: CartesianNaturalPositionControllerProxy.h:54
CartesianPositionController.h
armarx::CartesianNaturalPositionControllerProxy::KpBaseSettings::baseKpTcpPos
float baseKpTcpPos
Definition: CartesianNaturalPositionControllerProxy.h:52
armarx::CartesianNaturalPositionControllerProxy::createWaypoint
Waypoint createWaypoint(const Eigen::Vector3f &tcpTarget, const std::vector< float > &userNullspaceTargets, std::optional< float > minElbowHeight=std::nullopt)
Definition: CartesianNaturalPositionControllerProxy.cpp:317
armarx::CartesianNaturalPositionControllerProxy::ScaleVec
static std::vector< float > ScaleVec(const std::vector< float > &vec, float scale)
Definition: CartesianNaturalPositionControllerProxy.cpp:406
armarx::CartesianNaturalPositionControllerProxy::DynamicKp::enabled
bool enabled
Definition: CartesianNaturalPositionControllerProxy.h:44
armarx::NaturalIK::SoechtingForwardPositions::elbow
Eigen::Vector3f elbow
Definition: NaturalIK.h:97
ExpressionException.h
armarx::CartesianNaturalPositionControllerProxy::getElbPositionError
float getElbPositionError()
Definition: CartesianNaturalPositionControllerProxy.cpp:180
armarx::CartesianNaturalPositionControllerProxy::setNullspaceTarget
void setNullspaceTarget(const std::vector< float > &nullspaceTargets)
Definition: CartesianNaturalPositionControllerProxy.cpp:143
armarx::CartesianNaturalPositionControllerProxy::MakeConfig
static NJointCartesianNaturalPositionControllerConfigPtr MakeConfig(const std::string &rns, const std::string &elbowNode)
Definition: CartesianNaturalPositionControllerProxy.cpp:77
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
armarx::CartesianNaturalPositionControllerProxy::getTcpOrientationError
float getTcpOrientationError()
Definition: CartesianNaturalPositionControllerProxy.cpp:170
armarx::CartesianNaturalPositionControllerProxy::Waypoint::targets
WaypointTargets targets
Definition: CartesianNaturalPositionControllerProxy.h:95
armarx::CartesianNaturalPositionControllerProxy::WaypointConfig::thresholdTcpPosReached
float thresholdTcpPosReached
Definition: CartesianNaturalPositionControllerProxy.h:78
armarx::CartesianNaturalPositionControllerProxy::useCurrentFTasOffset
void useCurrentFTasOffset()
Definition: CartesianNaturalPositionControllerProxy.cpp:192
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
armarx::CartesianNaturalPositionControllerProxy::KpBaseSettings::maxNullspaceAcceleration
float maxNullspaceAcceleration
Definition: CartesianNaturalPositionControllerProxy.h:59
armarx::CartesianNaturalPositionControllerProxy::stopClear
void stopClear()
Definition: CartesianNaturalPositionControllerProxy.cpp:242
armarx::CartesianNaturalPositionControllerProxy::Waypoint::config
WaypointConfig config
Definition: CartesianNaturalPositionControllerProxy.h:96
armarx::CartesianNaturalPositionControllerProxy::init
void init()
Definition: CartesianNaturalPositionControllerProxy.cpp:88
IceInternal::ProxyHandle<::IceProxy::armarx::RobotUnitInterface >
armarx::CartesianNaturalPositionControllerProxy::Waypoint
Definition: CartesianNaturalPositionControllerProxy.h:92
armarx::CartesianNaturalPositionControllerProxy::setDynamicKp
void setDynamicKp(DynamicKp dynamicKp)
Definition: CartesianNaturalPositionControllerProxy.cpp:530
armarx::NaturalDiffIK::CalculateDiffIK
static Result CalculateDiffIK(const Eigen::Matrix4f &targetPose, const Eigen::Vector3f &elbowTarget, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, VirtualRobot::RobotNodePtr elbow, Mode setOri, Parameters params=Parameters())
Definition: NaturalDiffIK.cpp:50
armarx::CartesianNaturalPositionControllerProxy::setRuntimeConfig
void setRuntimeConfig(CartesianNaturalPositionControllerConfig runCfg)
Definition: CartesianNaturalPositionControllerProxy.cpp:292
armarx::CartesianPositionController::Reached
static bool Reached(const Eigen::Matrix4f &targetPose, const VirtualRobot::RobotNodePtr &tcp, bool checkOri, float thresholdPosReached, float thresholdOriReached, const VirtualRobot::RobotNodePtr &referenceFrame=nullptr)
Definition: CartesianPositionController.cpp:145
armarx::NaturalDiffIK::Result
Definition: NaturalDiffIK.h:76
armarx::CartesianNaturalPositionControllerProxy::setDefaultWaypointConfig
void setDefaultWaypointConfig(const WaypointConfig &config)
Definition: CartesianNaturalPositionControllerProxy.cpp:359
armarx::CartesianNaturalPositionControllerProxy::isLastWaypoint
bool isLastWaypoint()
Definition: CartesianNaturalPositionControllerProxy.cpp:518
Logging.h
armarx::navigation::core::Position
Eigen::Vector3f Position
Definition: basic_types.h:36
armarx::CartesianNaturalPositionControllerProxy::getTcpPositionError
float getTcpPositionError()
Definition: CartesianNaturalPositionControllerProxy.cpp:164
armarx::CartesianNaturalPositionControllerProxy::WaypointTargets::tcpTarget
Eigen::Matrix4f tcpTarget
Definition: CartesianNaturalPositionControllerProxy.h:85
armarx::CartesianNaturalPositionControllerProxy::getAverageFTValue
FTSensorValue getAverageFTValue(bool substactOffset)
Definition: CartesianNaturalPositionControllerProxy.cpp:230
armarx::CartesianNaturalPositionControllerProxy::KpBaseSettings::baseKpTcpOri
float baseKpTcpOri
Definition: CartesianNaturalPositionControllerProxy.h:53
armarx::CartesianNaturalPositionControllerProxy::CalculateMaxJointVelocity
static std::vector< float > CalculateMaxJointVelocity(const VirtualRobot::RobotNodeSetPtr &rns, float maxPosVel)
Definition: CartesianNaturalPositionControllerProxy.cpp:377
armarx::CartesianNaturalPositionControllerProxy::ScopedJointValueRestore
Definition: CartesianNaturalPositionControllerProxy.h:109
armarx::CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy
CartesianNaturalPositionControllerProxy(const NaturalIK &_ik, const NaturalIK::ArmJoints &arm, const RobotUnitInterfacePrx &_robotUnit, const std::string &_controllerName, NJointCartesianNaturalPositionControllerConfigPtr _config)
Definition: CartesianNaturalPositionControllerProxy.cpp:39
armarx::NaturalIK::solveSoechtingIK
NaturalIK::SoechtingForwardPositions solveSoechtingIK(const Eigen::Vector3f &targetPos, std::optional< float > minElbowHeight=std::nullopt)
Definition: NaturalIK.cpp:43
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::CartesianNaturalPositionControllerProxy::KpBaseSettings::baseKpJla
float baseKpJla
Definition: CartesianNaturalPositionControllerProxy.h:55
armarx::CartesianNaturalPositionControllerProxy::KpBaseSettings::maxOrientationAcceleration
float maxOrientationAcceleration
Definition: CartesianNaturalPositionControllerProxy.h:58
armarx::CartesianNaturalPositionControllerProxy::setActivateControllerOnInit
void setActivateControllerOnInit(bool value)
Definition: CartesianNaturalPositionControllerProxy.cpp:417
armarx::CartesianNaturalPositionControllerProxy::ScopedJointValueRestore::ScopedJointValueRestore
ScopedJointValueRestore(const VirtualRobot::RobotNodeSetPtr &rns)
Definition: CartesianNaturalPositionControllerProxy.cpp:606
norm
double norm(const Point &a)
Definition: point.hpp:102