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>
66 coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
67 coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
70 void sampleFullState(
const ob::StateSamplerPtr& sampler,
const std::vector<double>& coord,
ob::State* s)
const override
72 sampler->sampleUniform(s);
73 s->as<ob::SE2StateSpace::StateType>()->setXY(coord[0], coord[1]);
81 const auto *se2state = state->
as<ob::SE2StateSpace::StateType>();
84 const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
87 const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
93 return si->satisfiesBounds(state) && (
const void*)rot != (
const void*)pos;
98 const auto *se2state = start->
as<ob::SE2StateSpace::StateType>();
99 const double* pos = se2state->as<ob::RealVectorStateSpace::StateType>(0)->values;
100 const double rot = se2state->as<ob::SO2StateSpace::StateType>(1)->value;
101 const double* ctrl = control->
as<oc::RealVectorControlSpace::ControlType>()->values;
103 result->
as<ob::SE2StateSpace::StateType>()->setXY(
104 pos[0] + ctrl[0] * duration * cos(rot),
105 pos[1] + ctrl[0] *
duration * sin(rot));
106 result->
as<ob::SE2StateSpace::StateType>()->setYaw(
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); });
238 (*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
239 (*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.5;
240 (*goal)[1]->as<ob::SO2StateSpace::StateType>()->value = 0.0;
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();
The lower and upper bounds for an Rn space.
Definition of a scoped state.
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.
virtual void sampleFullState(const base::StateSamplerPtr &sampler, const std::vector< double > &coord, base::State *s) const =0
Samples a State using a projected coordinate and a StateSampler.
virtual void project(const base::State *s, std::vector< double > &coord) const =0
Project a given State to a set of coordinates in R^k, where k is the dimension of this Decomposition.
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,...
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...
std::chrono::system_clock::duration duration
Representation of a time duration.
A class to store the exit status of Planner::solve()