38#include "QuotientSpacePlanningCommon.h"
39#include <ompl/base/spaces/RealVectorStateSpace.h>
41#include <ompl/geometric/planners/quotientspace/QRRT.h>
43#include <ompl/tools/benchmark/Benchmark.h>
44#include <ompl/util/String.h>
46#include <boost/math/constants/constants.hpp>
47#include <boost/format.hpp>
50const double edgeWidth = 0.1;
51const unsigned int ndim = 10;
52const double runtime_limit = 10;
53const double memory_limit = 4096;
54const int run_count = 10;
71 bool foundMaxDim =
false;
73 for (
int i = nDim_ - 1; i >= 0; i--)
76 if ((*s)[i] > edgeWidth)
79 else if ((*s)[i] < (1. - edgeWidth))
91 if (params.
hasParam(std::string(
"range")))
93 benchmark.addPlanner(planner);
96ob::PlannerPtr GetQRRT(ob::SpaceInformationPtr si, ob::ProblemDefinitionPtr pdef,
unsigned int numLinks)
99 std::vector<ob::SpaceInformationPtr> si_vec;
101 for (
unsigned int k = 2; k < numLinks; k += 2)
103 OMPL_INFORM(
"Create QuotientSpace Chain with %d links.", k);
105 auto spaceK(std::make_shared<ompl::base::RealVectorStateSpace>(k));
109 spaceK->setBounds(bounds);
111 auto siK = std::make_shared<ob::SpaceInformation>(spaceK);
112 siK->setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(siK, k));
113 siK->setStateValidityCheckingResolution(0.001);
116 si_vec.push_back(siK);
118 OMPL_INFORM(
"Add Original Chain with %d links.", numLinks);
119 si_vec.push_back(si);
121 auto planner = std::make_shared<og::QRRT>(si_vec);
122 planner->setProblemDefinition(pdef);
123 std::string qName =
"QuotientSpaceRRT[" + std::to_string(si_vec.size()) +
"lvl]";
124 planner->setName(qName);
130 double range = edgeWidth * 0.5;
131 auto space(std::make_shared<ompl::base::RealVectorStateSpace>(ndim));
138 space->setBounds(bounds);
139 ss.setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(ss.getSpaceInformation(), ndim));
140 ss.getSpaceInformation()->setStateValidityCheckingResolution(0.001);
141 for (
unsigned int i = 0; i < ndim; ++i)
146 ss.setStartAndGoalStates(start, goal);
150 b.addExperimentParameter(
"num_dims",
"INTEGER", std::to_string(ndim));
152 ob::SpaceInformationPtr si = ss.getSpaceInformation();
154 ob::PlannerPtr quotientSpacePlanner = GetQRRT(ss.getSpaceInformation(), ss.getProblemDefinition(), ndim);
155 addPlanner(b, quotientSpacePlanner, range);
157 b.benchmark(request);
158 b.saveResultsToFile(boost::str(boost::format(
"hypercube_%i.log") % ndim).c_str());
160 printBenchmarkResults(b);
Maintain a set of parameters.
bool hasParam(const std::string &key) const
Check whether this set of parameters includes the parameter named key.
bool setParam(const std::string &key, const std::string &value)
Algorithms in OMPL often have parameters that can be set externally. While each algorithm will have t...
A shared pointer wrapper for ompl::base::Planner.
The lower and upper bounds for an Rn space.
The definition of a state 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.
Definition of an abstract state.
Create the set of classes typically needed to solve a geometric problem.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Main namespace. Contains everything in this library.
std::string toString(float val)
convert float to string using classic "C" locale semantics