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
53struct 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.
62using Environment = std::vector<Segment>;
63
64// simply use a random projection
66{
67public:
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
84protected:
85 ompl::base::ProjectionMatrix projectionMatrix_;
86};
87
89{
90public:
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
188protected:
189 double linkLength_;
190 Environment *environment_;
191};
192
194{
195public:
197 {
198 }
199
200 bool isValid(const ompl::base::State *state) const override
201 {
203 const auto *s = state->as<KinematicChainSpace::StateType>();
204
205 return isValidImpl(space, s);
206 }
207
208protected:
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
279Environment 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
317Environment 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
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
void project(const ompl::base::State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
bool equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const override
Checks whether two states are equal.
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 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-...
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
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....
bool isValid(const ompl::base::State *state) const override
Return true if the state state is valid. Usually, this means at least collision checking....
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
const StateSpace * space_
The state space this projection operates on.
A projection matrix – it allows multiplication of real vectors by a specified matrix....
Matrix mat
Projection 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.
void computeRandom(unsigned int from, unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
The lower and upper bounds for an Rn space.
A state space representing Rn. The distance function is the L2 norm.
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...
A shared pointer wrapper for ompl::base::SpaceInformation.
unsigned int getStateDimension() const
Return the dimension of the state space.
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:71
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
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 registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
Definition: StateSpace.cpp:752
Abstract definition for a class checking the validity of states. The implementation of this class mus...
SpaceInformation * si_
The instance of space information this state validity checker operates on.
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
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