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