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());
Definition of a scoped state.
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.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
StateValidityChecker(SpaceInformation *si)
Constructor.
Definition of an abstract state.
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().
A shared pointer wrapper for ompl::base::OptimizationObjective.
double * values
The value of the actual vector in Rn
The definition of a state in Rn
SpaceInformation * si_
The instance of space information this state validity checker operates on.