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);
56 const double *u = control->
as<oc::RealVectorControlSpace::ControlType>()->values;
63 si->getStateSpace()->copyState(result, state);
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)
79 if (!si->satisfiesBounds(result))
90 return si->satisfiesBounds(state) && (se2->getX() < -80. || se2->getY() > 80.);
94 int main(
int ,
char** )
97 auto SE2(std::make_shared<ob::SE2StateSpace>());
98 auto velocity(std::make_shared<ob::RealVectorStateSpace>(1));
100 auto gear(std::make_shared<ob::DiscreteStateSpace>(-1,3));
107 SE2->setBounds(bounds);
111 velocityBound.setLow(0);
112 velocityBound.setHigh(60);
113 velocity->setBounds(velocityBound);
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.;
132 oc::ControlSpacePtr cmanifold(std::make_shared<oc::RealVectorControlSpace>(stateSpace, 2));
137 cbounds.setLow(0, -1.);
138 cbounds.setHigh(0, 1.);
140 cbounds.setLow(1, -20.);
141 cbounds.setHigh(1, 20.);
146 setup.setStartAndGoalStates(start, goal, 5.);
147 setup.setStateValidityChecker([si](
const ob::State *state)
149 return isStateValid(si, state);
152 const double duration,
ob::State *result)
154 propagate(si, state, control, duration, result);
156 setup.getSpaceInformation()->setPropagationStepSize(.1);
157 setup.getSpaceInformation()->setMinMaxControlDuration(2, 3);
168 for(
unsigned int i=0; i<path.getStateCount(); ++i)
170 const ob::State* state = path.getState(i);
173 const auto *velocity =
177 std::cout << se2->getX() <<
' ' << se2->getY() <<
' ' << se2->getYaw()
178 <<
' ' << velocity->values[0] <<
' ' << gear->value <<
' ';
181 std::cout <<
"0 0 0";
186 path.getControl(i-1)->as<oc::RealVectorControlSpace::ControlType>()->values;
187 std::cout << u[0] <<
' ' << u[1] <<
' ' << path.getControlDuration(i-1);
189 std::cout << std::endl;
191 if (!setup.haveExactSolutionPath())
193 std::cout <<
"Solution is approximate. Distance to actual goal is " <<
194 setup.getProblemDefinition()->getSolutionDifference() << std::endl;
const T * as() const
Cast this instance to a desired type.
Definition of a compound state.
Definition of a scoped state.
Definition of an abstract control.
This namespace contains sampling based planning routines used by planning under differential constrai...
A shared pointer wrapper for ompl::base::StateSpace.
Create the set of classes typically needed to solve a control problem.
Definition of a control path.
A shared pointer wrapper for ompl::control::ControlSpace.
const T * as() const
Cast this instance to a desired type.
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
A control space representing Rn.
ompl::base::State StateType
Define the type of state allocated by this space.
std::chrono::system_clock::duration duration
Representation of a time duration.
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.
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.