38#include "MultiLevelPlanningCommon.h"
39#include "MultiLevelPlanningHyperCubeCommon.h"
40#include <ompl/base/spaces/RealVectorStateSpace.h>
42#include <ompl/multilevel/planners/qrrt/QRRT.h>
44#include <ompl/tools/benchmark/Benchmark.h>
45#include <ompl/util/String.h>
47#include <boost/math/constants/constants.hpp>
48#include <boost/format.hpp>
51const unsigned int ndim = 100;
55 auto space(std::make_shared<ompl::base::RealVectorStateSpace>(ndim));
64 space->setBounds(bounds);
65 ss.setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(si, ndim));
66 si->setStateValidityCheckingResolution(0.001);
67 for (
unsigned int i = 0; i < ndim; ++i)
72 ss.setStartAndGoalStates(start, goal);
74 std::vector<int> admissibleProjection = getHypercubeAdmissibleProjection(ndim);
75 ompl::base::PlannerPtr planner = GetMultiLevelPlanner<ompl::multilevel::QRRT>(admissibleProjection, si,
"QRRT");
76 ss.setPlanner(planner);
78 bool solved = ss.solve(1.0);
80 double timeToCompute = ss.getLastPlanComputationTime();
85 std::cout << std::string(80,
'-') << std::endl;
86 pdef->getSolutionPath()->print(std::cout);
87 std::cout << std::string(80,
'-') << std::endl;
88 OMPL_INFORM(
"Solved hypercube with %d dimensions after %f seconds.", ndim, timeToCompute);
92 OMPL_ERROR(
"Failed finding solution after %f seconds.", timeToCompute);
A shared pointer wrapper for ompl::base::Planner.
A shared pointer wrapper for ompl::base::ProblemDefinition.
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Create the set of classes typically needed to solve a geometric problem.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.