37#include <ompl/control/SpaceInformation.h>
38#include <ompl/base/goals/GoalState.h>
39#include <ompl/base/spaces/SE2StateSpace.h>
40#include <ompl/control/spaces/RealVectorControlSpace.h>
41#include <ompl/control/planners/kpiece/KPIECE1.h>
42#include <ompl/control/planners/rrt/RRT.h>
43#include <ompl/control/planners/est/EST.h>
44#include <ompl/control/planners/syclop/SyclopRRT.h>
45#include <ompl/control/planners/syclop/SyclopEST.h>
46#include <ompl/control/planners/pdst/PDST.h>
47#include <ompl/control/planners/syclop/GridDecomposition.h>
48#include <ompl/control/SimpleSetup.h>
49#include <ompl/config.h>
59 MyDecomposition(
const int length,
const ob::RealVectorBounds& bounds)
63 void project(
const ob::State* s, std::vector<double>& coord)
const override
70 void sampleFullState(
const ob::StateSamplerPtr& sampler,
const std::vector<double>& coord, ob::State* s)
const override
72 sampler->sampleUniform(s);
93 return si->satisfiesBounds(state) && (
const void*)rot != (
const void*)pos;
104 pos[0] + ctrl[0] * duration * cos(rot),
105 pos[1] + ctrl[0] * duration * sin(rot));
107 rot + ctrl[1] * duration);
114 auto space(std::make_shared<ob::SE2StateSpace>());
121 space->setBounds(bounds);
124 auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));
128 cbounds.setLow(-0.3);
129 cbounds.setHigh(0.3);
131 cspace->setBounds(cbounds);
134 auto si(std::make_shared<oc::SpaceInformation>(space, cspace));
137 si->setStateValidityChecker(
138 [&si](
const ob::State *state) {
return isStateValid(si.get(), state); });
141 si->setStatePropagator(propagate);
154 auto pdef(std::make_shared<ob::ProblemDefinition>(si));
157 pdef->setStartAndGoalStates(start, goal, 0.1);
163 auto decomp(std::make_shared<MyDecomposition>(32, bounds));
164 auto planner(std::make_shared<oc::SyclopEST>(si, decomp));
168 planner->setProblemDefinition(pdef);
175 si->printSettings(std::cout);
178 pdef->print(std::cout);
187 ob::PathPtr path = pdef->getSolutionPath();
188 std::cout <<
"Found solution:" << std::endl;
191 path->print(std::cout);
194 std::cout <<
"No solution found" << std::endl;
198void planWithSimpleSetup()
201 auto space(std::make_shared<ob::SE2StateSpace>());
208 space->setBounds(bounds);
211 auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));
215 cbounds.setLow(-0.3);
216 cbounds.setHigh(0.3);
218 cspace->setBounds(cbounds);
224 ss.setStatePropagator(propagate);
227 ss.setStateValidityChecker(
228 [&ss](
const ob::State *state) {
return isStateValid(ss.getSpaceInformation().get(), state); });
244 ss.setStartAndGoalStates(start, goal, 0.05);
253 std::cout <<
"Found solution:" << std::endl;
256 ss.getSolutionPath().printAsMatrix(std::cout);
259 std::cout <<
"No solution found" << std::endl;
262int main(
int ,
char ** )
264 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
270 planWithSimpleSetup();
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
The lower and upper bounds for an Rn space.
The definition of a state in Rn
A state in SE(2): (x, y, yaw)
The definition of a state in SO(2)
Definition of a scoped state.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
Definition of an abstract control.
const T * as() const
Cast this instance to a desired type.
A GridDecomposition is a Decomposition implemented using a grid.
GridDecomposition(int len, int dim, const base::RealVectorBounds &b)
Constructor. Creates a GridDecomposition as a hypercube with a given dimension, side length,...
The definition of a control in Rn
Create the set of classes typically needed to solve a control problem.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
A class to store the exit status of Planner::solve()