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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines