KinematicChain.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, 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 the 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: Bryant Gipson, Mark Moll */
36 
37 #ifndef OMPL_DEMO_KINEMATIC_CHAIN_
38 #define OMPL_DEMO_KINEMATIC_CHAIN_
39 
40 #include <ompl/base/spaces/RealVectorStateSpace.h>
41 #include <ompl/geometric/planners/rrt/RRT.h>
42 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
43 #include <ompl/geometric/planners/est/EST.h>
44 #include <ompl/geometric/planners/prm/PRM.h>
45 #include <ompl/geometric/planners/stride/STRIDE.h>
46 #include <ompl/tools/benchmark/Benchmark.h>
47 
48 #include <boost/math/constants/constants.hpp>
49 #include <boost/format.hpp>
50 #include <fstream>
51 
52 // a 2D line segment
53 struct Segment
54 {
55  Segment(double p0_x, double p0_y, double p1_x, double p1_y) : x0(p0_x), y0(p0_y), x1(p1_x), y1(p1_y)
56  {
57  }
58  double x0, y0, x1, y1;
59 };
60 
61 // the robot and environment are modeled both as a vector of segments.
62 using Environment = std::vector<Segment>;
63 
64 // simply use a random projection
66 {
67 public:
69  {
70  int dimension = std::max(2, (int)ceil(log((double)space->getDimension())));
71  projectionMatrix_.computeRandom(space->getDimension(), dimension);
72  }
73  unsigned int getDimension() const override
74  {
75  return projectionMatrix_.mat.rows();
76  }
77  void project(const ompl::base::State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
78  {
79  std::vector<double> v(space_->getDimension());
80  space_->copyToReals(v, state);
81  projectionMatrix_.project(&v[0], projection);
82  }
83 
84 protected:
85  ompl::base::ProjectionMatrix projectionMatrix_;
86 };
87 
89 {
90 public:
91  KinematicChainSpace(unsigned int numLinks, double linkLength, Environment *env = nullptr)
92  : ompl::base::RealVectorStateSpace(numLinks), linkLength_(linkLength), environment_(env)
93  {
94  ompl::base::RealVectorBounds bounds(numLinks);
95  bounds.setLow(-boost::math::constants::pi<double>());
96  bounds.setHigh(boost::math::constants::pi<double>());
97  setBounds(bounds);
98  }
99 
100  void registerProjections() override
101  {
102  registerDefaultProjection(std::make_shared<KinematicChainProjector>(this));
103  }
104 
105  double distance(const ompl::base::State *state1, const ompl::base::State *state2) const override
106  {
107  const auto *cstate1 = state1->as<StateType>();
108  const auto *cstate2 = state2->as<StateType>();
109  double theta1 = 0., theta2 = 0., dx = 0., dy = 0., dist = 0.;
110 
111  for (unsigned int i = 0; i < dimension_; ++i)
112  {
113  theta1 += cstate1->values[i];
114  theta2 += cstate2->values[i];
115  dx += cos(theta1) - cos(theta2);
116  dy += sin(theta1) - sin(theta2);
117  dist += sqrt(dx * dx + dy * dy);
118  }
119 
120  return dist * linkLength_;
121  }
122 
123  void enforceBounds(ompl::base::State *state) const override
124  {
125  auto *statet = state->as<StateType>();
126 
127  for (unsigned int i = 0; i < dimension_; ++i)
128  {
129  double v = fmod(statet->values[i], 2.0 * boost::math::constants::pi<double>());
130  if (v < -boost::math::constants::pi<double>())
131  v += 2.0 * boost::math::constants::pi<double>();
132  else if (v >= boost::math::constants::pi<double>())
133  v -= 2.0 * boost::math::constants::pi<double>();
134  statet->values[i] = v;
135  }
136  }
137 
138  bool equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const override
139  {
140  bool flag = true;
141  const auto *cstate1 = state1->as<StateType>();
142  const auto *cstate2 = state2->as<StateType>();
143 
144  for (unsigned int i = 0; i < dimension_ && flag; ++i)
145  flag &= fabs(cstate1->values[i] - cstate2->values[i]) < std::numeric_limits<double>::epsilon() * 2.0;
146 
147  return flag;
148  }
149 
150  void interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t,
151  ompl::base::State *state) const override
152  {
153  const auto *fromt = from->as<StateType>();
154  const auto *tot = to->as<StateType>();
155  auto *statet = state->as<StateType>();
156 
157  for (unsigned int i = 0; i < dimension_; ++i)
158  {
159  double diff = tot->values[i] - fromt->values[i];
160  if (fabs(diff) <= boost::math::constants::pi<double>())
161  statet->values[i] = fromt->values[i] + diff * t;
162  else
163  {
164  if (diff > 0.0)
165  diff = 2.0 * boost::math::constants::pi<double>() - diff;
166  else
167  diff = -2.0 * boost::math::constants::pi<double>() - diff;
168 
169  statet->values[i] = fromt->values[i] - diff * t;
170  if (statet->values[i] > boost::math::constants::pi<double>())
171  statet->values[i] -= 2.0 * boost::math::constants::pi<double>();
172  else if (statet->values[i] < -boost::math::constants::pi<double>())
173  statet->values[i] += 2.0 * boost::math::constants::pi<double>();
174  }
175  }
176  }
177 
178  double linkLength() const
179  {
180  return linkLength_;
181  }
182 
183  const Environment *environment() const
184  {
185  return environment_;
186  }
187 
188 protected:
189  double linkLength_;
190  Environment *environment_;
191 };
192 
194 {
195 public:
197  {
198  }
199 
200  bool isValid(const ompl::base::State *state) const override
201  {
202  const KinematicChainSpace *space = si_->getStateSpace()->as<KinematicChainSpace>();
203  const auto *s = state->as<KinematicChainSpace::StateType>();
204 
205  return isValidImpl(space, s);
206  }
207 
208 protected:
209  bool isValidImpl(const KinematicChainSpace *space, const KinematicChainSpace::StateType *s) const
210  {
211  unsigned int n = si_->getStateDimension();
212  Environment segments;
213  double linkLength = space->linkLength();
214  double theta = 0., x = 0., y = 0., xN, yN;
215 
216  segments.reserve(n + 1);
217  for (unsigned int i = 0; i < n; ++i)
218  {
219  theta += s->values[i];
220  xN = x + cos(theta) * linkLength;
221  yN = y + sin(theta) * linkLength;
222  segments.emplace_back(x, y, xN, yN);
223  x = xN;
224  y = yN;
225  }
226  xN = x + cos(theta) * 0.001;
227  yN = y + sin(theta) * 0.001;
228  segments.emplace_back(x, y, xN, yN);
229  return selfIntersectionTest(segments) && environmentIntersectionTest(segments, *space->environment());
230  }
231 
232  // return true iff env does *not* include a pair of intersecting segments
233  bool selfIntersectionTest(const Environment &env) const
234  {
235  for (unsigned int i = 0; i < env.size(); ++i)
236  for (unsigned int j = i + 1; j < env.size(); ++j)
237  if (intersectionTest(env[i], env[j]))
238  return false;
239  return true;
240  }
241  // return true iff no segment in env0 intersects any segment in env1
242  bool environmentIntersectionTest(const Environment &env0, const Environment &env1) const
243  {
244  for (const auto &i : env0)
245  for (const auto &j : env1)
246  if (intersectionTest(i, j))
247  return false;
248  return true;
249  }
250  // return true iff segment s0 intersects segment s1
251  bool intersectionTest(const Segment &s0, const Segment &s1) const
252  {
253  // adopted from:
254  // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect/1201356#1201356
255  double s10_x = s0.x1 - s0.x0;
256  double s10_y = s0.y1 - s0.y0;
257  double s32_x = s1.x1 - s1.x0;
258  double s32_y = s1.y1 - s1.y0;
259  double denom = s10_x * s32_y - s32_x * s10_y;
260  if (fabs(denom) < std::numeric_limits<double>::epsilon())
261  return false; // Collinear
262  bool denomPositive = denom > 0;
263 
264  double s02_x = s0.x0 - s1.x0;
265  double s02_y = s0.y0 - s1.y0;
266  double s_numer = s10_x * s02_y - s10_y * s02_x;
267  if ((s_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
268  return false; // No collision
269  double t_numer = s32_x * s02_y - s32_y * s02_x;
270  if ((t_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
271  return false; // No collision
272  if (((s_numer - denom > -std::numeric_limits<float>::epsilon()) == denomPositive) ||
273  ((t_numer - denom > std::numeric_limits<float>::epsilon()) == denomPositive))
274  return false; // No collision
275  return true;
276  }
277 };
278 
279 Environment createHornEnvironment(unsigned int d, double eps)
280 {
281  std::ofstream envFile(boost::str(boost::format("environment_%i.dat") % d));
282  std::vector<Segment> env;
283  double w = 1. / (double)d, x = w, y = -eps, xN, yN, theta = 0.,
284  scale = w * (1. + boost::math::constants::pi<double>() * eps);
285 
286  envFile << x << " " << y << std::endl;
287  for (unsigned int i = 0; i < d - 1; ++i)
288  {
289  theta += boost::math::constants::pi<double>() / (double)d;
290  xN = x + cos(theta) * scale;
291  yN = y + sin(theta) * scale;
292  env.emplace_back(x, y, xN, yN);
293  x = xN;
294  y = yN;
295  envFile << x << " " << y << std::endl;
296  }
297 
298  theta = 0.;
299  x = w;
300  y = eps;
301  envFile << x << " " << y << std::endl;
302  scale = w * (1.0 - boost::math::constants::pi<double>() * eps);
303  for (unsigned int i = 0; i < d - 1; ++i)
304  {
305  theta += boost::math::constants::pi<double>() / d;
306  xN = x + cos(theta) * scale;
307  yN = y + sin(theta) * scale;
308  env.emplace_back(x, y, xN, yN);
309  x = xN;
310  y = yN;
311  envFile << x << " " << y << std::endl;
312  }
313  envFile.close();
314  return env;
315 }
316 
317 Environment createEmptyEnvironment(unsigned int d)
318 {
319  std::ofstream envFile(boost::str(boost::format("environment_%i.dat") % d));
320  std::vector<Segment> env;
321  envFile.close();
322  return env;
323 }
324 
325 #endif
bool equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const override
Checks whether two states are equal.
A shared pointer wrapper for ompl::base::SpaceInformation.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
void computeRandom(unsigned int from, unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
Definition of an abstract state.
Definition: State.h:113
unsigned int getStateDimension() const
Return the dimension of the state space.
A projection matrix – it allows multiplication of real vectors by a specified matrix....
void project(const double *from, Eigen::Ref< Eigen::VectorXd > to) const
Multiply the vector from by the contained projection matrix to obtain the vector to.
virtual void copyToReals(std::vector< double > &reals, const State *source) const
Copy all the real values from a state source to the array reals using getValueAddressAtLocation()
Definition: StateSpace.cpp:329
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
bool isValid(const ompl::base::State *state) const override
Return true if the state state is valid. Usually, this means at least collision checking....
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
A state space representing Rn. The distance function is the L2 norm.
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
double distance(const ompl::base::State *state1, const ompl::base::State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
Definition: StateSpace.cpp:752
unsigned int dimension_
The dimension of the space.
void setBounds(const RealVectorBounds &bounds)
Set the bounds of this state space. This defines the range of the space in which sampling is performe...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
void interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
SpaceInformation * si_
The instance of space information this state validity checker operates on.
void enforceBounds(ompl::base::State *state) const override
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
void project(const ompl::base::State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
const StateSpace * space_
The state space this projection operates on.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Matrix mat
Projection matrix.
The lower and upper bounds for an Rn space.