37 #include <ompl/control/SpaceInformation.h>
38 #include <ompl/base/spaces/SE2StateSpace.h>
39 #include <ompl/control/ODESolver.h>
40 #include <ompl/control/spaces/RealVectorControlSpace.h>
41 #include <ompl/control/SimpleSetup.h>
42 #include <ompl/config.h>
47 namespace ob = ompl::base;
48 namespace oc = ompl::control;
56 space_ = si->getStateSpace();
63 EulerIntegration(state, control, duration, result);
71 std::valarray<double> dstate;
72 space_->copyState(result, start);
73 while (t < duration + std::numeric_limits<double>::epsilon())
75 ode(result, control, dstate);
76 update(result, timeStep_ * dstate);
79 if (t + std::numeric_limits<double>::epsilon() > duration)
81 ode(result, control, dstate);
82 update(result, (t - duration) * dstate);
87 void ode(
const ob::State *state,
const oc::Control *control, std::valarray<double> &dstate)
const
89 const double *u = control->
as<oc::RealVectorControlSpace::ControlType>()->values;
93 dstate[0] = u[0] * cos(theta);
94 dstate[1] = u[0] * sin(theta);
95 dstate[2] = u[0] * tan(u[1]) / carLength_;
99 void update(
ob::State *state,
const std::valarray<double> &dstate)
const
102 s.setX(s.getX() + dstate[0]);
103 s.setY(s.getY() + dstate[1]);
104 s.setYaw(s.getYaw() + dstate[2]);
105 space_->enforceBounds(state);
117 const double *u = control->
as<oc::RealVectorControlSpace::ControlType>()->values;
118 const double theta = q[2];
119 double carLength = 0.2;
122 qdot.resize (q.size (), 0);
124 qdot[0] = u[0] * cos(theta);
125 qdot[1] = u[0] * sin(theta);
126 qdot[2] = u[0] * tan(u[1]) / carLength;
153 return si->
satisfiesBounds(state) && (
const void*)rot != (
const void*)pos;
161 DemoControlSpace(
const ob::StateSpacePtr &stateSpace) : oc::RealVectorControlSpace(stateSpace, 2)
167 void planWithSimpleSetup(
void)
184 cbounds.setLow(-0.3);
185 cbounds.setHigh(0.3);
187 cspace->as<DemoControlSpace>()->setBounds(cbounds);
193 ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
217 ss.setStartAndGoalStates(start, goal, 0.05);
227 std::cout <<
"Found solution:" << std::endl;
230 ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
233 std::cout <<
"No solution found" << std::endl;
236 int main(
int,
char **)
238 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
240 planWithSimpleSetup();
Definition of a scoped state.
Definition of an abstract control.
A boost shared pointer wrapper for ompl::base::StateSpace.
virtual void propagate(const base::State *state, const Control *control, const 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...
Create the set of classes typically needed to solve a control problem.
static StatePropagatorPtr getStatePropagator(ODESolverPtr solver, const PostPropagationEvent &postEvent=NULL)
Retrieve a StatePropagator object that solves a system of ordinary differential equations defined by ...
CompoundState StateType
Define the type of state allocated by this state space.
A boost shared pointer wrapper for ompl::control::ODESolver.
Model the effect of controls on system states.
State StateType
Define the type of state allocated by this space.
const T * as(void) const
Cast this instance to a desired type.
A boost shared pointer wrapper for ompl::control::ControlSpace.
A control space representing Rn.
A state space representing SE(2)
std::vector< double > StateType
Portable data type for the state values.
A class to store the exit status of Planner::solve()
Definition of an abstract state.
Basic solver for ordinary differential equations of the type q' = f(q, u), where q is the current sta...
The lower and upper bounds for an Rn space.
const T * as(void) const
Cast this instance to a desired type.
A state space representing SO(2). The distance function and interpolation take into account angle wra...
virtual void enforceBounds(State *state) const
Normalize the value of the state to the interval (-Pi, Pi].
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
StatePropagator(SpaceInformation *si)
Constructor.