demos/HybridSystemPlanning.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Elizabeth Fudge */ 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 int nsteps = ceil(duration / timeStep); 00055 double dt = duration / nsteps; 00056 const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values; 00057 00058 ob::CompoundStateSpace::StateType& s = *result->as<ob::CompoundStateSpace::StateType>(); 00059 ob::SE2StateSpace::StateType& se2 = *s.as<ob::SE2StateSpace::StateType>(0); 00060 ob::RealVectorStateSpace::StateType& velocity = *s.as<ob::RealVectorStateSpace::StateType>(1); 00061 ob::DiscreteStateSpace::StateType& gear = *s.as<ob::DiscreteStateSpace::StateType>(2); 00062 00063 si->getStateSpace()->copyState(result, state); 00064 for(int i=0; i<nsteps; i++) 00065 { 00066 se2.setX(se2.getX() + dt * velocity.values[0] * cos(se2.getYaw())); 00067 se2.setY(se2.getY() + dt * velocity.values[0] * sin(se2.getYaw())); 00068 se2.setYaw(se2.getYaw() + dt * u[0]); 00069 velocity.values[0] = velocity.values[0] + dt * (u[1]*gear.value); 00070 00071 // 'guards' - conditions to change gears 00072 if (gear.value > 0) 00073 { 00074 if (gear.value < 3 && velocity.values[0] > 10*(gear.value + 1)) 00075 gear.value++; 00076 else if (gear.value > 1 && velocity.values[0] < 10*gear.value) 00077 gear.value--; 00078 } 00079 if (!si->satisfiesBounds(result)) 00080 return; 00081 } 00082 } 00083 00084 // The free space consists of two narrow corridors connected at right angle. 00085 // To make the turn, the car will have to downshift. 00086 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state) 00087 { 00088 const ob::SE2StateSpace::StateType *se2 = 00089 state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0); 00090 return si->satisfiesBounds(state) && (se2->getX() < -80. || se2->getY() > 80.); 00091 } 00092 00093 00094 int main(int, char**) 00095 { 00096 // plan for hybrid car in SE(2) with discrete gears 00097 ob::StateSpacePtr SE2(new ob::SE2StateSpace()); 00098 ob::StateSpacePtr velocity(new ob::RealVectorStateSpace(1)); 00099 // set the range for gears: [-1,3] inclusive 00100 ob::StateSpacePtr gear(new ob::DiscreteStateSpace(-1,3)); 00101 ob::StateSpacePtr stateSpace = SE2 + velocity + gear; 00102 00103 // set the bounds for the R^2 part of SE(2) 00104 ob::RealVectorBounds bounds(2); 00105 bounds.setLow(-100); 00106 bounds.setHigh(100); 00107 SE2->as<ob::SE2StateSpace>()->setBounds(bounds); 00108 00109 // set the bounds for the velocity 00110 ob::RealVectorBounds velocityBound(1); 00111 velocityBound.setLow(0); 00112 velocityBound.setHigh(60); 00113 velocity->as<ob::RealVectorStateSpace>()->setBounds(velocityBound); 00114 00115 // create start and goal states 00116 ob::ScopedState<> start(stateSpace); 00117 ob::ScopedState<> goal(stateSpace); 00118 00119 // Both start and goal are states with high velocity with the car in third gear. 00120 // However, to make the turn, the car cannot stay in third gear and will have to 00121 // shift to first gear. 00122 start[0] = start[1] = -90.; // position 00123 start[2] = boost::math::constants::pi<double>()/2; // orientation 00124 start[3] = 40.; // velocity 00125 start->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear 00126 00127 goal[0] = goal[1] = 90.; // position 00128 goal[2] = 0.; // orientation 00129 goal[3] = 40.; // velocity 00130 goal->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear 00131 00132 oc::ControlSpacePtr cmanifold(new oc::RealVectorControlSpace(stateSpace, 2)); 00133 00134 // set the bounds for the control manifold 00135 ob::RealVectorBounds cbounds(2); 00136 // bounds for steering input 00137 cbounds.setLow(0, -1.); 00138 cbounds.setHigh(0, 1.); 00139 // bounds for brake/gas input 00140 cbounds.setLow(1, -20.); 00141 cbounds.setHigh(1, 20.); 00142 cmanifold->as<oc::RealVectorControlSpace>()->setBounds(cbounds); 00143 00144 oc::SimpleSetup setup(cmanifold); 00145 setup.setStartAndGoalStates(start, goal, 5.); 00146 setup.setStateValidityChecker(boost::bind( 00147 &isStateValid, setup.getSpaceInformation().get(), _1)); 00148 setup.setStatePropagator(boost::bind( 00149 &propagate, setup.getSpaceInformation().get(), _1, _2, _3, _4)); 00150 setup.getSpaceInformation()->setPropagationStepSize(.1); 00151 setup.getSpaceInformation()->setMinMaxControlDuration(2, 3); 00152 00153 // try to solve the problem 00154 if (setup.solve(30)) 00155 { 00156 // print the (approximate) solution path: print states along the path 00157 // and controls required to get from one state to the next 00158 oc::PathControl& path(setup.getSolutionPath()); 00159 00160 // print out full state on solution path 00161 // (format: x, y, theta, v, u0, u1, dt) 00162 for(unsigned int i=0; i<path.getStateCount(); ++i) 00163 { 00164 const ob::State* state = path.getState(i); 00165 const ob::SE2StateSpace::StateType *se2 = 00166 state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0); 00167 const ob::RealVectorStateSpace::StateType *velocity = 00168 state->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(1); 00169 const ob::DiscreteStateSpace::StateType *gear = 00170 state->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2); 00171 std::cout << se2->getX() << ' ' << se2->getY() << ' ' << se2->getYaw() 00172 << ' ' << velocity->values[0] << ' ' << gear->value << ' '; 00173 if (i==0) 00174 // null controls applied for zero seconds to get to start state 00175 std::cout << "0 0 0"; 00176 else 00177 { 00178 // print controls and control duration needed to get from state i-1 to state i 00179 const double* u = 00180 path.getControl(i-1)->as<oc::RealVectorControlSpace::ControlType>()->values; 00181 std::cout << u[0] << ' ' << u[1] << ' ' << path.getControlDuration(i-1); 00182 } 00183 std::cout << std::endl; 00184 } 00185 if (!setup.haveExactSolutionPath()) 00186 { 00187 std::cout << "Solution is approximate. Distance to actual goal is " << 00188 setup.getProblemDefinition()->getSolutionDifference() << std::endl; 00189 } 00190 } 00191 00192 return 0; 00193 }