37 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
38 #include <ompl/base/spaces/RealVectorStateSpace.h>
39 #include <ompl/geometric/planners/rrt/RRTXstatic.h>
40 #include <ompl/geometric/planners/rrt/RRTsharp.h>
41 #include <ompl/geometric/planners/rrt/RRTstar.h>
42 #include <ompl/tools/benchmark/Benchmark.h>
44 #include <boost/format.hpp>
45 #include <boost/math/constants/constants.hpp>
61 sum += state2D->
values[i] * state2D->values[i];
63 return sqrt(sum) > 0.1;
67 int main(
int argc,
char **argv)
71 std::cout <<
"Usage: " << argv[0] <<
" dimensionOfTheProblem" << std::endl;
74 int dim = atoi(argv[1]);
76 auto space(std::make_shared<ompl::base::RealVectorStateSpace>(dim));
79 space->setBounds(-1, 1);
81 ss.setStateValidityChecker(std::make_shared<ValidityChecker>(si));
84 for (
int i = 0; i < dim; ++i)
90 ss.setStartAndGoalStates(start, goal);
93 double runtime_limit = 5, memory_limit = 1024;
98 double range = 0.1 * sqrt(dim);
100 auto lengthObj(std::make_shared<ompl::base::PathLengthOptimizationObjective>(si));
103 ss.setOptimizationObjective(oop);
107 auto rrtstar(std::make_shared<ompl::geometric::RRTstar>(si));
108 rrtstar->setName(
"RRT*");
109 rrtstar->setDelayCC(
true);
111 rrtstar->setRange(range);
112 rrtstar->setKNearest(knn);
113 b.addPlanner(rrtstar);
114 auto rrtsh(std::make_shared<ompl::geometric::RRTsharp>(si));
115 rrtsh->setRange(range);
116 rrtsh->setKNearest(knn);
130 auto rrtX1(std::make_shared<ompl::geometric::RRTXstatic>(si));
131 rrtX1->setName(
"RRTX0.1");
132 rrtX1->setEpsilon(0.1);
133 rrtX1->setRange(range);
135 rrtX1->setKNearest(knn);
137 auto rrtX2(std::make_shared<ompl::geometric::RRTXstatic>(si));
138 rrtX2->setName(
"RRTX0.01");
139 rrtX2->setEpsilon(0.01);
140 rrtX2->setRange(range);
142 rrtX2->setKNearest(knn);
144 auto rrtX3(std::make_shared<ompl::geometric::RRTXstatic>(si));
145 rrtX3->setName(
"RRTX0.001");
146 rrtX3->setEpsilon(0.001);
147 rrtX3->setRange(range);
149 rrtX3->setKNearest(knn);
151 b.benchmark(request);
152 b.saveResultsToFile(boost::str(boost::format(
"Diagonal.log")).c_str());
A shared pointer wrapper for ompl::base::OptimizationObjective.
The definition of a state in Rn
double * values
The value of the actual vector in Rn
Definition of a scoped state.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking....
StateValidityChecker(SpaceInformation *si)
Constructor.
SpaceInformation * si_
The instance of space information this state validity checker operates on.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
Create the set of classes typically needed to solve a geometric problem.
Main namespace. Contains everything in this library.