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;
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));
202 ss.setStatePropagator(odeSolver.getStatePropagator(&KinematicCarPostIntegration));
217 ss.setStartAndGoalStates(start, goal, 0.05);
227 std::cout <<
"Found solution:" << std::endl;
230 ss.getSolutionPath().asGeometric().print(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();