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
56 {
57 public:
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 
93 protected:
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
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:89
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of a goal region that can be sampled, but the sampling process can be slow....
Definition of an abstract state.
Definition: State.h:113
double * values
The value of the actual vector in Rn
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
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...