37 #include <ompl/base/goals/GoalState.h>
38 #include <ompl/base/spaces/SE2StateSpace.h>
39 #include <ompl/base/spaces/DiscreteStateSpace.h>
40 #include <ompl/control/spaces/RealVectorControlSpace.h>
41 #include <ompl/control/SimpleSetup.h>
42 #include <ompl/config.h>
45 #include <boost/math/constants/constants.hpp>
47 namespace ob = ompl::base;
48 namespace oc = ompl::control;
53 static double timeStep = .01;
54 static double carLength = 20.;
55 int nsteps = ceil(duration / timeStep);
56 double dt = duration / nsteps;
57 const double *u = control->
as<oc::RealVectorControlSpace::ControlType>()->values;
65 for(
int i=0; i<nsteps; i++)
67 se2.setX(se2.getX() + timeStep * velocity.values[0] * cos(se2.getYaw()));
68 se2.setY(se2.getY() + timeStep * velocity.values[0] * sin(se2.getYaw()));
69 se2.setYaw(se2.getYaw() + timeStep * u[0]);
70 velocity.values[0] = velocity.values[0] + timeStep * (u[1]*gear.value);
75 if (gear.value < 3 && velocity.values[0] > 10*(gear.value + 1))
77 else if (gear.value > 1 && velocity.values[0] < 10*gear.value)
91 return si->
satisfiesBounds(state) && (se2->getX() < -80. || se2->getY() > 80.);
95 int main(
int argc,
char* argv[])
112 velocityBound.setLow(0);
113 velocityBound.setHigh(60);
123 start[0] = start[1] = -90.;
124 start[2] = boost::math::constants::pi<double>()/2;
126 start->as<
ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3;
128 goal[0] = goal[1] = 90.;
138 cbounds.setLow(0, -1.);
139 cbounds.setHigh(0, 1.);
141 cbounds.setLow(1, -20.);
142 cbounds.setHigh(1, 20.);
146 setup.setStartAndGoalStates(start, goal, 5.);
147 setup.setStateValidityChecker(boost::bind(
148 &isStateValid, setup.getSpaceInformation().get(), _1));
149 setup.setStatePropagator(boost::bind(
150 &propagate, setup.getSpaceInformation().get(), _1, _2, _3, _4));
151 setup.getSpaceInformation()->setPropagationStepSize(.1);
152 setup.getSpaceInformation()->setMinMaxControlDuration(2, 3);
163 for(
unsigned int i=0; i<path.getStateCount(); ++i)
165 const ob::State* state = path.getState(i);
172 std::cout << se2->getX() <<
' ' << se2->getY() <<
' ' << se2->getYaw()
173 <<
' ' << velocity->values[0] <<
' ' << gear->value <<
' ';
176 std::cout <<
"0 0 0";
181 path.getControl(i-1)->as<oc::RealVectorControlSpace::ControlType>()->values;
182 std::cout << u[0] <<
' ' << u[1] <<
' ' << path.getControlDuration(i-1);
184 std::cout << std::endl;
186 if (!setup.haveExactSolutionPath())
188 std::cout <<
"Solution is approximate. Distance to actual goal is " <<
189 setup.getProblemDefinition()->getSolutionDifference() << std::endl;