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 using Environment = std::vector<Segment>;
67 :
ompl::base::ProjectionEvaluator(space)
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 =
nullptr)
91 : linkLength_(linkLength), environment_(env)
93 for (
unsigned int i = 0; i < numLinks; ++i)
94 addSubspace(std::make_shared<ompl::base::SO2StateSpace>(), 1.);
107 double theta1 = 0., theta2 = 0., dx = 0., dy = 0., dist = 0.;
113 dx += cos(theta1) - cos(theta2);
114 dy += sin(theta1) - sin(theta2);
115 dist += sqrt(dx * dx + dy * dy);
117 return dist * linkLength_;
119 double linkLength()
const 123 const Environment* environment()
const 130 Environment* environment_;
144 const KinematicChainSpace* space =
si_->
getStateSpace()->as<KinematicChainSpace>();
147 Environment segments;
148 double linkLength = space->linkLength();
149 double theta = 0., x = 0., y = 0., xN, yN;
151 segments.reserve(n + 1);
152 for(
unsigned int i = 0; i < n; ++i)
155 xN = x + cos(theta) * linkLength;
156 yN = y + sin(theta) * linkLength;
157 segments.emplace_back(x, y, xN, yN);
161 xN = x + cos(theta) * 0.001;
162 yN = y + sin(theta) * 0.001;
163 segments.emplace_back(x, y, xN, yN);
164 return selfIntersectionTest(segments)
165 && environmentIntersectionTest(segments, *space->environment());
170 bool selfIntersectionTest(
const Environment& env)
const 172 for (
unsigned int i = 0; i < env.size(); ++i)
173 for (
unsigned int j = i + 1; j < env.size(); ++j)
174 if (intersectionTest(env[i], env[j]))
179 bool environmentIntersectionTest(
const Environment& env0,
const Environment& env1)
const 181 for (
const auto & i : env0)
182 for (
const auto & j : env1)
183 if (intersectionTest(i, j))
188 bool intersectionTest(
const Segment& s0,
const Segment& s1)
const 192 double s10_x = s0.x1 - s0.x0;
193 double s10_y = s0.y1 - s0.y0;
194 double s32_x = s1.x1 - s1.x0;
195 double s32_y = s1.y1 - s1.y0;
196 double denom = s10_x * s32_y - s32_x * s10_y;
197 if (fabs(denom) < std::numeric_limits<double>::epsilon())
199 bool denomPositive = denom > 0;
201 double s02_x = s0.x0 - s1.x0;
202 double s02_y = s0.y0 - s1.y0;
203 double s_numer = s10_x * s02_y - s10_y * s02_x;
204 if ((s_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
206 double t_numer = s32_x * s02_y - s32_y * s02_x;
207 if ((t_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
209 if (((s_numer - denom > -std::numeric_limits<float>::epsilon()) == denomPositive)
210 || ((t_numer - denom > std::numeric_limits<float>::epsilon()) == denomPositive))
217 Environment createHornEnvironment(
unsigned int d,
double eps)
219 std::ofstream envFile(boost::str(boost::format(
"environment_%i.dat") % d));
220 std::vector<Segment> env;
221 double w = 1. / (double)d, x = w, y = -eps, xN, yN, theta = 0.,
222 scale = w * (1. + boost::math::constants::pi<double>() * eps);
224 envFile << x <<
" " << y << std::endl;
225 for(
unsigned int i = 0; i < d - 1; ++i)
227 theta += boost::math::constants::pi<double>() / (
double) d;
228 xN = x + cos(theta) * scale;
229 yN = y + sin(theta) * scale;
230 env.emplace_back(x, y, xN, yN);
233 envFile << x <<
" " << y << std::endl;
239 envFile << x <<
" " << y << std::endl;
240 scale = w * (1.0 - boost::math::constants::pi<double>() * eps);
241 for(
unsigned int i = 0; i < d - 1; ++i)
243 theta += boost::math::constants::pi<double>() / d;
244 xN = x + cos(theta) * scale;
245 yN = y + sin(theta) * scale;
246 env.emplace_back(x, y, xN, yN);
249 envFile << x <<
" " << y << std::endl;
256 int main(
int argc,
char **argv)
260 std::cout <<
"Usage:\n" << argv[0] <<
" <num_links>\n";
264 auto numLinks = boost::lexical_cast<
unsigned int>(std::string(argv[1]));
265 Environment env = createHornEnvironment(numLinks,
log((
double)numLinks) / (
double)numLinks);
266 auto chain(std::make_shared<KinematicChainSpace>(numLinks, 1. / (
double)numLinks, &env));
269 ss.setStateValidityChecker(std::make_shared<KinematicChainValidityChecker>(ss.getSpaceInformation()));
272 std::vector<double> startVec(numLinks, boost::math::constants::pi<double>() / (
double)numLinks);
273 std::vector<double> goalVec(numLinks, 0.);
276 goalVec[0] = boost::math::constants::pi<double>() - .001;
278 chain->copyFromReals(start.get(), startVec);
279 chain->copyFromReals(goal.get(), goalVec);
280 ss.setStartAndGoalStates(start, goal);
287 ss.setPlanner(std::make_shared<ompl::geometric::STRIDE>(ss.getSpaceInformation()));
291 ss.simplifySolution();
293 std::ofstream pathfile(
294 boost::str(boost::format(
"kinematic_path_%i.dat") % numLinks).c_str());
295 ss.getSolutionPath().printAsMatrix(pathfile);
300 double runtime_limit = 60, memory_limit = 1024;
304 b.addExperimentParameter(
"num_links",
"INTEGER", std::to_string(numLinks));
306 b.addPlanner(std::make_shared<ompl::geometric::STRIDE>(ss.getSpaceInformation()));
307 b.addPlanner(std::make_shared<ompl::geometric::EST>(ss.getSpaceInformation()));
308 b.addPlanner(std::make_shared<ompl::geometric::KPIECE1>(ss.getSpaceInformation()));
309 b.addPlanner(std::make_shared<ompl::geometric::RRT>(ss.getSpaceInformation()));
310 b.addPlanner(std::make_shared<ompl::geometric::PRM>(ss.getSpaceInformation()));
311 b.benchmark(request);
312 b.saveResultsToFile(boost::str(boost::format(
"kinematic_%i.log") % numLinks).c_str());
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
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.
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
The definition of a state in SO(2)
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() ...
const StateSpace * space_
The state space this projection operates on.
Main namespace. Contains everything in this library.
Create the set of classes typically needed to solve a geometric problem.
const T * as() const
Cast this instance to a desired type.
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
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.
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.
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().
SpaceInformation * si_
The instance of space information this state validity checker operates on.
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...