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
37using namespace armarx;
38using namespace VirtualRobot;
39
40KinematicSolverPtr KinematicSolver::singleton = 0;
41
42KinematicSolver::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
83KinematicSolver::IKCalibration
84KinematicSolver::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/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
104KinematicSolverPtr
105KinematicSolver::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
116void
117KinematicSolver::reset()
118{
119 ARMARX_INFO << "KinematicSolver reset";
120 singleton = 0;
121}
122
123/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
124///IK////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
125/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
126
127
128std::vector<double>
129KinematicSolver::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
168std::vector<double>
169KinematicSolver::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
179std::vector<float>
180KinematicSolver::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
202std::vector<float>
203KinematicSolver::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
217std::vector<double>
218KinematicSolver::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
236std::vector<double>
237KinematicSolver::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
250std::vector<std::vector<double>>
251KinematicSolver::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
437std::vector<std::vector<double>>
438KinematicSolver::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
496std::vector<std::vector<double>>
497KinematicSolver::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
512std::vector<std::vector<double>>
513KinematicSolver::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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
535PoseBasePtr
536KinematicSolver::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
543PoseBasePtr
544KinematicSolver::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
552bool
553KinematicSolver::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
575void
576KinematicSolver::syncRobotPrx(RobotNodeSetPtr rns)
577{
578 if (currentRns != rns->getName())
579 {
580 currentRns = rns->getName();
581 this->syncRobotPrx();
582 }
583}
584
585void
586KinematicSolver::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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
598GenericIKSolverPtr
599KinematicSolver::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
612RobotNodeSetPtr
613KinematicSolver::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
621KinematicSolver::IKCalibration
622KinematicSolver::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
636double
637KinematicSolver::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
656int
657KinematicSolver::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}
float
#define float
Definition 16_Level.h:22
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::KinematicSolver::getInstance
static std::shared_ptr< KinematicSolver > getInstance(VirtualRobot::ScenePtr scenario, VirtualRobot::RobotPtr robot)
SINGLETON-FEATURES///////////////////////////////////////////////////////////////////////////////////...
Definition KinematicSolver.cpp:105
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::syncRobotPrx
void syncRobotPrx()
syncRobotPrx always updates the robotProxy with the jointAngles of the "real" robot
Definition KinematicSolver.cpp:586
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::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::KinematicSolver::doForwardKinematic
PoseBasePtr doForwardKinematic(VirtualRobot::RobotNodeSetPtr kc, std::vector< double > jointAngles)
FORWARD KINEMATIC & REACHABILITY/////////////////////////////////////////////////////////////////////...
Definition KinematicSolver.cpp:536
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::KinematicSolver::solveIK
std::vector< double > solveIK(VirtualRobot::RobotNodeSetPtr kc, armarx::PoseBasePtr pose, VirtualRobot::IKSolver::CartesianSelection selection, int iterations)
IK///////////////////////////////////////////////////////////////////////////////////////////////////...
Definition KinematicSolver.cpp:129
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
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::Pose
The Pose class.
Definition Pose.h:243
armarx::Pose::toEigen
virtual Eigen::Matrix4f toEigen() const
Definition Pose.cpp:334
ARMARX_INFO
#define ARMARX_INFO
The normal logging level.
Definition Logging.h:181
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition VectorXD.h:704
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition Bus.h:19
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition basic_types.h:31
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition ArmarXTimeserver.cpp:28
armarx::KinematicSolverPtr
std::shared_ptr< KinematicSolver > KinematicSolverPtr
Definition KinematicSolver.h:287
armarx::PosePtr
IceInternal::Handle< Pose > PosePtr
Definition Pose.h:306
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition VectorHelpers.h:297
armarx::AbstractInterpolationPtr
std::shared_ptr< AbstractInterpolation > AbstractInterpolationPtr
Definition AbstractInterpolation.h:76
norm
double norm(const Point &a)
Definition point.hpp:102
distance
double distance(const Point &a, const Point &b)
Definition point.hpp:95