00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <ompl/base/goals/GoalState.h>
00038 #include <ompl/base/spaces/SE2StateSpace.h>
00039 #include <ompl/base/spaces/DiscreteStateSpace.h>
00040 #include <ompl/control/spaces/RealVectorControlSpace.h>
00041 #include <ompl/control/SimpleSetup.h>
00042 #include <ompl/config.h>
00043 #include <iostream>
00044 #include <limits>
00045 #include <boost/math/constants/constants.hpp>
00046
00047 namespace ob = ompl::base;
00048 namespace oc = ompl::control;
00049
00050 void propagate(const oc::SpaceInformation *si, const ob::State *state,
00051 const oc::Control* control, const double duration, ob::State *result)
00052 {
00053 static double timeStep = .01;
00054 static double carLength = 20.;
00055 int nsteps = ceil(duration / timeStep);
00056 double dt = duration / nsteps;
00057 const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;
00058
00059 ob::CompoundStateSpace::StateType& s = *result->as<ob::CompoundStateSpace::StateType>();
00060 ob::SE2StateSpace::StateType& se2 = *s.as<ob::SE2StateSpace::StateType>(0);
00061 ob::RealVectorStateSpace::StateType& velocity = *s.as<ob::RealVectorStateSpace::StateType>(1);
00062 ob::DiscreteStateSpace::StateType& gear = *s.as<ob::DiscreteStateSpace::StateType>(2);
00063
00064 si->getStateSpace()->copyState(result, state);
00065 for(int i=0; i<nsteps; i++)
00066 {
00067 se2.setX(se2.getX() + timeStep * velocity.values[0] * cos(se2.getYaw()));
00068 se2.setY(se2.getY() + timeStep * velocity.values[0] * sin(se2.getYaw()));
00069 se2.setYaw(se2.getYaw() + timeStep * u[0]);
00070 velocity.values[0] = velocity.values[0] + timeStep * (u[1]*gear.value);
00071
00072
00073 if (gear.value > 0)
00074 {
00075 if (gear.value < 3 && velocity.values[0] > 10*(gear.value + 1))
00076 gear.value++;
00077 else if (gear.value > 1 && velocity.values[0] < 10*gear.value)
00078 gear.value--;
00079 }
00080 if (!si->satisfiesBounds(result))
00081 return;
00082 }
00083 }
00084
00085
00086
00087 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
00088 {
00089 const ob::SE2StateSpace::StateType *se2 =
00090 state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
00091 return si->satisfiesBounds(state) && (se2->getX() < -80. || se2->getY() > 80.);
00092 }
00093
00094
00095 int main(int argc, char* argv[])
00096 {
00097
00098 ob::StateSpacePtr SE2(new ob::SE2StateSpace());
00099 ob::StateSpacePtr velocity(new ob::RealVectorStateSpace(1));
00100
00101 ob::StateSpacePtr gear(new ob::DiscreteStateSpace(-1,3));
00102 ob::StateSpacePtr stateSpace = SE2 + velocity + gear;
00103
00104
00105 ob::RealVectorBounds bounds(2);
00106 bounds.setLow(-100);
00107 bounds.setHigh(100);
00108 SE2->as<ob::SE2StateSpace>()->setBounds(bounds);
00109
00110
00111 ob::RealVectorBounds velocityBound(1);
00112 velocityBound.setLow(0);
00113 velocityBound.setHigh(60);
00114 velocity->as<ob::RealVectorStateSpace>()->setBounds(velocityBound);
00115
00116
00117 ob::ScopedState<> start(stateSpace);
00118 ob::ScopedState<> goal(stateSpace);
00119
00120
00121
00122
00123 start[0] = start[1] = -90.;
00124 start[2] = boost::math::constants::pi<double>()/2;
00125 start[3] = 40.;
00126 start->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3;
00127
00128 goal[0] = goal[1] = 90.;
00129 goal[2] = 0.;
00130 goal[3] = 40.;
00131 goal->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3;
00132
00133 oc::ControlSpacePtr cmanifold(new oc::RealVectorControlSpace(stateSpace, 2));
00134
00135
00136 ob::RealVectorBounds cbounds(2);
00137
00138 cbounds.setLow(0, -1.);
00139 cbounds.setHigh(0, 1.);
00140
00141 cbounds.setLow(1, -20.);
00142 cbounds.setHigh(1, 20.);
00143 cmanifold->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
00144
00145 oc::SimpleSetup setup(cmanifold);
00146 setup.setStartAndGoalStates(start, goal, 5.);
00147 setup.setStateValidityChecker(boost::bind(
00148 &isStateValid, setup.getSpaceInformation().get(), _1));
00149 setup.setStatePropagator(boost::bind(
00150 &propagate, setup.getSpaceInformation().get(), _1, _2, _3, _4));
00151 setup.getSpaceInformation()->setPropagationStepSize(.1);
00152 setup.getSpaceInformation()->setMinMaxControlDuration(2, 3);
00153
00154
00155 if (setup.solve(30))
00156 {
00157
00158
00159 oc::PathControl& path(setup.getSolutionPath());
00160
00161
00162
00163 for(unsigned int i=0; i<path.getStateCount(); ++i)
00164 {
00165 const ob::State* state = path.getState(i);
00166 const ob::SE2StateSpace::StateType *se2 =
00167 state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
00168 const ob::RealVectorStateSpace::StateType *velocity =
00169 state->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(1);
00170 const ob::DiscreteStateSpace::StateType *gear =
00171 state->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2);
00172 std::cout << se2->getX() << ' ' << se2->getY() << ' ' << se2->getYaw()
00173 << ' ' << velocity->values[0] << ' ' << gear->value << ' ';
00174 if (i==0)
00175
00176 std::cout << "0 0 0";
00177 else
00178 {
00179
00180 const double* u =
00181 path.getControl(i-1)->as<oc::RealVectorControlSpace::ControlType>()->values;
00182 std::cout << u[0] << ' ' << u[1] << ' ' << path.getControlDuration(i-1);
00183 }
00184 std::cout << std::endl;
00185 }
00186 if (!setup.haveExactSolutionPath())
00187 {
00188 std::cout << "Solution is approximate. Distance to actual goal is " <<
00189 setup.getProblemDefinition()->getSolutionDifference() << std::endl;
00190 }
00191 }
00192
00193 return 0;
00194 }