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> 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.
const T * as() const
Cast this instance to a desired type.
Definition of a scoped state.
Definition of an abstract control.
This namespace contains sampling based planning routines used by planning under differential constrai...
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.
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.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The lower and upper bounds for an Rn space.
A space representing discrete states; i.e. there are a small number of discrete states the system can...
const T * as() const
Cast this instance to a desired type.
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.