KinematicSolver.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 ArmarXGuiPlugins::RobotTrajectoryDesigner
17  * @author Timo Birr
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 #include "KinematicSolver.h"
23 
24 #include "RobotAPI/libraries/core/math/MathUtils.h"
25 
26 #include "RobotComponents/components/RobotIK/RobotIK.h"
27 
28 #include "Interpolation/LinearInterpolation.h"
29 #include "MotionPlanning/CSpace/CSpaceSampled.h"
30 #include "MotionPlanning/Planner/BiRrt.h"
31 #include "MotionPlanning/Planner/Rrt.h"
32 #include "MotionPlanning/PostProcessing/ShortcutProcessor.h"
33 #include "VirtualRobot/CollisionDetection/CDManager.h"
34 #include "VirtualRobot/Workspace/Reachability.h"
35 
36 
37 using namespace armarx;
38 using namespace VirtualRobot;
39 
40 KinematicSolverPtr KinematicSolver::singleton = 0;
41 
42 KinematicSolver::KinematicSolver(ScenePtr scenario, RobotPtr robot)
43 {
44  //bla
45  //initialize attributes
46  this->robot = robot;
47  //this->scenario = scenario;
48  this->robotProxy = robot->clone();
49  this->currentRns = "";
50 
51  //initialize IKCalibrations
52  this->IKCalibrationMap = std::map<std::string, IKCalibration>();
53  this->IKCalibrationMap.insert(std::pair<std::string, IKCalibration>(
54  "Armar3", IKCalibration::createIKCalibration(1000, 0.005, 500, 0.3, 100, 1.0)));
55  this->IKCalibrationMap.insert(std::pair<std::string, IKCalibration>(
56  "Armar4", IKCalibration::createIKCalibration(10000, 0.005, 100, 0.6, 100, 1.0)));
57  this->IKCalibrationMap.insert(std::pair<std::string, IKCalibration>(
58  "Armar6", IKCalibration::createIKCalibration(100, 0.5, 500, 1.1, 500, 1.1)));
59  this->IKCalibrationMap.insert(std::pair<std::string, IKCalibration>(
60  "Custom", IKCalibration::createIKCalibration(100, 100, 100, 100, 100, 100)));
61 
62  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
63  ///ALTERNATIVE WITH REACHABILITY MAPS
64  //this->scenario = scenario;
65  /*this->reachabilityMap = std::map<std::string, ReachabilityPtr>();
66  for (RobotNodeSetPtr rns : robotProxy->getRobotNodeSets())
67  {
68  if (!rns->isKinematicChain())
69  {
70  continue;
71  }
72  ReachabilityPtr currentReach = ReachabilityPtr(new Reachability(robotProxy));
73  float minB[6] = { -1000.0f, -1000.0f, -1000.0f, (float) - M_PI, (float) - M_PI, (float) - M_PI};
74  float maxB[6] = {1000.0f, 1000.0f, 1000.0f, (float)M_PI, (float)M_PI, (float)M_PI};
75  ARMARX_INFO << robotProxy->getRootNode()->getName();
76  currentReach->initialize(rns, 200.0, 0.4, minB, maxB, rns, rns, robotProxy->getRootNode(), rns->getTCP(), true);
77  currentReach->addRandomTCPPoses(100, 2, false);
78  reachabilityMap[rns->getName()] = currentReach;
79  }*/
80  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
81 }
82 
83 KinematicSolver::IKCalibration
84 KinematicSolver::IKCalibration::createIKCalibration(int smallIterations,
85  double smallStepSize,
86  int mediumIterations,
87  double mediumStepSize,
88  int largeIterations,
89  double largeStepSize)
90 {
91  IKCalibration calibration;
92  calibration.smallIterations = smallIterations;
93  calibration.smallStepSize = smallStepSize;
94  calibration.mediumIterations = mediumIterations;
95  calibration.mediumStepSize = mediumStepSize;
96  calibration.largeIterations = largeIterations;
97  calibration.largeStepSize = largeStepSize;
98  return calibration;
99 }
100 
101 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
102 ///SINGLETON-FEATURES////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
103 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
104 KinematicSolverPtr
105 KinematicSolver::getInstance(ScenePtr scenario = NULL, RobotPtr robot = NULL)
106 {
107  if (singleton == 0)
108  {
109  ARMARX_INFO << "KinematicSolver initialized";
110  singleton = KinematicSolverPtr(new KinematicSolver(scenario, robot));
111  return singleton;
112  }
113  return KinematicSolver::singleton;
114 }
115 
116 void
117 KinematicSolver::reset()
118 {
119  ARMARX_INFO << "KinematicSolver reset";
120  singleton = 0;
121 }
122 
123 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
124 ///IK////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
125 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
126 
127 
128 std::vector<double>
129 KinematicSolver::solveIK(RobotNodeSetPtr kc,
130  PoseBasePtr globalPose,
131  IKSolver::CartesianSelection selection,
132  int iterations)
133 {
134  IKCalibration calibration = calibrate();
135  syncRobotPrx(kc);
136  //get Proxy for Rns
137  RobotNodeSetPtr kcPrx = robotProxy->getRobotNodeSet(kc->getName());
138  std::vector<double> jointValues;
139  int failureCounter = 0;
140 
141  do
142  {
143  //set all joints randomly so a random solution for IK Problem is calculated
144  std::vector<float> jv;
145  float rn = 1.0f / (float)RAND_MAX;
146  for (unsigned int i = 0; i < kcPrx->getSize(); i++)
147  {
148  RobotNodePtr ro = kcPrx->getNode(i);
149  float r = (float)rand() * rn;
150  float v = ro->getJointLimitLo() + (ro->getJointLimitHi() - ro->getJointLimitLo()) * r;
151  jv.push_back(v);
152  }
153  robotProxy->setJointValues(kcPrx, jv);
154  //finished randomly setting starting Pose
155 
156  //solve actual IK
157  GenericIKSolverPtr ikSolver = this->IKSetup(
158  calibration.largeStepSize, calibration.largeIterations, 10.0, 0.50, kcPrx);
159  Pose targetPose = Pose(globalPose->position, globalPose->orientation);
160  std::vector<float> output = ikSolver->solveNoRNSUpdate(targetPose.toEigen(), selection);
161  jointValues = std::vector<double>(output.begin(), output.end());
162  failureCounter++;
163  } while (jointValues.size() == 0 &&
164  failureCounter <= iterations); //randomly resets the robot Pose
165  return jointValues;
166 }
167 
168 std::vector<double>
169 KinematicSolver::solveIKRelative(RobotNodeSetPtr kc,
170  PoseBasePtr framedPose,
171  IKSolver::CartesianSelection selection)
172 {
173  PosePtr localPose = PosePtr(new Pose(framedPose->position, framedPose->orientation));
174  PoseBasePtr globalPose = PoseBasePtr(
175  new Pose(kc->getKinematicRoot()->toGlobalCoordinateSystem(localPose->toEigen())));
176  return this->solveIK(kc, globalPose, selection, 50);
177 }
178 
179 std::vector<float>
180 KinematicSolver::solveRecursiveIK(RobotNodeSetPtr kc,
181  std::vector<float> startingPoint,
182  PoseBasePtr globalDestination,
183  VirtualRobot::IKSolver::CartesianSelection selection)
184 {
185  IKCalibration calibration = calibrate();
186  syncRobotPrx(kc);
187  RobotNodeSetPtr kcPrx = robotProxy->getRobotNodeSet(kc->getName());
188  kcPrx->setJointValues(startingPoint);
189  GenericIKSolverPtr ikSolver =
190  this->IKSetup(calibration.mediumStepSize, calibration.mediumIterations, 10.0, 1.0, kcPrx);
191  Pose targetPose = Pose(globalDestination->position, globalDestination->orientation);
192  std::vector<float> output = ikSolver->solveNoRNSUpdate(targetPose.toEigen(), selection);
193  if (output.size() == 0)
194  {
195  std::vector<double> random =
196  this->solveIK(kc, PoseBasePtr(new Pose(targetPose.toEigen())), selection, 5);
197  output = std::vector<float>(random.begin(), random.end());
198  }
199  return output;
200 }
201 
202 std::vector<float>
203 KinematicSolver::solveRecursiveIKRelative(RobotNodeSetPtr kc,
204  std::vector<float> startingPoint,
205  PoseBasePtr framedDestination,
206  IKSolver::CartesianSelection selection)
207 {
208  syncRobotPrx(kc);
209  PosePtr localPose =
210  PosePtr(new Pose(framedDestination->position, framedDestination->orientation));
211  //std::cout << localPose->toEigen();
212  PoseBasePtr globalPose = PoseBasePtr(
213  new Pose(kc->getKinematicRoot()->toGlobalCoordinateSystem(localPose->toEigen())));
214  return this->solveRecursiveIK(kc, startingPoint, globalPose, selection);
215 }
216 
217 std::vector<double>
218 KinematicSolver::solveRecursiveIK(RobotNodeSetPtr kc,
219  std::vector<double> startingPoint,
220  PoseBasePtr globalDestination,
221  IKSolver::CartesianSelection selection)
222 {
223  IKCalibration calibration = calibrate();
224  syncRobotPrx(kc);
225  RobotNodeSetPtr kcPrx = robotProxy->getRobotNodeSet(kc->getName());
226  kcPrx->setJointValues(std::vector<float>(startingPoint.begin(), startingPoint.end()));
227  GenericIKSolverPtr ikSolver =
228  this->IKSetup(calibration.smallStepSize, calibration.smallIterations, 10, 0.5, kcPrx);
229  Pose targetPose = Pose(globalDestination->position, globalDestination->orientation);
230  //std::cout << targetPose.toEigen();
231  std::vector<float> output = ikSolver->solveNoRNSUpdate(targetPose.toEigen(), selection);
232  std::vector<double> outputd = std::vector<double>(output.begin(), output.end());
233  return outputd;
234 }
235 
236 std::vector<double>
237 KinematicSolver::solveRecursiveIKRelative(RobotNodeSetPtr kc,
238  std::vector<double> startingPoint,
239  PoseBasePtr framedDestination,
240  IKSolver::CartesianSelection selection)
241 {
242  syncRobotPrx(kc);
243  PosePtr localPose =
244  PosePtr(new Pose(framedDestination->position, framedDestination->orientation));
245  PoseBasePtr globalPose = PoseBasePtr(
246  new Pose(kc->getKinematicRoot()->toGlobalCoordinateSystem(localPose->toEigen())));
247  return this->solveRecursiveIK(kc, startingPoint, globalPose, selection);
248 }
249 
250 std::vector<std::vector<double>>
251 KinematicSolver::solveRecursiveIK(RobotNodeSetPtr kc,
252  std::vector<double> startingPoint,
253  std::vector<PoseBasePtr> globalDestinations,
254  std::vector<IKSolver::CartesianSelection> selections)
255 {
256  if (!isReachable(kc,
257  globalDestinations[globalDestinations.size() - 1],
258  selections[selections.size() - 1]))
259  {
260  throw LocalException("Desired Pose is not Reachable");
261  }
262  syncRobotPrx(kc);
263  RobotNodeSetPtr kcPrx = robotProxy->getRobotNodeSet(kc->getName());
264 
265  std::vector<std::vector<double>> bestResult;
266  int minSingularities = -1;
267  int tries = 0;
268  while ((minSingularities == -1 || minSingularities != 0) && tries < 2)
269  {
270  int i = 0;
271  //setup data
272  int failureCounter = 0;
273  int singularities = 0;
274  std::vector<std::vector<double>> result = std::vector<std::vector<double>>();
275  std::vector<float> currentStart =
276  std::vector<float>(startingPoint.begin(), startingPoint.end());
277  //solve ik step-by-step
278  for (PoseBasePtr currentDest : globalDestinations)
279  {
280  std::vector<double> temp =
281  std::vector<double>(currentStart.begin(), currentStart.end());
282  if (i != 0)
283  {
284  PosePtr priorPose = PosePtr(new Pose(globalDestinations.at(i - 1)->position,
285  globalDestinations.at(i - 1)->orientation));
286  PosePtr currentPose =
287  PosePtr(new Pose(currentDest->position, currentDest->orientation));
288  if (priorPose->toEigen() == currentPose->toEigen() && temp.size() != 0)
289  {
290 
291  i++;
292  continue;
293  }
294  }
295  std::vector<double> currentResult = KinematicSolver::getInstance()->solveRecursiveIK(
296  kc, temp, currentDest, selections.at(i));
297  if (currentResult.size() == 0)
298  {
299  failureCounter++;
300  if (failureCounter >= 1)
301  {
302  failureCounter = 0;
303  singularities++;
304  Eigen::VectorXf start = Eigen::VectorXf(temp.size());
305  Eigen::VectorXf goal = Eigen::VectorXf(temp.size());
306  std::vector<double> end = this->solveIK(kc, currentDest, selections[i], 100);
307  if (end.size() == 0)
308  {
309  ARMARX_INFO << "No local result found";
310  i++;
311  failureCounter++;
312  continue;
313  }
314  int k = 0;
315  for (double d : end)
316  {
317  goal[k] = d;
318  k++;
319  }
320  k = 0;
321  for (double d : temp)
322  {
323  start[k] = d;
324  k++;
325  }
326  std::vector<double> nowStart = std::vector<double>();
327  IKSolver::CartesianSelection sel = selections[i];
328  nowStart = std::vector<double>(currentStart.begin(), currentStart.end());
329  int bestNode = getFurthestNode(kcPrx, start, goal);
330  RobotNodePtr node = kcPrx->getAllRobotNodes()[bestNode];
331  //ARMARX_WARNING << node->getName();
332  kcPrx->setJointValues(currentStart);
333 
334  PoseBasePtr startPose = PoseBasePtr(new Pose(node->getGlobalPose()));
335  kcPrx->setJointValues(goal);
336  PoseBasePtr endPose = PoseBasePtr(new Pose(node->getGlobalPose()));
337  std::vector<PoseBasePtr> cp = {startPose, endPose};
338  IKCalibration calibration = calibrate();
339  AbstractInterpolationPtr ip =
340  AbstractInterpolationPtr(new LinearInterpolation(cp));
341  for (int i = 0; i < 25; i++)
342  {
343  PoseBasePtr poseBase = ip->getPoseAt(i * 0.04);
344  PosePtr posePose =
345  PosePtr(new Pose(poseBase->position, poseBase->orientation));
346  GenericIKSolverPtr ikSolver = this->IKSetup(
347  calibration.smallStepSize, calibration.smallIterations, 5, 0.5, kcPrx);
348  DifferentialIKPtr dIK = ikSolver->getDifferentialIK();
349  dIK->initialize();
350  PosePtr pose =
351  PosePtr(new Pose(currentDest->position, currentDest->orientation));
352 
353  dIK->setGoal(posePose->toEigen(), node, IKSolver::CartesianSelection::X);
354  dIK->setGoal(pose->toEigen(), kcPrx->getTCP(), sel);
355  kcPrx->setJointValues(std::vector<float>(nowStart.begin(), nowStart.end()));
356 
357  if (dIK->solveIK(0.4))
358  {
359  //ARMARX_INFO << "IK SOLUTION FOUND " + std::to_string(i);
360  std::vector<float> ca = kcPrx->getJointValues();
361  nowStart = std::vector<double>(ca.begin(), ca.end());
362  result.push_back(std::vector<double>(ca.begin(), ca.end()));
363  }
364  else
365  {
366  dIK->setGoal(
367  posePose->toEigen(), node, IKSolver::CartesianSelection::Y);
368  kcPrx->setJointValues(
369  std::vector<float>(nowStart.begin(), nowStart.end()));
370  if (dIK->solveIK(0.4))
371  {
372  //ARMARX_INFO << "IK SOLUTION FOUND " + std::to_string(i);
373  std::vector<float> ca = kcPrx->getJointValues();
374  nowStart = std::vector<double>(ca.begin(), ca.end());
375  result.push_back(std::vector<double>(ca.begin(), ca.end()));
376  }
377  else
378  {
379  if (dIK->solveIK(0.4))
380  {
381  dIK->setGoal(
382  posePose->toEigen(), node, IKSolver::CartesianSelection::Z);
383  kcPrx->setJointValues(
384  std::vector<float>(nowStart.begin(), nowStart.end()));
385  //ARMARX_INFO << "IK SOLUTION FOUND " + std::to_string(i);
386  std::vector<float> ca = kcPrx->getJointValues();
387  nowStart = std::vector<double>(ca.begin(), ca.end());
388  result.push_back(std::vector<double>(ca.begin(), ca.end()));
389  }
390  else
391  {
392  // ARMARX_INFO << "NO IK SOLUTION FOUND " + std::to_string(i);
393  }
394  }
395  if (singularities == 1)
396  {
397  //ARMARX_INFO << "NO IK SOLUTION FOUND " + std::to_string(i);
398  }
399  }
400 
401  /*for (unsigned int j = 0; j < result[0].size(); j++)
402  {
403  if (!kcPrx->getAllRobotNodes()[j]->isLimitless() && abs(result[result.size() - 2][j] - result[result.size() - 1][j]) > 0.1 && i > 0)
404  {
405  ARMARX_INFO << "WRONG!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
406  ARMARX_INFO << result[result.size() - 2];
407  ARMARX_INFO << result[result.size() - 1];
408  }
409  else if (kcPrx->getAllRobotNodes()[j]->isLimitless() && abs(math::MathUtils::AngleDelta(result[result.size() - 2][j], result[result.size() - 1][j])) > 0.1 && i > 0)
410  {
411  ARMARX_INFO << "WRONG!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
412  ARMARX_INFO << result[result.size() - 2];
413  ARMARX_INFO << result[result.size() - 1];
414  }
415  }*/
416  }
417  currentStart = std::vector<float>(nowStart.begin(), nowStart.end());
418  }
419  }
420 
421  else
422  {
423  result.push_back(currentResult);
424  currentStart = std::vector<float>(currentResult.begin(), currentResult.end());
425  }
426  }
427  tries++;
428  if (minSingularities <= 0 || singularities <= minSingularities)
429  {
430  minSingularities = singularities;
431  bestResult = result;
432  }
433  }
434  return bestResult;
435 }
436 
437 std::vector<std::vector<double>>
438 KinematicSolver::solveRecursiveIKNoMotionPlanning(
439  RobotNodeSetPtr kc,
440  std::vector<double> startingPoint,
441  std::vector<PoseBasePtr> globalDestinations,
442  std::vector<IKSolver::CartesianSelection> selections)
443 {
444  syncRobotPrx(kc);
445  //setup data
446  int failureCounter = 0;
447  RobotNodeSetPtr kcPrx = robotProxy->getRobotNodeSet(kc->getName());
448  std::vector<std::vector<double>> result = std::vector<std::vector<double>>();
449  std::vector<float> currentStart =
450  std::vector<float>(startingPoint.begin(), startingPoint.end());
451  int i = 0;
452 
453  //solve ik step-by-step
454  for (PoseBasePtr currentDest : globalDestinations)
455  {
456  std::vector<double> temp = std::vector<double>(currentStart.begin(), currentStart.end());
457  if (i != 0)
458  {
459  PosePtr priorPose = PosePtr(new Pose(globalDestinations.at(i - 1)->position,
460  globalDestinations.at(i - 1)->orientation));
461  PosePtr currentPose =
462  PosePtr(new Pose(currentDest->position, currentDest->orientation));
463  if (priorPose->toEigen() == currentPose->toEigen() && temp.size() != 0)
464  {
465  result.push_back(temp);
466  i++;
467  continue;
468  }
469  }
470  std::vector<double> currentResult = KinematicSolver::getInstance()->solveRecursiveIK(
471  kc, temp, currentDest, selections.at(i));
472  i++;
473  if (currentResult.size() == 0)
474  {
475  result.push_back(temp);
476  failureCounter++;
477  currentStart = std::vector<float>(temp.begin(), temp.end());
478  if (failureCounter > (double)globalDestinations.size() / 20.0)
479  {
480  throw LocalException(
481  "Too, many faults at current calculation: " + std::to_string(failureCounter) +
482  " Poses not reachable - only " +
483  std::to_string((double)globalDestinations.size() / 5.0) + " allowed!");
484  }
485  }
486  else
487  {
488  //std::cout << "OK!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
489  result.push_back(currentResult);
490  currentStart = std::vector<float>(currentResult.begin(), currentResult.end());
491  }
492  }
493  return result;
494 }
495 
496 std::vector<std::vector<double>>
497 KinematicSolver::solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc,
498  std::vector<double> startingPoint,
499  std::vector<PoseBasePtr> globalDestinations,
500  IKSolver::CartesianSelection selection)
501 {
502  syncRobotPrx(kc);
503  std::vector<IKSolver::CartesianSelection> selections =
504  std::vector<IKSolver::CartesianSelection>();
505  for (unsigned int i = 0; i < globalDestinations.size(); i++)
506  {
507  selections.push_back(selection);
508  }
509  return this->solveRecursiveIK(kc, startingPoint, globalDestinations, selections);
510 }
511 
512 std::vector<std::vector<double>>
513 KinematicSolver::solveRecursiveIKRelative(RobotNodeSetPtr kc,
514  std::vector<double> startingPoint,
515  std::vector<PoseBasePtr> framedDestinations,
516  std::vector<IKSolver::CartesianSelection> selections)
517 {
518  syncRobotPrx(kc);
519  std::vector<PoseBasePtr> globalDestinations = std::vector<PoseBasePtr>();
520  for (PoseBasePtr framedDestination : framedDestinations)
521  {
522  PosePtr localPose =
523  PosePtr(new Pose(framedDestination->position, framedDestination->orientation));
524  //std::cout << localPose->toEigen();
525  PoseBasePtr globalPose = PoseBasePtr(
526  new Pose(kc->getKinematicRoot()->toGlobalCoordinateSystem(localPose->toEigen())));
527  globalDestinations.push_back(globalPose);
528  }
529  return this->solveRecursiveIK(kc, startingPoint, globalDestinations, selections);
530 }
531 
532 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
533 ///FORWARD KINEMATIC & REACHABILITY///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
534 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
535 PoseBasePtr
536 KinematicSolver::doForwardKinematic(RobotNodeSetPtr kc, std::vector<double> jointAngles)
537 {
538  syncRobotPrx(kc);
539  RobotNodeSetPtr kcPrx = this->setupProxy(kc, jointAngles);
540  return PoseBasePtr(new Pose(kcPrx->getTCP()->getGlobalPose()));
541 }
542 
543 PoseBasePtr
544 KinematicSolver::doForwardKinematicRelative(RobotNodeSetPtr kc, std::vector<double> jointAngles)
545 {
546  syncRobotPrx(kc);
547  RobotNodeSetPtr kcPrx = this->setupProxy(kc, jointAngles);
548  return PoseBasePtr(new Pose(
549  kcPrx->getKinematicRoot()->toLocalCoordinateSystem(kcPrx->getTCP()->getGlobalPose())));
550 }
551 
552 bool
553 KinematicSolver::isReachable(RobotNodeSetPtr rns, PoseBasePtr pose, IKSolver::CartesianSelection cs)
554 {
555  syncRobotPrx(rns);
556  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
557  ///ALTERNATIVE WITH REACHABILITY MAPS
558  /*RobotNodeSetPtr set = robotProxy->getRobotNodeSet(rns->getName());
559  if ((reachabilityMap.find(set->getName()) == reachabilityMap.end()) && set->isKinematicChain())
560  {
561  ReachabilityPtr currentReach = ReachabilityPtr(new Reachability(robotProxy));
562  float minB[6] = { -1000.0f, -1000.0f, -1000.0f, (float) - M_PI, (float) - M_PI, (float) - M_PI};
563  float maxB[6] = {1000.0f, 1000.0f, 1000.0f, (float)M_PI, (float)M_PI, (float)M_PI};
564  ARMARX_INFO << robotProxy->getRootNode()->getName();
565  currentReach->initialize(rns, 200.0, 0.4, minB, maxB, set, set, robotProxy->getRootNode(), set->getTCP(), true);
566  currentReach->addRandomTCPPoses(1000000, 2, false);
567  reachabilityMap[set->getName()] = currentReach;
568  }
569  ReachabilityPtr reach = reachabilityMap[rns->getName()];
570  return reach->isReachable(Pose(pose->position, pose->orientation).toEigen());*/
571  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
572  return this->solveIK(rns, pose, cs, 50).size() != 0;
573 }
574 
575 void
576 KinematicSolver::syncRobotPrx(RobotNodeSetPtr rns)
577 {
578  if (currentRns != rns->getName())
579  {
580  currentRns = rns->getName();
581  this->syncRobotPrx();
582  }
583 }
584 
585 void
586 KinematicSolver::syncRobotPrx()
587 {
588  ARMARX_INFO << "Syncing Proxy";
589  for (RobotNodePtr node : robot->getRobotNodes())
590  {
591  this->robotProxy->getRobotNode(node->getName())->setJointValue(node->getJointValue());
592  }
593 }
594 
595 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
596 ///CONVINIENCE-METHODS///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
597 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
598 GenericIKSolverPtr
599 KinematicSolver::IKSetup(double jacStepSize,
600  int IKIterations,
601  double vectorError,
602  double orientationError,
603  RobotNodeSetPtr kc)
604 {
605  GenericIKSolverPtr ikSolver =
606  GenericIKSolverPtr(new GenericIKSolver(kc, JacobiProvider::eSVDDamped));
607  ikSolver->setupJacobian(jacStepSize, IKIterations);
608  ikSolver->setMaximumError(vectorError, orientationError);
609  return ikSolver;
610 }
611 
612 RobotNodeSetPtr
613 KinematicSolver::setupProxy(RobotNodeSetPtr kc, std::vector<double> jointAngles)
614 {
615  RobotNodeSetPtr kcPrx = robotProxy->getRobotNodeSet(kc->getName());
616  std::vector<float> jointValues = std::vector<float>(jointAngles.begin(), jointAngles.end());
617  kcPrx->setJointValues(jointValues);
618  return kcPrx;
619 }
620 
621 KinematicSolver::IKCalibration
622 KinematicSolver::calibrate()
623 {
624 
625  if (robot->getName() != "Armar3" && robot->getName() != "Armar4" &&
626  robot->getName() != "Armar6")
627  {
628  return this->IKCalibrationMap.at("Armar3");
629  }
630  else
631  {
632  return this->IKCalibrationMap.at(robot->getName());
633  }
634 }
635 
636 double
637 KinematicSolver::getMaxDistance(Eigen::Vector3f destination,
638  std::vector<std::vector<double>> jointAngles,
639  RobotNodeSetPtr rns)
640 {
641  double max = 0;
642  for (std::vector<double> ja : jointAngles)
643  {
644  PoseBasePtr pose = doForwardKinematicRelative(rns, ja);
645  double distance = sqrt(pow(pose->position->x - destination[0], 2) +
646  pow(pose->position->y - destination[1], 2) +
647  pow(pose->position->z - destination[2], 2));
648  if (distance > max)
649  {
650  max = distance;
651  }
652  }
653  return max;
654 }
655 
656 int
657 KinematicSolver::getFurthestNode(RobotNodeSetPtr rns,
658  Eigen::VectorXf startConfig,
659  Eigen::VectorXf endConfig)
660 {
661  double max = 0;
662  int best = rns->getSize() - 1;
663  rns->setJointValues(startConfig);
664  std::vector<Eigen::Vector3f> startPoses = std::vector<Eigen::Vector3f>();
665  std::vector<Eigen::Vector3f> endPoses = std::vector<Eigen::Vector3f>();
666  for (RobotNodePtr node : rns->getAllRobotNodes())
667  {
668  Eigen::Vector3f currentPose = Eigen::Vector3f();
669  currentPose[0] = node->getGlobalPose()(0, 3);
670  currentPose[1] = node->getGlobalPose()(1, 3);
671  currentPose[2] = node->getGlobalPose()(2, 3);
672  startPoses.push_back(currentPose);
673  }
674  rns->setJointValues(endConfig);
675  for (RobotNodePtr node : rns->getAllRobotNodes())
676  {
677  Eigen::Vector3f currentPose = Eigen::Vector3f();
678  currentPose[0] = node->getGlobalPose()(0, 3);
679  currentPose[1] = node->getGlobalPose()(1, 3);
680  currentPose[2] = node->getGlobalPose()(2, 3);
681  endPoses.push_back(currentPose);
682  }
683  for (unsigned int i = 0; i < startPoses.size(); i++)
684  {
685  double distance = (startPoses[i] - endPoses[i]).norm();
686  if (distance > max)
687  {
688  max = distance;
689  best = i;
690  }
691  }
692  return best;
693 }
MathUtils.h
armarx::KinematicSolver::solveRecursiveIKRelative
std::vector< float > solveRecursiveIKRelative(VirtualRobot::RobotNodeSetPtr kc, std::vector< float > startingPoint, PoseBasePtr framedDestination, VirtualRobot::IKSolver::CartesianSelection selection)
solveRecursiveIK solves IK based on a starting point and an End point based on one step
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
VirtualRobot
Definition: FramedPose.h:42
armarx::PosePtr
IceInternal::Handle< Pose > PosePtr
Definition: Pose.h:306
armarx::KinematicSolver::reset
static void reset()
reset Destroys the current instance of KinematicSolver that a new one can be created via call of getI...
Definition: KinematicSolver.cpp:117
armarx::KinematicSolver
Realizes the Singleton-Pattern, Provides Methods to solve Kinematic Problems (forward and inverse)
Definition: KinematicSolver.h:52
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:297
scene3D::ScenePtr
std::shared_ptr< Scene > ScenePtr
Definition: PointerDefinitions.h:36
armarx::KinematicSolver::solveIK
std::vector< double > solveIK(VirtualRobot::RobotNodeSetPtr kc, armarx::PoseBasePtr pose, VirtualRobot::IKSolver::CartesianSelection selection, int iterations)
IK///////////////////////////////////////////////////////////////////////////////////////////////////...
Definition: KinematicSolver.cpp:129
IceInternal::Handle< Pose >
armarx::KinematicSolver::solveIKRelative
std::vector< double > solveIKRelative(VirtualRobot::RobotNodeSetPtr kc, armarx::PoseBasePtr framedPose, VirtualRobot::IKSolver::CartesianSelection selection)
solveIK returns a random solution for an inverse Kinematic problem with no consideration of the curre...
Definition: KinematicSolver.cpp:169
armarx::KinematicSolver::isReachable
bool isReachable(VirtualRobot::RobotNodeSetPtr rns, PoseBasePtr globalPose, VirtualRobot::IKSolver::CartesianSelection cs)
isReachable calculates whether the PoseBase pose is reachable by the loaded robot or not
Definition: KinematicSolver.cpp:553
armarx::AbstractInterpolationPtr
std::shared_ptr< AbstractInterpolation > AbstractInterpolationPtr
Definition: AbstractInterpolation.h:76
armarx::KinematicSolver::doForwardKinematic
PoseBasePtr doForwardKinematic(VirtualRobot::RobotNodeSetPtr kc, std::vector< double > jointAngles)
FORWARD KINEMATIC & REACHABILITY/////////////////////////////////////////////////////////////////////...
Definition: KinematicSolver.cpp:536
armarx::NJointTaskSpaceDMPControllerMode::CartesianSelection
CartesianSelection
Definition: ControllerInterface.ice:34
RobotIK.h
armarx::KinematicSolver::solveRecursiveIK
std::vector< float > solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc, std::vector< float > startingPoint, PoseBasePtr globalDestination, VirtualRobot::IKSolver::CartesianSelection selection)
solveRecursiveIK solves IK based on a starting point and an End point based on one step
armarx::Pose::toEigen
virtual Eigen::Matrix4f toEigen() const
Definition: Pose.cpp:334
armarx::KinematicSolver::doForwardKinematicRelative
PoseBasePtr doForwardKinematicRelative(VirtualRobot::RobotNodeSetPtr kc, std::vector< double > jointAngles)
doForwardKinematic executes the given jointAngles on the loaded Robot
Definition: KinematicSolver.cpp:544
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
armarx::LinearInterpolation
The LinearInterpolation class represents a linear Interpolation between a series of control points Li...
Definition: LinearInterpolation.h:35
armarx::KinematicSolver::solveRecursiveIKNoMotionPlanning
std::vector< std::vector< double > > solveRecursiveIKNoMotionPlanning(VirtualRobot::RobotNodeSetPtr kc, std::vector< double > startingPoint, std::vector< PoseBasePtr > globalDestinations, std::vector< VirtualRobot::IKSolver::CartesianSelection > selections)
Definition: KinematicSolver.cpp:438
KinematicSolver.h
armarx::channels::KinematicUnitObserver::jointAngles
const KinematicUnitDatafieldCreator jointAngles("jointAngles")
armarx::KinematicSolver::getInstance
static std::shared_ptr< KinematicSolver > getInstance(VirtualRobot::ScenePtr scenario, VirtualRobot::RobotPtr robot)
SINGLETON-FEATURES///////////////////////////////////////////////////////////////////////////////////...
Definition: KinematicSolver.cpp:105
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:704
armarx::Pose
The Pose class.
Definition: Pose.h:242
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
LinearInterpolation.h
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:95
armarx::KinematicSolverPtr
std::shared_ptr< KinematicSolver > KinematicSolverPtr
Definition: KinematicSolver.h:287
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::KinematicSolver::syncRobotPrx
void syncRobotPrx()
syncRobotPrx always updates the robotProxy with the jointAngles of the "real" robot
Definition: KinematicSolver.cpp:586
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
norm
double norm(const Point &a)
Definition: point.hpp:102