NaturalIKTest.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package RobotAPI::ArmarXObjects::NaturalIKTest
17  * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
18  * @date 2020
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "NaturalIKTest.h"
30 #include <VirtualRobot/Robot.h>
31 #include <VirtualRobot/XML/RobotIO.h>
32 #include <VirtualRobot/math/Helpers.h>
33 
34 #include <random>
35 
36 #include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
37 
38 #include <VirtualRobot/IK/constraints/OrientationConstraint.h>
39 #include <VirtualRobot/IK/constraints/PoseConstraint.h>
40 #include <VirtualRobot/IK/constraints/PositionConstraint.h>
41 
43 
45 
46 namespace armarx
47 {
50  {
51  //defineRequiredProperty<std::string>("PropertyName", "Description");
52  //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
53 
54  defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
55  defineOptionalProperty<std::string>("ArVizTopicName", "ArVizTopic", "Layer updates are sent over this topic.");
56 
57  //defineOptionalProperty<std::string>("MyProxyName", "MyProxy", "Name of the proxy to be used");
58  }
59 
60 
61  std::string NaturalIKTest::getDefaultName() const
62  {
63  return "NaturalIKTest";
64  }
65 
66 
68  {
69  // Register offered topices and used proxies here.
70  offeringTopicFromProperty("DebugObserverName");
71  // debugDrawer.offeringTopic(*this); // Calls this->offeringTopic().
72 
73  // Using a proxy will cause the component to wait until the proxy is available.
74  // usingProxyFromProperty("MyProxyName");
75 
76  offeringTopicFromProperty("ArVizTopicName");
77  }
78 
79 
81  {
82  // Get topics and proxies here. Pass the *InterfacePrx type as template argument.
83  debugObserver = getTopicFromProperty<DebugObserverInterfacePrx>("DebugObserverName");
84  // debugDrawer.getTopic(*this); // Calls this->getTopic().
85 
86  // getProxyFromProperty<MyProxyInterfacePrx>("MyProxyName");
87 
89  {
90  processGui(prx);
91  });
92 
94  vizTask->start();
95  }
96 
98  {
99  vizTask->stop();
100  vizTask = nullptr;
101  }
102 
104  {
107  .addTextLabel("L.X")
108  .addChild(RemoteGui::makeFloatSlider("L.X").min(-600).max(600).value(0))
109 
110  .addTextLabel("L.Y")
111  .addChild(RemoteGui::makeFloatSlider("L.Y").min(200).max(1200).value(600))
112 
113  .addTextLabel("L.Z")
114  .addChild(RemoteGui::makeFloatSlider("L.Z").min(-600).max(600).value(-200))
115 
116  .addTextLabel("L.scale")
117  .addChild(RemoteGui::makeFloatSlider("L.scale").min(0.5).max(2).value(1.3f))
118 
119  .addTextLabel("L.minElbZ")
120  .addChild(RemoteGui::makeFloatSlider("L.minElbZ").min(500).max(2000).value(500))
121 
122  .addChild(RemoteGui::makeCheckBox("L.setOri").value(true))
123  .addChild(new RemoteGui::HSpacer)
124 
125  .addTextLabel("L.rX")
126  .addChild(RemoteGui::makeFloatSlider("L.rX").min(-180).max(180).value(0))
127 
128  .addTextLabel("L.rY")
129  .addChild(RemoteGui::makeFloatSlider("L.rY").min(-180).max(180).value(0))
130 
131  .addTextLabel("L.rZ")
132  .addChild(RemoteGui::makeFloatSlider("L.rZ").min(-180).max(180).value(0))
133  );
136  .addTextLabel("R.X")
137  .addChild(RemoteGui::makeFloatSlider("R.X").min(-600).max(600).value(0))
138 
139  .addTextLabel("R.Y")
140  .addChild(RemoteGui::makeFloatSlider("R.Y").min(200).max(1200).value(600))
141 
142  .addTextLabel("R.Z")
143  .addChild(RemoteGui::makeFloatSlider("R.Z").min(-600).max(600).value(-200))
144 
145  .addTextLabel("R.scale")
146  .addChild(RemoteGui::makeFloatSlider("R.scale").min(0.5).max(2).value(1.3f))
147 
148  .addTextLabel("R.minElbZ")
149  .addChild(RemoteGui::makeFloatSlider("R.minElbZ").min(500).max(2000).value(500))
150 
151  .addChild(RemoteGui::makeCheckBox("R.setOri").value(true))
152  .addChild(new RemoteGui::HSpacer)
153 
154  .addTextLabel("R.rX")
155  .addChild(RemoteGui::makeFloatSlider("R.rX").min(-180).max(180).value(0))
156 
157  .addTextLabel("R.rY")
158  .addChild(RemoteGui::makeFloatSlider("R.rY").min(-180).max(180).value(0))
159 
160  .addTextLabel("R.rZ")
161  .addChild(RemoteGui::makeFloatSlider("R.rZ").min(-180).max(180).value(0))
162  );
163 
165  .addChild(RemoteGui::makeCheckBox("useCompositeIK").value(false))
166  .addChild(RemoteGui::makeSimpleGridLayout().cols(2).addChild(leftBox).addChild(rightBox))
168  .addTextLabel("elbowKp")
169  .addChild(RemoteGui::makeFloatSlider("elbowKp").min(0).max(1).value(1))
170 
171  .addTextLabel("jlaKp")
172  .addChild(RemoteGui::makeFloatSlider("jlaKp").min(0).max(1).value(0))
173  )
174  .addChild(RemoteGui::makeLabel("errors").value("<errors>"))
175  .addChild(RemoteGui::makeButton("runTest").label("run test"));
176  }
177 
179  {
180  prx.receiveUpdates();
181 
182  p.useCompositeIK = prx.getValue<bool>("useCompositeIK").get();
183 
184  float x, y, z, rx, ry, rz;
185 
186  x = prx.getValue<float>("R.X").get();
187  y = prx.getValue<float>("R.Y").get();
188  z = prx.getValue<float>("R.Z").get();
189  p.p_R.target = Eigen::Vector3f(x, y, z);
190  p.p_R.scale = prx.getValue<float>("R.scale").get();
191  p.p_R.ikparams.minimumElbowHeight = prx.getValue<float>("R.minElbZ").get();
192  p.p_R.setOri = prx.getValue<bool>("R.setOri").get();
193  rx = prx.getValue<float>("R.rX").get();
194  ry = prx.getValue<float>("R.rY").get();
195  rz = prx.getValue<float>("R.rZ").get();
196  p.p_R.targetRotation = Eigen::Vector3f(rx, ry, rz);
197 
198  x = prx.getValue<float>("L.X").get();
199  y = prx.getValue<float>("L.Y").get();
200  z = prx.getValue<float>("L.Z").get();
201  p.p_L.target = Eigen::Vector3f(x, y, z);
202  p.p_L.scale = prx.getValue<float>("L.scale").get();
203  p.p_L.ikparams.minimumElbowHeight = prx.getValue<float>("L.minElbZ").get();
204  p.p_L.setOri = prx.getValue<bool>("L.setOri").get();
205  rx = prx.getValue<float>("L.rX").get();
206  ry = prx.getValue<float>("L.rY").get();
207  rz = prx.getValue<float>("L.rZ").get();
208  p.p_L.targetRotation = Eigen::Vector3f(rx, ry, rz);
209 
210 
211  p.p_R.ikparams.diffIKparams.elbowKp = prx.getValue<float>("elbowKp").get();
212  p.p_R.ikparams.diffIKparams.jointLimitAvoidanceKp = prx.getValue<float>("jlaKp").get();
214 
215  p.p_L.ikparams.diffIKparams.elbowKp = prx.getValue<float>("elbowKp").get();
216  p.p_L.ikparams.diffIKparams.jointLimitAvoidanceKp = prx.getValue<float>("jlaKp").get();
218 
219 
220  p.targetValid = true;
221  std::stringstream ss;
222  ss << "err_R: " << p.err_R(0) << " " << p.err_R(1) << " " << p.err_R(2) << "\n";
223  ss << "err_L: " << p.err_L(0) << " " << p.err_L(1) << " " << p.err_L(2) << "";
224  prx.getValue<std::string>("errors").set(ss.str());
225 
226  bool runTest = prx.getButtonClicked("runTest");
227  if (runTest)
228  {
229  runTestTask = new RunningTask<NaturalIKTest>(this, &NaturalIKTest::testTaskRun);
230  runTestTask->start();
231  }
232 
233 
234  prx.sendUpdates();
235  }
236 
237  struct IKStats
238  {
239  IKStats(const std::string& name) : name(name) {}
240  std::string name;
241  int solved = 0;
242  float duration = 0;
243  std::vector<Eigen::Vector3f> elbow;
244  Eigen::Vector3f averageElb()
245  {
246  Eigen::Vector3f elb = Eigen::Vector3f ::Zero();
247  for (auto e : elbow)
248  {
249  elb += e;
250  }
251  return elb / elbow.size();
252  }
253  };
254 
255  struct SoftPositionConstraint : public VirtualRobot::PositionConstraint
256  {
257  public:
258  SoftPositionConstraint(const VirtualRobot::RobotPtr& robot, const VirtualRobot::RobotNodeSetPtr& nodeSet, const VirtualRobot::SceneObjectPtr& node, const VirtualRobot::SceneObjectPtr& tcp, const Eigen::Vector3f& target,
259  VirtualRobot::IKSolver::CartesianSelection cartesianSelection = VirtualRobot::IKSolver::All, float tolerance = 3.0f)
260  : PositionConstraint(robot, nodeSet, node, target, cartesianSelection, tolerance), tcp(tcp)
261  {
262  optimizationFunctions.clear();
263 
264  addOptimizationFunction(0, true);
265  }
267 
268 
269  Eigen::VectorXf optimizationGradient(unsigned int id) override
270  {
271  int size = nodeSet->getSize();
272  (void) size;
273 
274  Eigen::MatrixXf J = ik->getJacobianMatrix(tcp);//.block(0, 0, 3, size);
275  //Eigen::Vector3f d = eef->getGlobalPose().block<3,1>(0,3) - target;
276 
277  Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(J);
278  Eigen::MatrixXf nullspace = lu_decomp.kernel();
279 
280  Eigen::VectorXf grad = PositionConstraint::optimizationGradient(id);
281 
282  Eigen::VectorXf mapped = Eigen::VectorXf::Zero(grad.size());
283  for (int i = 0; i < nullspace.cols(); i++)
284  {
285  float squaredNorm = nullspace.col(i).squaredNorm();
286  // Prevent division by zero
287  if (squaredNorm > 1.0e-32f)
288  {
289  mapped += nullspace.col(i) * nullspace.col(i).dot(grad) / nullspace.col(i).squaredNorm();
290  }
291  }
292  return mapped;
293  }
294 
295  bool checkTolerances() override
296  {
297  return true;
298  }
299  };
300 
302  {
303  CMakePackageFinder finder("armar6_rt");
304  std::string robotFile = finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
305  ARMARX_IMPORTANT << "loading robot from " << robotFile;
306  VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
307 
308  VirtualRobot::RobotNodeSetPtr rns_R = robot->getRobotNodeSet("RightArm");
309  VirtualRobot::RobotNodeSetPtr rns_L = robot->getRobotNodeSet("LeftArm");
310 
311  VirtualRobot::RobotNodePtr sho1_R = rns_R->getNode(1);
312  VirtualRobot::RobotNodePtr sho1_L = rns_L->getNode(1);
313 
314  VirtualRobot::RobotNodePtr elb_R = rns_R->getNode(4);
315  VirtualRobot::RobotNodePtr wri1_R = rns_R->getNode(6);
316  VirtualRobot::RobotNodePtr tcp_R = rns_R->getTCP();
317 
318 
319  float upper_arm_length = (sho1_R->getPositionInRootFrame() - elb_R->getPositionInRootFrame()).norm();
320  //float lower_arm_length = (elb_R->getPositionInRootFrame() - wri1_R->getPositionInRootFrame()).norm();
321  float lower_arm_length = (elb_R->getPositionInRootFrame() - rns_R->getTCP()->getPositionInRootFrame()).norm();
322 
323 
324 
325 
326  Eigen::Vector3f shoulder_R = sho1_R->getPositionInRootFrame();
327  Eigen::Vector3f shoulder_L = sho1_L->getPositionInRootFrame();
328 
329  Eigen::Vector3f offset = (shoulder_R + shoulder_L) / 2;
330  (void) offset;
331 
332  NaturalIK ik_R("Right", shoulder_R, 1);
333  NaturalIK ik_L("Left", shoulder_L, 1);
334 
335  ik_R.setUpperArmLength(upper_arm_length);
336  ik_R.setLowerArmLength(lower_arm_length);
337  ik_L.setUpperArmLength(upper_arm_length);
338  ik_L.setLowerArmLength(lower_arm_length);
339 
340 
341  NaturalIK::ArmJoints arm_R;
342  arm_R.rns = rns_R;
343  arm_R.elbow = elb_R;
344  arm_R.tcp = rns_R->getTCP();
345 
346  std::vector<NaturalIK::Parameters> ikParamList;
347  std::vector<IKStats> ikStats = {IKStats("NaturalIK"), IKStats("SimpleDiffIK"), IKStats("OptIK"), IKStats("DiffIK"), IKStats("CompositeIK")};
348 
350  p0.diffIKparams.resetRnsValues = false;
354 
355  {
356  NaturalIK::Parameters p = p0;
357  p.diffIKparams.resetRnsValues = false;
358  p.diffIKparams.elbowKp = 1;
361  ikParamList.emplace_back(p);
362  }
363  {
364  NaturalIK::Parameters p = p0;
365  p.diffIKparams.resetRnsValues = false;
366  p.diffIKparams.elbowKp = 0;
368  ikParamList.emplace_back(p);
369  }
370 
372  pc.resetRnsValues = false;
373 
374 
375 
376  std::random_device rd; //Will be used to obtain a seed for the random number engine
377  std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
378  std::uniform_real_distribution<> dis(0.0, 1.0);
379 
380  std::vector<Eigen::VectorXf> initialConfigurations;
381  for (VirtualRobot::RobotNodePtr rn : rns_R->getAllRobotNodes())
382  {
383  if (rn->isLimitless())
384  {
385  rn->setJointValue(0);
386  }
387  else
388  {
389  rn->setJointValue((rn->getJointLimitHi() + rn->getJointLimitLo()) / 2);
390  }
391  }
392 
393  rns_R->getNode(1)->setJointValue(0);
394  rns_R->getNode(3)->setJointValue(0);
395  rns_R->getNode(5)->setJointValue(0);
396  initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
397  rns_R->getNode(1)->setJointValue(0);
398  rns_R->getNode(3)->setJointValue(0);
399  rns_R->getNode(5)->setJointValue(M_PI);
400  initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
401  rns_R->getNode(1)->setJointValue(0);
402  rns_R->getNode(3)->setJointValue(M_PI);
403  rns_R->getNode(5)->setJointValue(0);
404  initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
405  rns_R->getNode(1)->setJointValue(0);
406  rns_R->getNode(3)->setJointValue(M_PI);
407  rns_R->getNode(5)->setJointValue(M_PI);
408  initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
409 
410 
411 
412  std::vector<Eigen::Matrix4f> targets;
413  while (targets.size() < 100)
414  {
415  for (size_t i = 0; i < rns_R->getSize(); i++)
416  {
417  if (rns_R->getNode(i)->isLimitless())
418  {
419  rns_R->getNode(i)->setJointValue(dis(gen) * 2 * M_PI);
420  }
421  else
422  {
423  float lo = rns_R->getNode(i)->getJointLimitLo();
424  float hi = rns_R->getNode(i)->getJointLimitHi();
425  rns_R->getNode(i)->setJointValue(dis(gen) * (hi - lo) + lo);
426  }
427  }
428  Eigen::Vector3f tcpPos = tcp_R->getPositionInRootFrame();
429  if (tcpPos.y() > 0)
430  {
431  targets.emplace_back(tcp_R->getPoseInRootFrame());
432  }
433  }
434 
435 
436  IceUtil::Time start;
437  for (size_t i = 0; i < ikParamList.size(); i++)
438  {
439  start = TimeUtil::GetTime();
440  for (size_t n = 0; n < targets.size(); n++)
441  {
442  for (const Eigen::VectorXf& initjv : initialConfigurations)
443  {
444  rns_R->setJointValues(initjv);
445  NaturalDiffIK::Result ikResult = ik_R.calculateIK(targets.at(n), arm_R, ikParamList.at(i));
446  if (ikResult.reached)
447  {
448  ikStats.at(i).solved = ikStats.at(i).solved + 1;
449  ikStats.at(i).elbow.emplace_back(elb_R->getPositionInRootFrame());
450  break;
451  }
452  }
453  }
454  ikStats.at(i).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
455  }
456  start = TimeUtil::GetTime();
457 
458  std::vector<VirtualRobot::RobotNodePtr> elbJoints_R;
459  for (int i = 0; i < 4; i++)
460  {
461  elbJoints_R.push_back(rns_R->getNode(i));
462  }
463  VirtualRobot::RobotNodeSetPtr rnsElb_R = VirtualRobot::RobotNodeSet::createRobotNodeSet(robot, "ELB_R", elbJoints_R, rns_R->getKinematicRoot(), elb_R);
464  for (size_t n = 0; n < targets.size(); n++)
465  {
466  rns_R->setJointValues(initialConfigurations.at(0));
467  VirtualRobot::ConstrainedOptimizationIK optIK(robot, rns_R);
468  //VirtualRobot::ConstraintPtr conPose(new VirtualRobot::PoseConstraint(robot, rns_R, tcp_R, targets.at(n)));
469  //optIK.addConstraint(conPose);
470  VirtualRobot::ConstraintPtr posConstraint(new VirtualRobot::PositionConstraint(robot, rns_R, tcp_R,
471  math::Helpers::GetPosition(targets.at(n)), VirtualRobot::IKSolver::CartesianSelection::All));
472  posConstraint->setOptimizationFunctionFactor(1);
473 
474  VirtualRobot::ConstraintPtr oriConstraint(new VirtualRobot::OrientationConstraint(robot, rns_R, tcp_R,
475  math::Helpers::GetOrientation(targets.at(n)), VirtualRobot::IKSolver::CartesianSelection::All, 2.f / 180 * M_PI)); // was VirtualRobot::MathTools::deg2rad(...)
476  oriConstraint->setOptimizationFunctionFactor(1000);
477 
478 
479  Eigen::Vector3f elbowPos = ik_R.solveSoechtingIK(math::Helpers::GetPosition(targets.at(n))).elbow;
480 
481  VirtualRobot::ConstraintPtr elbConstraint(new SoftPositionConstraint(robot, rns_R, elb_R, tcp_R,
482  elbowPos, VirtualRobot::IKSolver::CartesianSelection::All));
483  elbConstraint->setOptimizationFunctionFactor(0.01);
484  //elbConstraint->
485 
486  //ARMARX_IMPORTANT << VAROUT(elbConstraint->optimizationFunction(0));
487  //ARMARX_IMPORTANT << VAROUT(elbConstraint->optimizationGradient(0));
488 
489  optIK.addConstraint(posConstraint);
490  optIK.addConstraint(oriConstraint);
491  //optIK.addConstraint(elbConstraint);
492 
493  optIK.initialize();
494  bool reached = optIK.solve();
495  if (reached)
496  {
497  ikStats.at(2).solved = ikStats.at(2).solved + 1;
498  ikStats.at(2).elbow.emplace_back(elb_R->getPositionInRootFrame());
499  //float perr = CartesianPositionController::GetPositionError(targets.at(n), tcp_R);
500  //float oerr = 180 / M_PI * CartesianPositionController::GetOrientationError(targets.at(n), tcp_R);
501  //ARMARX_IMPORTANT << VAROUT(perr) << VAROUT(oerr);
502  //ARMARX_IMPORTANT << VAROUT(targets.at(n)) << VAROUT(tcp_R->getPoseInRootFrame());
503  }
504  }
505  ikStats.at(2).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
506 
507  start = TimeUtil::GetTime();
508  for (size_t n = 0; n < targets.size(); n++)
509  {
510  rns_R->setJointValues(initialConfigurations.at(0));
511  VirtualRobot::DifferentialIK diffIK(rns_R);
512  diffIK.setGoal(targets.at(n));
513  bool reached = diffIK.solveIK();
514  if (reached)
515  {
516  ikStats.at(3).solved = ikStats.at(3).solved + 1;
517  ikStats.at(3).elbow.emplace_back(elb_R->getPositionInRootFrame());
518  }
519  }
520  ikStats.at(3).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
521 
522  start = TimeUtil::GetTime();
523  for (size_t n = 0; n < targets.size(); n++)
524  {
525  for (const Eigen::VectorXf& initjv : initialConfigurations)
526  {
527  rns_R->setJointValues(initjv);
528  CompositeDiffIK ik(rns_R);
529  CompositeDiffIK::TargetPtr t(new CompositeDiffIK::Target(rns_R, tcp_R, targets.at(n), VirtualRobot::IKSolver::CartesianSelection::All));
530  ik.addTarget(t);
532  nsjt->set(2, 0.2, 1);
533  ik.addNullspaceGradient(nsjt);
535  nsjla->setWeight(2, 0);
536  ik.addNullspaceGradient(nsjla);
537  CompositeDiffIK::Result result = ik.solve(pc);
538  if (result.reached)
539  {
540  ikStats.at(4).solved = ikStats.at(4).solved + 1;
541  ikStats.at(4).elbow.emplace_back(elb_R->getPositionInRootFrame());
542  break;
543  }
544  }
545  }
546  ikStats.at(4).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
547 
548  for (size_t i = 0; i < ikStats.size(); i++)
549  {
550  ARMARX_IMPORTANT << ikStats.at(i).name << " solved: " << ikStats.at(i).solved << ", T: " << ikStats.at(i).duration;
551  }
552 
553  //ARMARX_IMPORTANT << "NaturalIK solved: " << ikStats.at(0).solved << ", T: " << durations.at(0);
554  //ARMARX_IMPORTANT << "SimpleDiffIK solved: " << ikStats.at(1).solved;
555  //ARMARX_IMPORTANT << "OptIK solved: " << ikStats.at(2).solved;
556  //ARMARX_IMPORTANT << "DiffIK solved: " << ikStats.at(3).solved;
557 
558  //ARMARX_IMPORTANT << VAROUT(ikStats.at(0).averageElb());
559  //ARMARX_IMPORTANT << VAROUT(ikStats.at(1).solved);
560  //ARMARX_IMPORTANT << VAROUT(ikStats.at(1).averageElb());
561 
562  }
563 
564  struct Side
565  {
567  Eigen::Vector3f shoulder;
568 
569  };
570 
572  {
573  float SE, SR;
575  };
576 
578  {
579  //ARMARX_IMPORTANT << "vizTask starts";
580  viz::Client arviz(*this);
581 
582  viz::Layer layer_iksteps = arviz.layer("ikSteps");
583 
584  viz::Layer layer_robot = arviz.layer("Robot");
585  viz::Robot vizrobot = viz::Robot("robot")
586  .position(Eigen::Vector3f::Zero())
587  .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
588  vizrobot.useFullModel();
589  layer_robot.add(vizrobot);
590 
591  CMakePackageFinder finder("armar6_rt");
592  std::string robotFile = finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
593  ARMARX_IMPORTANT << "loading robot from " << robotFile;
594  VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
595 
596  VirtualRobot::RobotNodeSetPtr rns_R = robot->getRobotNodeSet("RightArm");
597  VirtualRobot::RobotNodeSetPtr rns_L = robot->getRobotNodeSet("LeftArm");
598 
599  VirtualRobot::RobotNodePtr sho1_R = rns_R->getNode(1);
600  VirtualRobot::RobotNodePtr sho1_L = rns_L->getNode(1);
601 
602  VirtualRobot::RobotNodePtr elb_R = rns_R->getNode(4);
603  VirtualRobot::RobotNodePtr wri1_R = rns_R->getNode(6);
604 
605  VirtualRobot::RobotNodePtr elb_L = rns_L->getNode(4);
606 
607 
608  //float value = 0.5f * (1.0f + std::sin(timeInSeconds));
609  //robot.joint("ArmR2_Sho1", value);
610  //robot.joint("ArmR3_Sho2", value);
611 
612  //layer.add(robot);
613 
614  float upper_arm_length = (sho1_R->getPositionInRootFrame() - elb_R->getPositionInRootFrame()).norm();
615  //float lower_arm_length = (elb_R->getPositionInRootFrame() - wri1_R->getPositionInRootFrame()).norm();
616  float lower_arm_length = (elb_R->getPositionInRootFrame() - rns_R->getTCP()->getPositionInRootFrame()).norm();
617 
618 
619 
620  viz::Layer layer_R = arviz.layer("Right");
621  viz::Layer layer_L = arviz.layer("Left");
622 
623  //Eigen::Vector3f shoulder_R(NaturalIK::SoechtingAngles::MMM_SHOULDER_POS, 0, 0);
624  //Eigen::Vector3f shoulder_L(-NaturalIK::SoechtingAngles::MMM_SHOULDER_POS, 0, 0);
625  Eigen::Vector3f shoulder_R = sho1_R->getPositionInRootFrame();
626  Eigen::Vector3f shoulder_L = sho1_L->getPositionInRootFrame();
627 
628  Eigen::Vector3f offset = (shoulder_R + shoulder_L) / 2;
629 
630  NaturalIK ik_R("Right", shoulder_R, 1);
631  NaturalIK ik_L("Left", shoulder_L, 1);
632 
633  ik_R.setUpperArmLength(upper_arm_length);
634  ik_R.setLowerArmLength(lower_arm_length);
635  ik_L.setUpperArmLength(upper_arm_length);
636  ik_L.setLowerArmLength(lower_arm_length);
637 
638 
639 
640  NaturalIK::ArmJoints arm_R;
641  arm_R.rns = rns_R;
642  arm_R.elbow = elb_R;
643  arm_R.tcp = rns_R->getTCP();
644 
645  NaturalIK::ArmJoints arm_L;
646  arm_L.rns = rns_L;
647  arm_L.elbow = elb_L;
648  arm_L.tcp = rns_L->getTCP();
649 
650 
651  auto makeVizSide = [&](viz::Layer & layer, NaturalIK & ik, Eigen::Vector3f target, Eigen::Vector3f & err)
652  {
653  ik.setScale(p.p_R.scale);
654 
655  Eigen::Vector3f vtgt = target;
656  NaturalIK::SoechtingAngles soechtingAngles = ik.CalculateSoechtingAngles(vtgt);
657  NaturalIK::SoechtingForwardPositions fwd = ik.forwardKinematics(soechtingAngles);
658  err = target - fwd.wrist;
659  vtgt = vtgt + 1.0 * err;
660 
661  layer.add(viz::Box("Wrist_orig").position(fwd.wrist)
662  .size(20).color(viz::Color::fromRGBA(0, 0, 120)));
663 
664  fwd = ik.forwardKinematics(ik.CalculateSoechtingAngles(vtgt));
665  err = target - fwd.wrist;
666 
667 
668  layer.clear();
669 
670  layer.add(viz::Box("Shoulder").position(ik.getShoulderPos())
671  .size(20).color(viz::Color::fromRGBA(255, 0, 0)));
672  layer.add(viz::Box("Elbow").position(fwd.elbow)
673  .size(20).color(viz::Color::fromRGBA(255, 0, 255)));
674  layer.add(viz::Box("Wrist").position(fwd.wrist)
675  .size(20).color(viz::Color::fromRGBA(0, 0, 255)));
676 
677  layer.add(viz::Box("Target").position(target)
678  .size(20).color(viz::Color::fromRGBA(255, 165, 0)));
679 
680  viz::Cylinder arrUpperArm = viz::Cylinder("UpperArm");
681  arrUpperArm.fromTo(ik.getShoulderPos(), fwd.elbow);
682  arrUpperArm.color(viz::Color::blue());
683 
684  viz::Arrow arrLowerArm = viz::Arrow("LowerArm");
685  arrLowerArm.fromTo(fwd.elbow, fwd.wrist);
686  arrLowerArm.color(viz::Color::red());
687 
688  layer.add(arrUpperArm);
689  layer.add(arrLowerArm);
690 
691 
692  };
693 
694  CycleUtil c(20);
695  while (!vizTask->isStopped())
696  {
697  if (!p.targetValid)
698  {
699  c.waitForCycleDuration();
700  continue;
701  }
702 
703  makeVizSide(layer_R, ik_R, p.p_R.target + offset, p.err_R);
704  makeVizSide(layer_L, ik_L, p.p_L.target + offset, p.err_L);
705 
706 
707  //Eigen::Quaternionf targetOri(0.70029222965240479,
708  // -0.6757887601852417,
709  // 0.036805182695388794,
710  // 0.22703713178634644);
711  //Eigen::Matrix4f target_R = math::Helpers::Pose(target + offset, targetOri.toRotationMatrix());
712 
713  //Eigen::Quaternionf referenceOri(0,
714  // 0.70710688,
715  // -0.70710682,
716  // 0);
717  Eigen::Quaternionf referenceOri_R(0.70029222965240479,
718  -0.6757887601852417,
719  0.036805182695388794,
720  0.22703713178634644);
721  Eigen::Quaternionf referenceOri_L = mirrorOri(referenceOri_R);
722 
723  Eigen::AngleAxisf aa_R(p.p_R.targetRotation.norm() / 180 * M_PI, p.p_R.targetRotation.normalized());
724  Eigen::Matrix3f targetOri_R = referenceOri_R.toRotationMatrix() * aa_R.toRotationMatrix();
725  Eigen::Matrix4f target_R = math::Helpers::Pose(p.p_R.target + offset, targetOri_R);
726 
727  Eigen::AngleAxisf aa_L(p.p_L.targetRotation.norm() / 180 * M_PI, p.p_L.targetRotation.normalized());
728  Eigen::Matrix3f targetOri_L = referenceOri_L.toRotationMatrix() * aa_L.toRotationMatrix();
729  Eigen::Matrix4f target_L = math::Helpers::Pose(p.p_L.target + offset, targetOri_L);
730 
731  // X_Platform
732  // Y_Platform
733  // Yaw_Platform
734  // ArmL1_Cla1
735  // ArmL2_Sho1
736  // ArmL3_Sho2
737  // ArmL4_Sho3
738  // ArmL5_Elb1
739  // ArmL6_Elb2
740  // ArmL7_Wri1
741  // ArmL8_Wri2
742  // ArmR1_Cla1
743  // ArmR2_Sho1
744  // ArmR3_Sho2
745  // ArmR4_Sho3
746  // ArmR5_Elb1
747  // ArmR6_Elb2
748  // ArmR7_Wri1
749  // ArmR8_Wri2
750 
751  auto calcShoulderAngles = [&](NaturalIK & natik, Eigen::Matrix4f target)
752  {
754  Eigen::Vector3f elb = fwd.elbow - fwd.shoulder;
755  float SE = fwd.soechtingAngles.SE;
756  elb = Eigen::AngleAxisf(-SE, Eigen::Vector3f::UnitX()) * elb;
757  float SR = std::atan2(elb(0), -elb(2));
758  SR = std::max(-0.1f, SR);
759  ShoulderAngles sa;
760  sa.SE = SE;
761  sa.SR = SR;
762  sa.fwd = fwd;
763  return sa;
764  };
765 
766  //VirtualRobot::RobotNodeSetPtr rnsP_R = robot->getRobotNodeSet("PlatformPlanningRightArm");
767  VirtualRobot::RobotNodeSetPtr rnsP = robot->getRobotNodeSet("PlatformPlanningBothArms");
768  if (p.useCompositeIK)
769  {
770  CompositeDiffIK cik(rnsP);
771 
772 
773  VirtualRobot::IKSolver::CartesianSelection mode_R = p.p_R.setOri ? VirtualRobot::IKSolver::CartesianSelection::All : VirtualRobot::IKSolver::CartesianSelection::Position;
774  VirtualRobot::IKSolver::CartesianSelection mode_L = p.p_L.setOri ? VirtualRobot::IKSolver::CartesianSelection::All : VirtualRobot::IKSolver::CartesianSelection::Position;
775  CompositeDiffIK::TargetPtr tgt_R = cik.addTarget(arm_R.tcp, target_R, mode_R);
776  CompositeDiffIK::TargetPtr tgt_L = cik.addTarget(arm_L.tcp, target_L, mode_L);
777 
779  //NaturalIK::SoechtingForwardPositions fwd_R = ik_R.solveSoechtingIK(math::Helpers::Position(target_R));
780  //Eigen::Vector3f elb_R = fwd_R.elbow - fwd_R.shoulder;
781  //float SE = fwd_R.soechtingAngles.SE;
782  //elb_R = Eigen::AngleAxisf(-SE, Eigen::Vector3f::UnitX()) * elb;
783  //float SR = std::atan2(elb_R(0), -elb_R(2));
784  //SR = std::max(-0.1f, SR);
785 
786  ShoulderAngles sa_R = calcShoulderAngles(ik_R, target_R);
787  ShoulderAngles sa_L = calcShoulderAngles(ik_L, target_L);
788 
789  nsjt->set("ArmR2_Sho1", +sa_R.SE, 1);
790  nsjt->set("ArmR3_Sho2", +sa_R.SR, 2);
791  nsjt->set("ArmR4_Sho3", -M_PI / 4, 0.5f);
792 
793  nsjt->set("ArmL2_Sho1", -sa_L.SE, 1);
794  nsjt->set("ArmL3_Sho2", +sa_L.SR, 2);
795  nsjt->set("ArmL4_Sho3", +M_PI / 4, 0.5f);
796 
797  nsjt->set("ArmR1_Cla1", 0, 0.5f);
798  nsjt->set("ArmL1_Cla1", 0, 0.5f);
799 
800 
801  //ARMARX_IMPORTANT << VAROUT(SE) << VAROUT(SR);
802  nsjt->kP = 1;
803  cik.addNullspaceGradient(nsjt);
804 
805  //CompositeDiffIK::NullspaceTargetPtr nst_R(new CompositeDiffIK::NullspaceTarget(rnsP, arm_R.elbow,
806  // math::Helpers::CreatePose(fwd_R.elbow, Eigen::Matrix3f::Identity()),
807  // VirtualRobot::IKSolver::CartesianSelection::Position));
808  //nst_R->kP = 0;
809  //cik.addNullspaceGradient(nst_R);
811  nst_R->kP = 0;
813  nst_L->kP = 0;
814 
816  nsjla->setWeight(0, 0);
817  nsjla->setWeight(1, 0);
818  nsjla->setWeight(2, 0);
819  nsjla->setWeight("ArmR4_Sho3", 0);
820  nsjla->setWeight("ArmL4_Sho3", 0);
821  nsjla->kP = 0.1;
822  cik.addNullspaceGradient(nsjla);
824  cp.resetRnsValues = true;
825  cp.returnIKSteps = true;
826  CompositeDiffIK::Result cikResult = cik.solve(cp);
827 
828  layer_iksteps.clear();
829  int i = 0;
830  for (const CompositeDiffIK::TargetStep& s : tgt_R->ikSteps)
831  {
832  layer_iksteps.add(viz::Box("tcp_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
833  i++;
834  }
835  for (const CompositeDiffIK::TargetStep& s : tgt_L->ikSteps)
836  {
837  layer_iksteps.add(viz::Box("tcp_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
838  i++;
839  }
840  for (const CompositeDiffIK::NullspaceTargetStep& s : nst_R->ikSteps)
841  {
842  layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
843  i++;
844  }
845  for (const CompositeDiffIK::NullspaceTargetStep& s : nst_L->ikSteps)
846  {
847  layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
848  i++;
849  }
850 
851  }
852  else
853  {
854  rnsP->getNode(0)->setJointValue(0);
855  rnsP->getNode(1)->setJointValue(0);
856  rnsP->getNode(2)->setJointValue(0);
857  NaturalDiffIK::Result ikResult;
858  if (p.p_R.setOri)
859  {
860  ikResult = ik_R.calculateIK(target_R, arm_R, p.p_R.ikparams);
861  }
862  else
863  {
864  ikResult = ik_R.calculateIKpos(math::Helpers::Position(target_R), arm_R, p.p_R.ikparams);
865  }
866 
867  layer_iksteps.clear();
868  std::stringstream ss;
869  int i = 0;
870  for (const NaturalDiffIK::IKStep& s : ikResult.ikSteps)
871  {
872  ss << s.pdTcp.norm() << "; " << s.odTcp.norm() << "; " << s.pdElb.norm() << "\n";
873  layer_iksteps.add(viz::Box("tcp_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
874  layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.elbPose).color(viz::Color::blue()));
875  i++;
876  }
877  //ARMARX_IMPORTANT << ss.str();
878  }
879 
880 
881 
882  vizrobot.joints(rnsP->getJointValueMap());
883 
884 
885  arviz.commit({layer_R, layer_L, layer_robot, layer_iksteps});
886  //ARMARX_IMPORTANT << "arviz.commit";
887 
888 
889  c.waitForCycleDuration();
890  }
891  }
892 
893 
895  {
896  Eigen::Quaternionf oriOld(oldPose.block<3, 3>(0, 0));
897  Eigen::Quaternionf ori;
898 
899  ori.w() = oriOld.z();
900  ori.x() = oriOld.y() * -1;
901  ori.y() = oriOld.x() * -1;
902  ori.z() = oriOld.w();
903 
905  pose.block<3, 3>(0, 0) = (ori * Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())).toRotationMatrix();
906  pose(0, 3) = -oldPose(0, 3);
907  pose(1, 3) = oldPose(1, 3);
908  pose(2, 3) = oldPose(2, 3);
909 
910  return pose;
911  }
913  {
914  Eigen::Quaternionf ori;
915 
916  ori.w() = oriOld.z();
917  ori.x() = oriOld.y() * -1;
918  ori.y() = oriOld.x() * -1;
919  ori.z() = oriOld.w();
920 
921  return (ori * Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()));
922  }
923 
924 
925 
927  {
928 
929  }
930 
931 
932 
933 
935  {
938  }
939 }
armarx::CompositeDiffIK::TargetStep
Definition: CompositeDiffIK.h:131
Client.h
armarx::RemoteGui::detail::GroupBoxBuilder
Definition: LayoutWidgets.h:166
armarx::NaturalDiffIK::Parameters::maxJointAngleStep
float maxJointAngleStep
Definition: NaturalDiffIK.h:49
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition: Client.cpp:80
armarx::RemoteGui::makeVBoxLayout
detail::VBoxLayoutBuilder makeVBoxLayout(std::string const &name="")
Definition: LayoutWidgets.h:245
armarx::NaturalIK::Parameters
Definition: NaturalIK.h:52
armarx::NaturalIK::SoechtingAngles
Definition: NaturalIK.h:68
armarx::control::njoint_mp_controller::task_space::ConstraintPtr
std::shared_ptr< Constraint > ConstraintPtr
Definition: KVILImpedanceController.h:60
armarx::natik
Brief description of class natik.
Definition: natik.h:39
armarx::SoftPositionConstraint::SoftPositionConstraint
SoftPositionConstraint(const VirtualRobot::RobotPtr &robot, const VirtualRobot::RobotNodeSetPtr &nodeSet, const VirtualRobot::SceneObjectPtr &node, const VirtualRobot::SceneObjectPtr &tcp, const Eigen::Vector3f &target, VirtualRobot::IKSolver::CartesianSelection cartesianSelection=VirtualRobot::IKSolver::All, float tolerance=3.0f)
Definition: NaturalIKTest.cpp:258
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::SoftPositionConstraint::tcp
VirtualRobot::SceneObjectPtr tcp
Definition: NaturalIKTest.cpp:266
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
armarx::viz::Cylinder::fromTo
Cylinder & fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
Definition: Elements.cpp:82
armarx::NaturalIK::calculateIK
NaturalDiffIK::Result calculateIK(const Eigen::Matrix4f &targetPose, ArmJoints arm, NaturalIK::Parameters params=NaturalIK::Parameters())
Definition: NaturalIK.cpp:84
armarx::RemoteGui::makeButton
detail::ButtonBuilder makeButton(std::string const &name)
Definition: IntegerWidgets.h:48
armarx::CompositeDiffIK::Parameters::resetRnsValues
bool resetRnsValues
Definition: CompositeDiffIK.h:52
armarx::viz::Layer::clear
void clear()
Definition: Layer.h:23
armarx::NaturalIK::ArmJoints::tcp
VirtualRobot::RobotNodePtr tcp
Definition: NaturalIK.h:64
armarx::CompositeDiffIK::Result::reached
bool reached
Definition: CompositeDiffIK.h:180
armarx::IKStats::solved
int solved
Definition: NaturalIKTest.cpp:241
armarx::NaturalIK::SoechtingForwardPositions::wrist
Eigen::Vector3f wrist
Definition: NaturalIK.h:93
armarx::NaturalIKTest::createPropertyDefinitions
virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: NaturalIKTest.cpp:934
armarx::RemoteGui::makeLabel
detail::LabelBuilder makeLabel(std::string const &name)
Definition: StaticWidgets.h:17
armarx::NaturalIKTestPropertyDefinitions
Definition: NaturalIKTest.h:45
armarx::NaturalIKTest::onExitComponent
virtual void onExitComponent() override
Definition: NaturalIKTest.cpp:926
WidgetBuilder.h
armarx::NaturalIK::ArmJoints::elbow
VirtualRobot::RobotNodePtr elbow
Definition: NaturalIK.h:62
armarx::viz::Color::fromRGBA
static Color fromRGBA(int r, int g, int b, int a=255)
Construct a byte color from R, G, B and optional alpha.
Definition: Color.h:42
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:688
armarx::viz::Color::blue
static Color blue(int b=255, int a=255)
Definition: Color.h:89
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition: CycleUtil.h:40
armarx::NaturalDiffIK::Result::reached
bool reached
Definition: NaturalDiffIK.h:78
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:267
armarx::CompositeDiffIK::NullspaceJointLimitAvoidancePtr
std::shared_ptr< class NullspaceJointLimitAvoidance > NullspaceJointLimitAvoidancePtr
Definition: CompositeDiffIK.h:129
armarx::Component::offeringTopicFromProperty
void offeringTopicFromProperty(const std::string &propertyName)
Offer a topic whose name is specified by the given property.
Definition: Component.cpp:154
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:53
armarx::IKStats::name
std::string name
Definition: NaturalIKTest.cpp:240
armarx::viz::Arrow
Definition: Elements.h:198
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::NaturalIKTest::mirrorPose
Eigen::Matrix4f mirrorPose(Eigen::Matrix4f oldPose)
Definition: NaturalIKTest.cpp:894
armarx::NaturalIKTest::GuiParams::targetValid
std::atomic_bool targetValid
Definition: NaturalIKTest.h:85
armarx::IKStats::duration
float duration
Definition: NaturalIKTest.cpp:242
lo
#define lo(x)
Definition: AbstractInterface.h:43
armarx::NaturalIKTest::onDisconnectComponent
virtual void onDisconnectComponent() override
Definition: NaturalIKTest.cpp:97
armarx::NaturalIKTest::GuiSideParams::ikparams
NaturalIK::Parameters ikparams
Definition: NaturalIKTest.h:78
armarx::viz::Layer::add
void add(ElementT const &element)
Definition: Layer.h:29
armarx::NaturalIK::ArmJoints
Definition: NaturalIK.h:59
armarx::RunningTask
Definition: ArmarXMultipleObjectsScheduler.h:35
armarx::RemoteGui::makeSimpleGridLayout
detail::SimpleGridLayoutBuilder makeSimpleGridLayout(std::string const &name="")
Definition: LayoutWidgets.h:235
armarx::viz::Robot::useFullModel
Robot & useFullModel()
Definition: Robot.h:29
armarx::CompositeDiffIK
Definition: CompositeDiffIK.h:41
armarx::viz::ElementOps::position
DerivedT & position(float x, float y, float z)
Definition: ElementOps.h:127
armarx::NaturalIKTest::mirrorOri
Eigen::Quaternionf mirrorOri(Eigen::Quaternionf oriOld)
Definition: NaturalIKTest.cpp:912
armarx::IKStats::averageElb
Eigen::Vector3f averageElb()
Definition: NaturalIKTest.cpp:244
armarx::CompositeDiffIK::solve
Result solve(Parameters params)
Definition: CompositeDiffIK.cpp:67
armarx::NaturalIK::setLowerArmLength
void setLowerArmLength(float value)
Definition: NaturalIK.cpp:214
armarx::CompositeDiffIK::NullspaceTargetPtr
std::shared_ptr< class NullspaceTarget > NullspaceTargetPtr
Definition: CompositeDiffIK.h:92
armarx::NaturalIKTest::testTaskRun
void testTaskRun()
Definition: NaturalIKTest.cpp:301
armarx::NaturalIKTest::GuiParams::err_L
Eigen::Vector3f err_L
Definition: NaturalIKTest.h:86
armarx::RemoteGui::TabProxy
Definition: WidgetProxy.h:17
armarx::CompositeDiffIK::NullspaceJointTargetPtr
std::shared_ptr< class NullspaceJointTarget > NullspaceJointTargetPtr
Definition: CompositeDiffIK.h:110
armarx::viz::Robot::joints
Robot & joints(std::map< std::string, float > const &values)
Definition: Robot.h:57
armarx::CompositeDiffIK::addTarget
void addTarget(const TargetPtr &target)
Definition: CompositeDiffIK.cpp:40
armarx::NaturalIK
The NaturalIK class.
Definition: NaturalIK.h:49
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::CompositeDiffIK::NullspaceJointTarget
Definition: CompositeDiffIK.h:94
armarx::CompositeDiffIK::Target
Definition: CompositeDiffIK.h:139
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
armarx::RemoteGui::TabProxy::receiveUpdates
void receiveUpdates()
Definition: WidgetProxy.h:131
armarx::NaturalIKTest::GuiSideParams::scale
float scale
Definition: NaturalIKTest.h:75
armarx::NaturalIK::setUpperArmLength
void setUpperArmLength(float value)
Definition: NaturalIK.cpp:204
armarx::RemoteGui::detail::ChildrenMixin::addChild
Derived & addChild(WidgetPtr const &child)
Definition: LayoutWidgets.h:22
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::RemoteGui::makeCheckBox
detail::CheckBoxBuilder makeCheckBox(std::string const &name)
Definition: BoolWidgets.h:26
armarx::NaturalIKTest::GuiSideParams::setOri
std::atomic_bool setOri
Definition: NaturalIKTest.h:77
armarx::NaturalIK::Parameters::diffIKparams
NaturalDiffIK::Parameters diffIKparams
Definition: NaturalIK.h:55
armarx::NaturalIKTest::GuiSideParams::targetRotation
Eigen::Vector3f targetRotation
Definition: NaturalIKTest.h:74
armarx::armem::human::Robot
@ Robot
Definition: util.h:14
armarx::viz::Cylinder
Definition: Elements.h:74
armarx::RemoteGui::makeFloatSlider
detail::FloatSliderBuilder makeFloatSlider(std::string const &name)
Definition: FloatWidgets.h:57
armarx::NaturalIK::calculateIKpos
NaturalDiffIK::Result calculateIKpos(const Eigen::Vector3f &targetPos, ArmJoints arm, NaturalIK::Parameters params=NaturalIK::Parameters())
Definition: NaturalIK.cpp:91
armarx::CMakePackageFinder::getDataDir
std::string getDataDir() const
Definition: CMakePackageFinder.h:176
armarx::NaturalIKTest::GuiParams::err_R
Eigen::Vector3f err_R
Definition: NaturalIKTest.h:86
armarx::NJointTaskSpaceDMPControllerMode::CartesianSelection
CartesianSelection
Definition: ControllerInterface.ice:34
armarx::CompositeDiffIK::TargetPtr
std::shared_ptr< Target > TargetPtr
Definition: CompositeDiffIK.h:166
armarx::NaturalIKTest::GuiParams::p_R
GuiSideParams p_R
Definition: NaturalIKTest.h:83
armarx::ShoulderAngles
Definition: NaturalIKTest.cpp:571
armarx::RemoteGui::detail::LabelMixin::label
Derived & label(std::string const &label)
Definition: Basic.h:191
armarx::RemoteGui::makeGroupBox
detail::GroupBoxBuilder makeGroupBox(std::string const &name="")
Definition: LayoutWidgets.h:250
armarx::viz::Box
Definition: Elements.h:51
armarx::SoftPositionConstraint::checkTolerances
bool checkTolerances() override
Definition: NaturalIKTest.cpp:295
armarx::NaturalDiffIK::Parameters::resetRnsValues
bool resetRnsValues
Definition: NaturalDiffIK.h:50
armarx::viz::Robot
Definition: Robot.h:9
armarx::NaturalDiffIK::Parameters::ikStepLengthFineTune
float ikStepLengthFineTune
Definition: NaturalDiffIK.h:42
max
T max(T t1, T t2)
Definition: gdiam.h:48
armarx::ShoulderAngles::SR
float SR
Definition: NaturalIKTest.cpp:573
armarx::SoftPositionConstraint
Definition: NaturalIKTest.cpp:255
armarx::viz::Color::red
static Color red(int r=255, int a=255)
Definition: Color.h:79
armarx::NaturalIK::ArmJoints::rns
VirtualRobot::RobotNodeSetPtr rns
Definition: NaturalIK.h:61
armarx::NaturalIKTest::vizTaskRun
void vizTaskRun()
Definition: NaturalIKTest.cpp:577
armarx::NaturalIKTest::onConnectComponent
virtual void onConnectComponent() override
Definition: NaturalIKTest.cpp:80
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
armarx::NaturalIKTest::GuiParams::p_L
GuiSideParams p_L
Definition: NaturalIKTest.h:84
armarx::NaturalIKTest::buildGui
RemoteGui::WidgetPtr buildGui()
Definition: NaturalIKTest.cpp:103
armarx::WidgetDescription::WidgetPtr
::IceInternal::Handle<::armarx::WidgetDescription::Widget > WidgetPtr
Definition: NJointControllerBase.h:66
CartesianPositionController.h
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
armarx::RemoteGui::TabProxy::getValue
ValueProxy< T > getValue(std::string const &name)
Definition: WidgetProxy.h:161
armarx::NaturalIK::SoechtingForwardPositions::elbow
Eigen::Vector3f elbow
Definition: NaturalIK.h:92
CycleUtil.h
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
armarx::viz::Arrow::fromTo
Arrow & fromTo(const Eigen::Vector3f &from, const Eigen::Vector3f &to)
Definition: Elements.h:218
CMakePackageFinder.h
armarx::IKStats::elbow
std::vector< Eigen::Vector3f > elbow
Definition: NaturalIKTest.cpp:243
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::RemoteGui::TabProxy::getButtonClicked
bool getButtonClicked(std::string const &name)
Definition: WidgetProxy.cpp:11
armarx::NaturalIKTest::getDefaultName
virtual std::string getDefaultName() const override
Definition: NaturalIKTest.cpp:61
armarx::NaturalIKTest::GuiSideParams::target
Eigen::Vector3f target
Definition: NaturalIKTest.h:73
armarx::Side
Definition: NaturalIKTest.cpp:564
armarx::Side::layer
viz::Layer layer
Definition: NaturalIKTest.cpp:566
armarx::NaturalIKTest::onInitComponent
virtual void onInitComponent() override
Definition: NaturalIKTest.cpp:67
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
TimeUtil.h
armarx::NaturalIK::SoechtingAngles::SE
float SE
Definition: NaturalIK.h:70
armarx::NaturalIK::Parameters::minimumElbowHeight
std::optional< float > minimumElbowHeight
Definition: NaturalIK.h:56
armarx::CompositeDiffIK::Result
Definition: CompositeDiffIK.h:171
armarx::Quaternion< float, 0 >
set
set(LIBS ArmarXCoreInterfaces ${CMAKE_THREAD_LIBS_INIT} ${dl_LIBRARIES} ${rt_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} BoostAssertionHandler ArmarXCPPUtility SimoxUtility) set(LIB_FILES ArmarXManager.cpp ArmarXMultipleObjectsScheduler.cpp ArmarXObjectScheduler.cpp ManagedIceObject.cpp ManagedIceObjectPlugin.cpp Component.cpp ComponentPlugin.cpp IceGridAdmin.cpp ArmarXObjectObserver.cpp IceManager.cpp PackagePath.cpp RemoteReferenceCount.cpp logging/LoggingUtil.cpp logging/Logging.cpp logging/LogSender.cpp logging/ArmarXLogBuf.cpp system/ArmarXDataPath.cpp system/DynamicLibrary.cpp system/ProcessWatcher.cpp system/FactoryCollectionBase.cpp system/cmake/CMakePackageFinder.cpp system/cmake/CMakePackageFinderCache.cpp system/cmake/ArmarXPackageToolInterface.cpp system/RemoteObjectNode.cpp services/sharedmemory/HardwareId.cpp services/tasks/RunningTask.cpp services/tasks/ThreadList.cpp services/tasks/ThreadPool.cpp services/profiler/Profiler.cpp services/profiler/FileLoggingStrategy.cpp services/profiler/IceLoggingStrategy.cpp application/Application.cpp application/ApplicationOptions.cpp application/ApplicationProcessFacet.cpp application/ApplicationNetworkStats.cpp application/properties/PropertyUser.cpp application/properties/Property.cpp application/properties/PropertyDefinition.cpp application/properties/PropertyDefinitionContainer.cpp application/properties/PropertyDefinitionHelpFormatter.cpp application/properties/PropertyDefinitionConfigFormatter.cpp application/properties/PropertyDefinitionBriefHelpFormatter.cpp application/properties/PropertyDefinitionXmlFormatter.cpp application/properties/PropertyDefinitionDoxygenFormatter.cpp application/properties/PropertyDefinitionDoxygenComponentPagesFormatter.cpp application/properties/PropertyDefinitionContainerBriefHelpFormatter.cpp application/properties/IceProperties.cpp exceptions/Exception.cpp exceptions/local/UnexpectedEnumValueException.cpp util/FileSystemPathBuilder.cpp util/StringHelpers.cpp util/IceReportSkipper.cpp util/Throttler.cpp util/distributed/AMDCallbackCollection.cpp util/distributed/RemoteHandle/ClientSideRemoteHandleControlBlock.cpp util/distributed/RemoteHandle/RemoteHandle.cpp util/distributed/RemoteHandle/RemoteHandleControlBlock.cpp time/ice_conversions.cpp time/json_conversions.cpp time/CallbackWaitLock.cpp time/Clock.cpp time/ClockType.cpp time/ClockTypeNames.cpp time/CycleUtil.cpp time/DateTime.cpp time/Duration.cpp time/Frequency.cpp time/LocalTimeServer.cpp time/Metronome.cpp time/ScopedStopWatch.cpp time/StopWatch.cpp time/Timer.cpp time/TimeKeeper.cpp time/TimeUtil.cpp csv/CsvWriter.cpp csv/CsvReader.cpp eigen/conversions.cpp eigen/ice_conversions.cpp) set(LIB_HEADERS ArmarXManager.h ArmarXDummyManager.h ArmarXMultipleObjectsScheduler.h ArmarXObjectObserver.h ArmarXObjectScheduler.h ArmarXFwd.h Component.h ComponentPlugin.h ComponentFactories.h CoreObjectFactories.h IceGridAdmin.h IceManager.h IceManagerImpl.h json_conversions.h ManagedIceObject.h ManagedIceObjectPlugin.h ManagedIceObjectImpl.h ManagedIceObjectDependency.h ManagedIceObjectRegistryInterface.h PackagePath.h RemoteReferenceCount.h system/ImportExport.h system/ImportExportComponent.h system/AbstractFactoryMethod.h system/FactoryCollectionBase.h system/Synchronization.h system/ArmarXDataPath.h system/DynamicLibrary.h system/ProcessWatcher.h system/ConditionSynchronization.h system/cmake/CMakePackageFinder.h system/cmake/CMakePackageFinderCache.h system/cmake/FindPackageX.cmake system/cmake/ArmarXPackageToolInterface.h system/RemoteObjectNode.h logging/LoggingUtil.h logging/LogSender.h logging/Logging.h logging/ArmarXLogBuf.h logging/SpamFilterData.h services/tasks/RunningTask.h services/tasks/PeriodicTask.h services/tasks/ThreadList.h services/tasks/TaskUtil.h services/tasks/ThreadPool.h services/sharedmemory/SharedMemoryProvider.h services/sharedmemory/SharedMemoryConsumer.h services/sharedmemory/IceSharedMemoryProvider.h services/sharedmemory/IceSharedMemoryConsumer.h services/sharedmemory/HardwareIdentifierProvider.h services/sharedmemory/HardwareId.h services/sharedmemory/exceptions/SharedMemoryExceptions.h services/profiler/Profiler.h services/profiler/LoggingStrategy.h services/profiler/FileLoggingStrategy.h services/profiler/IceLoggingStrategy.h application/Application.h application/ApplicationOptions.h application/ApplicationProcessFacet.h application/ApplicationNetworkStats.h application/properties/forward_declarations.h application/properties/Properties.h application/properties/Property.h application/properties/PluginEigen.h application/properties/PluginEnumNames.h application/properties/PluginCfgStruct.h application/properties/PluginAll.h application/properties/PropertyUser.h application/properties/PropertyDefinition.h application/properties/PropertyDefinition.hpp application/properties/PropertyDefinitionInterface.h application/properties/PropertyDefinitionContainer.h application/properties/PropertyDefinitionFormatter.h application/properties/PropertyDefinitionContainerFormatter.h application/properties/PropertyDefinitionConfigFormatter.h application/properties/PropertyDefinitionHelpFormatter.h application/properties/PropertyDefinitionBriefHelpFormatter.h application/properties/PropertyDefinitionXmlFormatter.h application/properties/PropertyDefinitionDoxygenFormatter.h application/properties/PropertyDefinitionDoxygenComponentPagesFormatter.h application/properties/PropertyDefinitionContainerBriefHelpFormatter.h application/properties/ProxyPropertyDefinition.h application/properties/IceProperties.h exceptions/Exception.h exceptions/LocalException.h exceptions/local/DynamicLibraryException.h exceptions/local/ExpressionException.h exceptions/local/FileIOException.h exceptions/local/InvalidPropertyValueException.h exceptions/local/MissingRequiredPropertyException.h exceptions/local/PropertyInheritanceCycleException.h exceptions/local/ProxyNotInitializedException.h exceptions/local/UnexpectedEnumValueException.h exceptions/local/UnmappedValueException.h exceptions/local/ValueRangeExceededException.h exceptions/user/NotImplementedYetException.h rapidxml/rapidxml.hpp rapidxml/rapidxml_print.hpp rapidxml/rapidxml_iterators.hpp rapidxml/rapidxml_utils.hpp rapidxml/wrapper/RapidXmlReader.h rapidxml/wrapper/RapidXmlWriter.h rapidxml/wrapper/DefaultRapidXmlReader.h rapidxml/wrapper/MultiNodeRapidXMLReader.h util/IceBlobToObject.h util/ObjectToIceBlob.h util/FileSystemPathBuilder.h util/FiniteStateMachine.h util/StringHelpers.h util/StringHelperTemplates.h util/algorithm.h util/OnScopeExit.h util/Predicates.h util/Preprocessor.h util/PropagateConst.h util/Registrar.h util/TemplateMetaProgramming.h util/TripleBuffer.h util/IceReportSkipper.h util/Throttler.h util/distributed/AMDCallbackCollection.h util/distributed/RemoteHandle/ClientSideRemoteHandleControlBlock.h util/distributed/RemoteHandle/RemoteHandle.h util/distributed/RemoteHandle/RemoteHandleControlBlock.h util/SimpleStatemachine.h time.h time_minimal.h time/forward_declarations.h time/ice_conversions.h time/json_conversions.h time/CallbackWaitLock.h time/Clock.h time/ClockType.h time/ClockTypeNames.h time/CycleUtil.h time/DateTime.h time/Duration.h time/Frequency.h time/LocalTimeServer.h time/Metronome.h time/ScopedStopWatch.h time/StopWatch.h time/Timer.h time/TimeUtil.h time/TimeKeeper.h csv/CsvWriter.h csv/CsvReader.h eigen/conversions.h eigen/ice_conversions.h ice_conversions.h ice_conversions/ice_conversions_boost_templates.h ice_conversions/ice_conversions_templates.h ice_conversions/ice_conversions_templates.tpp $
Definition: CMakeLists.txt:12
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::min
std::vector< T > min(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:294
armarx::viz::Box::size
Box & size(Eigen::Vector3f const &s)
Definition: Elements.h:55
armarx::viz::ElementOps::color
DerivedT & color(Color color)
Definition: ElementOps.h:195
armarx::CompositeDiffIK::NullspaceJointLimitAvoidance
Definition: CompositeDiffIK.h:112
hi
#define hi(x)
Definition: AbstractInterface.h:42
armarx::NaturalDiffIK::Parameters::jointLimitAvoidanceKp
float jointLimitAvoidanceKp
Definition: NaturalDiffIK.h:47
scene3D::SceneObjectPtr
boost::intrusive_ptr< SceneObject > SceneObjectPtr
Definition: PointerDefinitions.h:40
armarx::NaturalIKTest::processGui
void processGui(RemoteGui::TabProxy &prx)
Definition: NaturalIKTest.cpp:178
armarx::RemoteGui::TabProxy::sendUpdates
void sendUpdates()
Definition: WidgetProxy.h:146
armarx::NaturalIK::SoechtingForwardPositions
Definition: NaturalIK.h:89
armarx::ShoulderAngles::SE
float SE
Definition: NaturalIKTest.cpp:573
armarx::IKStats::IKStats
IKStats(const std::string &name)
Definition: NaturalIKTest.cpp:239
armarx::NaturalIKTestPropertyDefinitions::NaturalIKTestPropertyDefinitions
NaturalIKTestPropertyDefinitions(std::string prefix)
Definition: NaturalIKTest.cpp:48
armarx::Side::shoulder
Eigen::Vector3f shoulder
Definition: NaturalIKTest.cpp:567
armarx::NaturalDiffIK::Result
Definition: NaturalDiffIK.h:69
armarx::RemoteGuiComponentPluginUser::createOrUpdateRemoteGuiTab
void createOrUpdateRemoteGuiTab(Ts &&...ts)
Definition: RemoteGuiComponentPlugin.h:255
armarx::CompositeDiffIK::Parameters
Definition: CompositeDiffIK.h:44
armarx::CompositeDiffIK::Parameters::returnIKSteps
bool returnIKSteps
Definition: CompositeDiffIK.h:53
pc
Introduction Thank you for taking interest in our work and downloading this software This library implements the algorithm described in the paper R R R Klein Efficient RANSAC for Point Cloud Shape in Computer Graphics Blackwell June If you use this software you should cite the aforementioned paper in any resulting publication Please send comments or bug reports to Ruwen Roland BUT NOT LIMITED THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY OR CONSEQUENTIAL WHETHER IN STRICT OR EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE Example usage This section shows how to use the library to detect the shapes in a point cloud PointCloud pc
Definition: ReadMe.txt:68
armarx::navigation::core::Position
Eigen::Vector3f Position
Definition: basic_types.h:36
armarx::NaturalIKTest::GuiParams::useCompositeIK
std::atomic_bool useCompositeIK
Definition: NaturalIKTest.h:88
armarx::viz::Client::layer
Layer layer(std::string const &name) const
Definition: Client.cpp:73
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
armarx::viz::Client
Definition: Client.h:109
armarx::NaturalDiffIK::Parameters::returnIKSteps
bool returnIKSteps
Definition: NaturalDiffIK.h:51
armarx::NaturalDiffIK::IKStep
Definition: NaturalDiffIK.h:53
armarx::NaturalDiffIK::Parameters::ikStepLengthInitial
float ikStepLengthInitial
Definition: NaturalDiffIK.h:41
armarx::SoftPositionConstraint::optimizationGradient
Eigen::VectorXf optimizationGradient(unsigned int id) override
Definition: NaturalIKTest.cpp:269
armarx::viz::Layer
Definition: Layer.h:12
armarx::NaturalDiffIK::Result::ikSteps
std::vector< IKStep > ikSteps
Definition: NaturalDiffIK.h:82
armarx::CompositeDiffIK::addNullspacePositionTarget
NullspaceTargetPtr addNullspacePositionTarget(const VirtualRobot::RobotNodePtr &tcp, const Eigen::Vector3f &target)
Definition: CompositeDiffIK.cpp:57
armarx::NaturalDiffIK::Parameters::elbowKp
float elbowKp
Definition: NaturalDiffIK.h:48
armarx::CompositeDiffIK::NullspaceTargetStep
Definition: CompositeDiffIK.h:68
armarx::NaturalIK::solveSoechtingIK
NaturalIK::SoechtingForwardPositions solveSoechtingIK(const Eigen::Vector3f &targetPos, std::optional< float > minElbowHeight=std::nullopt)
Definition: NaturalIK.cpp:39
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx::IKStats
Definition: NaturalIKTest.cpp:237
armarx::ShoulderAngles::fwd
NaturalIK::SoechtingForwardPositions fwd
Definition: NaturalIKTest.cpp:574
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::NaturalIK::SoechtingForwardPositions::soechtingAngles
SoechtingAngles soechtingAngles
Definition: NaturalIK.h:94
CompositeDiffIK.h
armarx::CompositeDiffIK::addNullspaceGradient
void addNullspaceGradient(const NullspaceGradientPtr &gradient)
Definition: CompositeDiffIK.cpp:52
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
NaturalIK.h
armarx::NaturalIK::SoechtingForwardPositions::shoulder
Eigen::Vector3f shoulder
Definition: NaturalIK.h:91
norm
double norm(const Point &a)
Definition: point.hpp:94
NaturalIKTest.h