PlanarManipulatorTSRRTConfig.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_TSRRT_CONFIG_H_
38 #define PLANAR_MANIPULATOR_TSRRT_CONFIG_H_
39 
40 #include "PlanarManipulator.h"
41 
42 #include <ompl/geometric/planners/rrt/TSRRT.h>
43 #include <ompl/util/RandomNumbers.h>
44 
46 {
47 public:
49  : manip_(manip), task_space_bounds_(bounds)
50  {
51  }
52 
53  virtual int getDimension() const
54  {
55  return 2;
56  }
57 
58  virtual void project(const ompl::base::State *state, Eigen::Ref<Eigen::VectorXd> ts_proj) const
59  {
60  project(state->as<PlanarManipulatorStateSpace::StateType>()->values, ts_proj);
61  }
62 
63  virtual void sample(Eigen::Ref<Eigen::VectorXd> ts_proj) const
64  {
65  // Sample point uniformly in task-space
66  for (unsigned int dim = 0; dim < task_space_bounds_.low.size(); ++dim)
67  {
68  ts_proj[dim] = rng_.uniformReal(task_space_bounds_.low[dim], task_space_bounds_.high[dim]);
69  }
70  }
71 
72  // Given a point in task-space, generate a configuraton space state
73  // that projects to this point. seed is the nearest task-space neighbor.
74  // Returns false if a lifted configuration could not be generated.
75  virtual bool lift(const Eigen::Ref<Eigen::VectorXd> &ts_proj, const ompl::base::State *seed,
76  ompl::base::State *state) const
77  {
79  std::vector<double> seed_angles(manip_->getNumLinks());
80  for (unsigned int ix = 0; ix < seed_angles.size(); ++ix)
81  {
82  seed_angles[ix] = (*pm_seed)[ix];
83  }
84 
85  // Allow any angle.
86  Eigen::Affine2d end_frame(Eigen::Rotation2Dd(0.0) * Eigen::Translation2d(ts_proj[0], ts_proj[1]));
87 
88  std::vector<double> lifted_angles;
89  if (!manip_->FABRIK(lifted_angles, seed_angles, end_frame, /*xyTol=*/1e-4,
90  /*thetaTol=*/2.0 * M_PI))
91  {
92  return false;
93  }
94 
96  for (unsigned int ix = 0; ix < manip_->getNumLinks(); ++ix)
97  {
98  (*pm_state)[ix] = lifted_angles[ix];
99  }
100  return true;
101  }
102 
103 private:
104  // Forward kinematics to find the end-effector position (proj) in the task space.
105  void project(const double *angles, Eigen::Ref<Eigen::VectorXd> proj) const
106  {
107  Eigen::Affine2d frame;
108  manip_->FK(angles, frame);
109  proj[0] = frame.translation()(0);
110  proj[1] = frame.translation()(1);
111  }
112 
113  const PlanarManipulator *manip_;
114  const ompl::base::RealVectorBounds task_space_bounds_;
115  mutable ompl::RNG rng_;
116 };
117 
118 #endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:90
Definition of an abstract state.
Definition: State.h:114
std::vector< double > low
Lower bound.
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
std::vector< double > high
Upper bound.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
The lower and upper bounds for an Rn space.