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 int nsteps = ceil(duration / timeStep);
55 double dt = duration / nsteps;
56 const double *u = control->
as<oc::RealVectorControlSpace::ControlType>()->values;
64 for(
int i=0; i<nsteps; i++)
66 se2.setX(se2.getX() + dt * velocity.values[0] * cos(se2.getYaw()));
67 se2.setY(se2.getY() + dt * velocity.values[0] * sin(se2.getYaw()));
68 se2.setYaw(se2.getYaw() + dt * u[0]);
69 velocity.values[0] = velocity.values[0] + dt * (u[1]*gear.value);
74 if (gear.value < 3 && velocity.values[0] > 10*(gear.value + 1))
76 else if (gear.value > 1 && velocity.values[0] < 10*gear.value)
90 return si->
satisfiesBounds(state) && (se2->getX() < -80. || se2->getY() > 80.);
111 velocityBound.setLow(0);
112 velocityBound.setHigh(60);
122 start[0] = start[1] = -90.;
123 start[2] = boost::math::constants::pi<double>()/2;
125 start->as<
ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3;
127 goal[0] = goal[1] = 90.;
137 cbounds.setLow(0, -1.);
138 cbounds.setHigh(0, 1.);
140 cbounds.setLow(1, -20.);
141 cbounds.setHigh(1, 20.);
145 setup.setStartAndGoalStates(start, goal, 5.);
146 setup.setStateValidityChecker(boost::bind(
147 &isStateValid, setup.getSpaceInformation().get(), _1));
148 setup.setStatePropagator(boost::bind(
149 &propagate, setup.getSpaceInformation().get(), _1, _2, _3, _4));
150 setup.getSpaceInformation()->setPropagationStepSize(.1);
151 setup.getSpaceInformation()->setMinMaxControlDuration(2, 3);
162 for(
unsigned int i=0; i<path.getStateCount(); ++i)
164 const ob::State* state = path.getState(i);
171 std::cout << se2->getX() <<
' ' << se2->getY() <<
' ' << se2->getYaw()
172 <<
' ' << velocity->values[0] <<
' ' << gear->value <<
' ';
175 std::cout <<
"0 0 0";
180 path.getControl(i-1)->as<oc::RealVectorControlSpace::ControlType>()->values;
181 std::cout << u[0] <<
' ' << u[1] <<
' ' << path.getControlDuration(i-1);
183 std::cout << std::endl;
185 if (!setup.haveExactSolutionPath())
187 std::cout <<
"Solution is approximate. Distance to actual goal is " <<
188 setup.getProblemDefinition()->getSolutionDifference() << std::endl;
Definition of a compound state.
Definition of a scoped state.
Definition of an abstract control.
A boost shared pointer wrapper for ompl::base::StateSpace.
Create the set of classes typically needed to solve a control problem.
CompoundState StateType
Define the type of state allocated by this state space.
Definition of a control path.
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 state space representing Rn. The distance function is the L2 norm.
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.
A space representing discrete states; i.e. there are a small number of discrete states the system can...
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.