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>
51 ValidityChecker(
const ompl::base::SpaceInformationPtr &si) : ompl::base::
StateValidityChecker(si)
55 bool isValid(
const ompl::base::State *state)
const override
60 for (
unsigned i = 0; i <
si_->getStateSpace()->getDimension(); ++i)
61 sum += state2D->values[i] * state2D->values[i];
63 return sqrt(sum) > 0.1;
67int 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));
78 const ompl::base::SpaceInformationPtr &si = ss.getSpaceInformation();
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));
101 ompl::base::OptimizationObjectivePtr oop((0.5 / sqrt(dim)) * lengthObj);
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.
ompl::base::State StateType
Define the type of state allocated by this space.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
StateValidityChecker(SpaceInformation *si)
Constructor.
SpaceInformation * si_
The instance of space information this state validity checker operates on.
const T * as() const
Cast this instance to a desired type.
Create the set of classes typically needed to solve a geometric problem.