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;
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().print(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();