Loading...
Searching...
No Matches
PlanarManipulatorIKGoal.h
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ryan Luna */
36
37#ifndef PLANAR_MANIPULATOR_IK_GOAL_H_
38#define PLANAR_MANIPULATOR_IK_GOAL_H_
39
40#include <ompl/base/goals/GoalLazySamples.h>
41#include <Eigen/Dense>
42#include "PlanarManipulator.h"
43#include "PlanarManipulatorStateSpace.h"
44
45#include <boost/math/constants/constants.hpp>
46
47#ifndef PI
48#define PI boost::math::constants::pi<double>()
49#define TWOPI boost::math::constants::two_pi<double>()
50#endif
51
52// A goal that allows for specification of position (and optionally, the
53// orientation) for the end effector of a planar manipulator.
54// Uses GoalLazySamples to sample valid IK positions
55class PlanarManipulatorIKGoal : public ompl::base::GoalLazySamples
56{
57public:
58 // If fixedOrientation is false, the orientation in the goalPose is not
59 // considered (a random orientation will be sampled).
60 PlanarManipulatorIKGoal(const ompl::base::SpaceInformationPtr &si, const Eigen::Affine2d &goalPose,
61 const PlanarManipulator *manipulator, bool fixedOrientation = true)
63 si, [this](const ompl::base::GoalLazySamples *, ompl::base::State *st) { return sampleGoalThread(st); },
64 true)
65 , goalPose_(goalPose)
66 , manipulator_(manipulator)
67 , fixedOrientation_(fixedOrientation)
68 {
69 }
70
71 virtual double distanceGoal(const ompl::base::State *st) const
72 {
73 const double *angles = st->as<PlanarManipulatorStateSpace::StateType>()->values;
74
75 // Figure out where the end effector is
76 Eigen::Affine2d eeFrame;
77 manipulator_->FK(angles, eeFrame);
78
79 double cartesianDist = (eeFrame.translation() - goalPose_.translation()).norm();
80
81 // Orientation does not matter
82 if (!fixedOrientation_)
83 return cartesianDist;
84
85 double eeRot = acos(eeFrame.matrix()(0, 0));
86 double goalRot = acos(goalPose_.matrix()(0, 0));
87 double angleDiff = fabs(goalRot - eeRot);
88
89 // return the translational and rotational differences
90 return cartesianDist + angleDiff;
91 }
92
93protected:
94 bool sampleGoalThread(ompl::base::State *st) const
95 {
96 std::vector<double> seed(manipulator_->getNumLinks());
97 std::vector<double> soln(manipulator_->getNumLinks());
98
99 bool good = false;
100 unsigned int maxTries = 1000;
101 unsigned int tries = 0;
102 do
103 {
104 // random seed
105 for (size_t i = 0; i < seed.size(); ++i)
106 seed[i] = rng_.uniformReal(-PI, PI);
107
108 for (size_t i = 0; i < 10 && !good; ++i)
109 {
110 Eigen::Affine2d pose(goalPose_);
111 if (!fixedOrientation_)
112 {
113 // Sample the orientation if it does not matter
114 pose.rotate(rng_.uniformReal(-PI, PI));
115 }
116
117 if (manipulator_->FABRIK(soln, seed, pose))
118 {
119 // copy values into state
120 memcpy(st->as<PlanarManipulatorStateSpace::StateType>()->values, &soln[0],
121 soln.size() * sizeof(double));
122 // GoalLazySamples will check validity
123 good = true;
124 }
125 tries++;
126 }
127
128 } while (!good && tries < maxTries);
129
130 return good;
131 }
132
133 Eigen::Affine2d goalPose_;
134 const PlanarManipulator *manipulator_;
135 bool fixedOrientation_;
136 mutable ompl::RNG rng_;
137};
138
139#endif
virtual double distanceGoal(const ompl::base::State *st) const
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition of a goal region that can be sampled, but the sampling process can be slow....
ompl::base::State StateType
Define the type of state allocated by this space.
Definition StateSpace.h:78
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66