RigidBodyPlanningWithODESolverAndControls.cpp
50 // Kinematic car model object definition. This class does NOT use ODESolver to propagate the system.
61 virtual void propagate(const ob::State *state, const oc::Control* control, const double duration, ob::State *result) const
68 void EulerIntegration(const ob::State *start, const oc::Control *control, const double duration, ob::State *result) const
87 void ode(const ob::State *state, const oc::Control *control, std::valarray<double> &dstate) const
115 void KinematicCarODE (const oc::ODESolver::StateType& q, const oc::Control* control, oc::ODESolver::StateType& qdot)
130 void KinematicCarPostIntegration (const ob::State* /*state*/, const oc::Control* /*control*/, const double /*duration*/, ob::State *result)
134 SO2.enforceBounds (result->as<ob::SE2StateSpace::StateType>()->as<ob::SO2StateSpace::StateType>(1));
144 const ob::RealVectorStateSpace::StateType *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
152 // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
161 DemoControlSpace(const ob::StateSpacePtr &stateSpace) : oc::RealVectorControlSpace(stateSpace, 2)
197 //ss.setStatePropagator(oc::StatePropagatorPtr(new KinematicCarModel(ss.getSpaceInformation())));
201 oc::ODESolverPtr odeSolver(new oc::ODEBasicSolver<> (ss.getSpaceInformation(), &KinematicCarODE));
202 ss.setStatePropagator(oc::ODESolver::getStatePropagator(odeSolver, &KinematicCarPostIntegration));
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
A boost shared pointer wrapper for ompl::base::StateSpace.
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:64
static StatePropagatorPtr getStatePropagator(ODESolverPtr solver, const PostPropagationEvent &postEvent=NULL)
Retrieve a StatePropagator object that solves a system of ordinary differential equations defined by ...
Definition: ODESolver.h:129
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...
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:549
A boost shared pointer wrapper for ompl::control::ODESolver.
Model the effect of controls on system states.
Definition: StatePropagator.h:62
A boost shared pointer wrapper for ompl::control::ControlSpace.
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
Definition: SpaceInformation.h:119
A control space representing Rn.
Definition: RealVectorControlSpace.h:63
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
A boost shared pointer wrapper for ompl::control::SpaceInformation.
Basic solver for ordinary differential equations of the type q' = f(q, u), where q is the current sta...
Definition: ODESolver.h:198
The lower and upper bounds for an Rn space.
Definition: RealVectorBounds.h:48
A state space representing SO(2). The distance function and interpolation take into account angle wra...
Definition: SO2StateSpace.h:65
virtual void enforceBounds(State *state) const
Normalize the value of the state to the interval (-Pi, Pi].
Definition: SO2StateSpace.cpp:83
Space information containing necessary information for planning with controls. setup() needs to be ca...
Definition: SpaceInformation.h:69
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:109