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>
49 namespace ob = ompl::base;
50 namespace oc = ompl::control;
54 class 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;
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
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(
void)
const
125 void setTimeStep(
double timeStep)
127 timeStep_ = timeStep;
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(
void)
const
188 return integrator_.getTimeStep();
191 EulerIntegrator<KinematicCarModel> integrator_;
196 void planWithSimpleSetup(
void)
213 cbounds.setLow(-0.3);
214 cbounds.setHigh(0.3);
216 cspace->as<DemoControlSpace>()->setBounds(cbounds);
222 ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
225 ss.setStatePropagator(oc::StatePropagatorPtr(
new DemoStatePropagator(ss.getSpaceInformation())));
240 ss.setStartAndGoalStates(start, goal, 0.05);
244 static_cast<DemoStatePropagator*
>(ss.getStatePropagator().get())->setIntegrationTimeStep(ss.getSpaceInformation()->getPropagationStepSize());
251 std::cout <<
"Found solution:" << std::endl;
254 ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
257 std::cout <<
"No solution found" << std::endl;
260 int main(
int,
char **)
262 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
264 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.
CompoundState StateType
Define the type of state allocated by this state space.
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)
A class to store the exit status of Planner::solve()
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition of an abstract state.
The lower and upper bounds for an Rn space.
const T * as(void) const
Cast this instance to a desired type.