37 #include <ompl/base/spaces/SO2StateSpace.h>
38 #include <ompl/geometric/planners/rrt/RRT.h>
39 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
40 #include <ompl/geometric/planners/est/EST.h>
41 #include <ompl/geometric/planners/prm/PRM.h>
42 #include <ompl/geometric/planners/stride/STRIDE.h>
43 #include <ompl/tools/benchmark/Benchmark.h>
45 #include <boost/math/constants/constants.hpp>
46 #include <boost/format.hpp>
52 Segment(
double p0_x,
double p0_y,
double p1_x,
double p1_y)
53 : x0(p0_x), y0(p0_y), x1(p1_x), y1(p1_y)
56 double x0, y0, x1, y1;
60 typedef std::vector<Segment> Environment;
69 int dimension = std::max(2, (
int)ceil(
log((
double) space->
getDimension())));
70 projectionMatrix_.computeRandom(space->
getDimension(), dimension);
74 return projectionMatrix_.mat.size1();
80 projectionMatrix_.project(&v[0], projection);
90 KinematicChainSpace(
unsigned int numLinks,
double linkLength, Environment *env = NULL)
93 for (
unsigned int i = 0; i < numLinks; ++i)
101 new KinematicChainProjector(
this)));
108 double theta1 = 0., theta2 = 0., dx = 0., dy = 0., dist = 0.;
114 dx += cos(theta1) - cos(theta2);
115 dy += sin(theta1) - sin(theta2);
116 dist += sqrt(dx * dx + dy * dy);
118 return dist * linkLength_;
120 double linkLength()
const
124 const Environment* environment()
const
131 Environment* environment_;
145 const KinematicChainSpace* space =
static_cast<const KinematicChainSpace*
>(
si_->
getStateSpace().get());
148 Environment segments;
149 double linkLength = space->linkLength();
150 double theta = 0., x = 0., y = 0., xN, yN;
152 segments.reserve(n + 1);
153 for(
unsigned int i = 0; i < n; ++i)
156 xN = x + cos(theta) * linkLength;
157 yN = y + sin(theta) * linkLength;
158 segments.push_back(Segment(x, y, xN, yN));
162 xN = x + cos(theta) * 0.001;
163 yN = y + sin(theta) * 0.001;
164 segments.push_back(Segment(x, y, xN, yN));
165 return selfIntersectionTest(segments)
166 && environmentIntersectionTest(segments, *space->environment());
171 bool selfIntersectionTest(
const Environment& env)
const
173 for (
unsigned int i = 0; i < env.size(); ++i)
174 for (
unsigned int j = i + 1; j < env.size(); ++j)
175 if (intersectionTest(env[i], env[j]))
180 bool environmentIntersectionTest(
const Environment& env0,
const Environment& env1)
const
182 for (
unsigned int i = 0; i < env0.size(); ++i)
183 for (
unsigned int j = 0; j < env1.size(); ++j)
184 if (intersectionTest(env0[i], env1[j]))
189 bool intersectionTest(
const Segment& s0,
const Segment& s1)
const
193 double s10_x = s0.x1 - s0.x0;
194 double s10_y = s0.y1 - s0.y0;
195 double s32_x = s1.x1 - s1.x0;
196 double s32_y = s1.y1 - s1.y0;
197 double denom = s10_x * s32_y - s32_x * s10_y;
198 if (fabs(denom) < std::numeric_limits<double>::epsilon())
200 bool denomPositive = denom > 0;
202 double s02_x = s0.x0 - s1.x0;
203 double s02_y = s0.y0 - s1.y0;
204 double s_numer = s10_x * s02_y - s10_y * s02_x;
205 if ((s_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
207 double t_numer = s32_x * s02_y - s32_y * s02_x;
208 if ((t_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
210 if (((s_numer - denom > -std::numeric_limits<float>::epsilon()) == denomPositive)
211 || ((t_numer - denom > std::numeric_limits<float>::epsilon()) == denomPositive))
218 Environment createHornEnvironment(
unsigned int d,
double eps)
220 std::ofstream envFile(
"environment.dat");
221 std::vector<Segment> env;
222 double w = 1. / (double)d, x = w, y = -eps, xN, yN, theta = 0.,
223 scale = w * (1. + boost::math::constants::pi<double>() * eps);
225 envFile << x <<
" " << y << std::endl;
226 for(
unsigned int i = 0; i < d - 1; ++i)
228 theta += boost::math::constants::pi<double>() / (
double) d;
229 xN = x + cos(theta) * scale;
230 yN = y + sin(theta) * scale;
231 env.push_back(Segment(x, y, xN, yN));
234 envFile << x << " " << y << std::endl;
240 envFile << x << " " << y << std::endl;
241 scale = w * (1.0 - boost::math::constants::pi<
double>() * eps);
242 for(
unsigned int i = 0; i < d - 1; ++i)
244 theta += boost::math::constants::pi<double>() / d;
245 xN = x + cos(theta) * scale;
246 yN = y + sin(theta) * scale;
247 env.push_back(Segment(x, y, xN, yN));
250 envFile << x <<
" " << y << std::endl;
257 int main(
int argc,
char **argv)
261 std::cout <<
"Usage:\n" << argv[0] <<
" <num_links>\n";
265 unsigned int numLinks = boost::lexical_cast<
unsigned int>(std::string(argv[1]));
266 Environment env = createHornEnvironment(numLinks,
log((
double)numLinks) / (
double)numLinks);
271 new KinematicChainValidityChecker(ss.getSpaceInformation())));
274 std::vector<double> startVec(numLinks, boost::math::constants::pi<double>() / (
double)numLinks);
275 std::vector<double> goalVec(numLinks, 0.);
278 goalVec[0] = boost::math::constants::pi<double>() - .001;
280 chain->copyFromReals(start.get(), startVec);
281 chain->copyFromReals(goal.get(), goalVec);
282 ss.setStartAndGoalStates(start, goal);
293 ss.simplifySolution();
296 std::vector<double> v;
299 chain->copyToReals(v, path.
getState(i));
300 std::copy(v.begin(), v.end(), std::ostream_iterator<double>(std::cout,
" "));
301 std::cout << std::endl;
307 double runtime_limit = 7200, memory_limit = 1024;
317 b.benchmark(request);
318 b.saveResultsToFile(boost::str(boost::format(
"kinematic_%i.log") % numLinks).c_str());
Search Tree with Resolution Independent Density Estimation.
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 of a scoped state.
A boost shared pointer wrapper for ompl::base::StateSpace.
ProjectionEvaluator(const StateSpace *space)
Construct a projection evaluator for a specific state space.
virtual unsigned int getDimension(void) const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
CompoundState StateType
Define the type of state allocated by this state space.
virtual unsigned int getDimension(void) const =0
Return the dimension of the projection defined by this evaluator.
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
base::State * getState(unsigned int index)
Get the state located at index along the path.
std::size_t getStateCount(void) const
Get the number of states (way-points) that make up this path.
The definition of a state in SO(2)
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
const T * as(void) const
Cast this instance to a desired type.
virtual void registerProjections(void)
Register the projections for this state space. Usually, this is at least the default projection...
const StateSpace * space_
The state space this projection operates on.
Create the set of classes typically needed to solve a geometric problem.
A boost shared pointer wrapper for ompl::base::Planner.
Kinematic Planning by Interior-Exterior Cell Exploration.
A space to allow the composition of state spaces.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
StateValidityChecker(SpaceInformation *si)
Constructor.
unsigned int getSubspaceCount(void) const
Get the number of state spaces that make up the compound state space.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
Rapidly-exploring Random Trees.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
CompoundStateSpace(void)
Construct an empty compound state space.
void lock(void)
Lock this state space. This means no further spaces can be added as components. This function can be ...
A projection matrix – it allows multiplication of real vectors by a specified matrix. The matrix can also be randomly generated.
void addSubspace(const StateSpacePtr &component, double weight)
Adds a new state space as part of the compound state space. For computing distances within the compou...
virtual void project(const State *state, EuclideanProjection &projection) const =0
Compute the projection as an array of double values.
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking. If it is possible that ompl::base::StateSpace::interpolate() or ompl::control::ControlSpace::propagate() return states that are outside of bounds, this function should also make a call to ompl::base::SpaceInformation::satisfiesBounds().
Probabilistic RoadMap planner.
State ** components
The components that make up a compound state.
A state space representing SO(2). The distance function and interpolation take into account angle wra...
SpaceInformation * si_
The instance of space information this state validity checker operates on.
Definition of a geometric path.
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() ...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...