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;
67 :
ompl::base::ProjectionEvaluator(space)
69 int dimension = std::max(2, (
int)ceil(
log((
double) space->
getDimension())));
70 projectionMatrix_.computeRandom(space->
getDimension(), dimension);
72 virtual unsigned int getDimension(
void)
const 74 return projectionMatrix_.mat.size1();
78 std::vector<double> v(space_->getDimension());
79 space_->copyToReals(v, state);
80 projectionMatrix_.project(&v[0], projection);
90 KinematicChainSpace(
unsigned int numLinks,
double linkLength, Environment *env = NULL)
91 :
ompl::base::CompoundStateSpace(), linkLength_(linkLength), environment_(env)
93 for (
unsigned int i = 0; i < numLinks; ++i)
98 void registerProjections()
101 new KinematicChainProjector(
this)));
106 const StateType *cstate1 = state1->
as<StateType>();
107 const StateType *cstate2 = state2->
as<StateType>();
108 double theta1 = 0., theta2 = 0., dx = 0., dy = 0., dist = 0.;
110 for (
unsigned int i = 0; i < getSubspaceCount(); ++i)
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_;
139 :
ompl::base::StateValidityChecker(si)
145 const KinematicChainSpace* space = si_->getStateSpace()->
as<KinematicChainSpace>();
147 unsigned int n = si_->getStateDimension();
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 = 60, 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.
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
base::State * getState(unsigned int index)
Get the state located at index along the path.
T * as()
Cast this instance to a desired type.
The definition of a state in SO(2)
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
Main namespace. Contains everything in this library.
Create the set of classes typically needed to solve a geometric problem.
A boost shared pointer wrapper for ompl::base::Planner.
const T * as() const
Cast this instance to a desired type.
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...
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.
A projection matrix – it allows multiplication of real vectors by a specified matrix. The matrix can also be randomly generated.
Probabilistic RoadMap planner.
A state space representing SO(2). The distance function and interpolation take into account angle wra...
Definition of a geometric path.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...