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/SimpleSetup.h>
44#include <ompl/config.h>
54class KinematicCarModel
58 KinematicCarModel(
const ob::StateSpace *space) : space_(space), carLength_(0.2)
63 void operator()(
const ob::State *state,
const oc::Control *control, std::valarray<double> &dstate)
const
65 const double *u = control->
as<oc::RealVectorControlSpace::ControlType>()->values;
66 const double theta = state->
as<ob::SE2StateSpace::StateType>()->getYaw();
69 dstate[0] = u[0] * cos(theta);
70 dstate[1] = u[0] * sin(theta);
71 dstate[2] = u[0] * tan(u[1]) / carLength_;
75 void update(
ob::State *state,
const std::valarray<double> &dstate)
const
77 ob::SE2StateSpace::StateType &s = *state->
as<ob::SE2StateSpace::StateType>();
78 s.setX(s.getX() + dstate[0]);
79 s.setY(s.getY() + dstate[1]);
80 s.setYaw(s.getYaw() + dstate[2]);
81 space_->enforceBounds(state);
87 const double carLength_;
98 EulerIntegrator(
const ob::StateSpace *space,
double timeStep) : space_(space), timeStep_(timeStep), ode_(space)
104 double t = timeStep_;
105 std::valarray<double> dstate;
106 space_->copyState(result, start);
107 while (t < duration + std::numeric_limits<double>::epsilon())
109 ode_(result, control, dstate);
110 ode_.update(result, timeStep_ * dstate);
113 if (t + std::numeric_limits<double>::epsilon() > duration)
115 ode_(result, control, dstate);
116 ode_.update(result, (t - duration) * dstate);
120 double getTimeStep()
const
125 void setTimeStep(
double timeStep)
127 timeStep_ = timeStep;
142 const auto *se2state = state->
as<ob::SE2StateSpace::StateType>();
145 const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
148 const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
154 return si->satisfiesBounds(state) && (
const void*)rot != (
const void*)pos;
162 DemoControlSpace(
const ob::StateSpacePtr &stateSpace) :
oc::RealVectorControlSpace(stateSpace, 2)
172 integrator_(si->getStateSpace().get(), 0.0)
178 integrator_.propagate(state, control, duration, result);
181 void setIntegrationTimeStep(
double timeStep)
183 integrator_.setTimeStep(timeStep);
186 double getIntegrationTimeStep()
const
188 return integrator_.getTimeStep();
191 EulerIntegrator<KinematicCarModel> integrator_;
196void planWithSimpleSetup()
199 auto space(std::make_shared<ob::SE2StateSpace>());
206 space->setBounds(bounds);
209 auto cspace(std::make_shared<DemoControlSpace>(space));
213 cbounds.setLow(-0.3);
214 cbounds.setHigh(0.3);
216 cspace->setBounds(cbounds);
223 ss.setStateValidityChecker(
224 [si](
const ob::State *state) {
return isStateValid(si, state); });
227 auto propagator(std::make_shared<DemoStatePropagator>(si));
228 ss.setStatePropagator(propagator);
243 ss.setStartAndGoalStates(start, goal, 0.05);
247 propagator->setIntegrationTimeStep(si->getPropagationStepSize());
254 std::cout <<
"Found solution:" << std::endl;
257 ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
260 std::cout <<
"No solution found" << std::endl;
263int main(
int ,
char ** )
265 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
267 planWithSimpleSetup();
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Representation of a space in which planning can be performed. Topology specific sampling,...
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 control space representing Rn.
Create the set of classes typically needed to solve a control problem.
Model the effect of controls on system states.
virtual void propagate(const base::State *state, const Control *control, double duration, base::State *result) const =0
Propagate from a state, given a control, for some specified amount of time (the amount of time can al...
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()