demos/RigidBodyPlanningWithODESolverAndControls.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, 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: Ryan Luna */
00036 
00037 #include <ompl/control/SpaceInformation.h>
00038 #include <ompl/base/spaces/SE2StateSpace.h>
00039 #include <ompl/control/ODESolver.h>
00040 #include <ompl/control/spaces/RealVectorControlSpace.h>
00041 #include <ompl/control/SimpleSetup.h>
00042 #include <ompl/config.h>
00043 #include <iostream>
00044 #include <valarray>
00045 #include <limits>
00046 
00047 namespace ob = ompl::base;
00048 namespace oc = ompl::control;
00049 
00050 // Kinematic car model object definition.  This class does NOT use ODESolver to propagate the system.
00051 class KinematicCarModel : public oc::StatePropagator
00052 {
00053     public:
00054         KinematicCarModel(const oc::SpaceInformationPtr &si) : oc::StatePropagator(si)
00055         {
00056             space_     = si->getStateSpace();
00057             carLength_ = 0.2;
00058             timeStep_  = 0.01;
00059         }
00060 
00061         virtual void propagate(const ob::State *state, const oc::Control* control, const double duration, ob::State *result) const
00062         {
00063             EulerIntegration(state, control, duration, result);
00064         }
00065 
00066     protected:
00067         // Explicit Euler Method for numerical integration.
00068         void EulerIntegration(const ob::State *start, const oc::Control *control, const double duration, ob::State *result) const
00069         {
00070             double t = timeStep_;
00071             std::valarray<double> dstate;
00072             space_->copyState(result, start);
00073             while (t < duration + std::numeric_limits<double>::epsilon())
00074             {
00075                 ode(result, control, dstate);
00076                 update(result, timeStep_ * dstate);
00077                 t += timeStep_;
00078             }
00079             if (t + std::numeric_limits<double>::epsilon() > duration)
00080             {
00081                 ode(result, control, dstate);
00082                 update(result, (t - duration) * dstate);
00083             }
00084         }
00085 
00087         void ode(const ob::State *state, const oc::Control *control, std::valarray<double> &dstate) const
00088         {
00089             const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;
00090             const double theta = state->as<ob::SE2StateSpace::StateType>()->getYaw();
00091 
00092             dstate.resize(3);
00093             dstate[0] = u[0] * cos(theta);
00094             dstate[1] = u[0] * sin(theta);
00095             dstate[2] = u[0] * tan(u[1]) / carLength_;
00096         }
00097 
00099         void update(ob::State *state, const std::valarray<double> &dstate) const
00100         {
00101             ob::SE2StateSpace::StateType &s = *state->as<ob::SE2StateSpace::StateType>();
00102             s.setX(s.getX() + dstate[0]);
00103             s.setY(s.getY() + dstate[1]);
00104             s.setYaw(s.getYaw() + dstate[2]);
00105             space_->enforceBounds(state);
00106         }
00107 
00108         ob::StateSpacePtr        space_;
00109         double                   carLength_;
00110         double                   timeStep_;
00111 };
00112 
00113 // Definition of the ODE for the kinematic car.
00114 // This method is analogous to the above KinematicCarModel::ode function.
00115 void KinematicCarODE (const oc::ODESolver::StateType& q, const oc::Control* control, oc::ODESolver::StateType& qdot)
00116 {
00117     const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;
00118     const double theta = q[2];
00119     double carLength = 0.2;
00120 
00121     // Zero out qdot
00122     qdot.resize (q.size (), 0);
00123 
00124     qdot[0] = u[0] * cos(theta);
00125     qdot[1] = u[0] * sin(theta);
00126     qdot[2] = u[0] * tan(u[1]) / carLength;
00127 }
00128 
00129 // This is a callback method invoked after numerical integration.
00130 void KinematicCarPostIntegration (const ob::State* /*state*/, const oc::Control* /*control*/, const double /*duration*/, ob::State *result)
00131 {
00132     // Normalize orientation between 0 and 2*pi
00133     ob::SO2StateSpace SO2;
00134     SO2.enforceBounds (result->as<ob::SE2StateSpace::StateType>()->as<ob::SO2StateSpace::StateType>(1));
00135 }
00136 
00137 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
00138 {
00139     //    ob::ScopedState<ob::SE2StateSpace>
00141     const ob::SE2StateSpace::StateType *se2state = state->as<ob::SE2StateSpace::StateType>();
00142 
00144     const ob::RealVectorStateSpace::StateType *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
00145 
00147     const ob::SO2StateSpace::StateType *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
00148 
00150 
00151 
00152     // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
00153     return si->satisfiesBounds(state) && (const void*)rot != (const void*)pos;
00154 }
00155 
00157 class DemoControlSpace : public oc::RealVectorControlSpace
00158 {
00159 public:
00160 
00161     DemoControlSpace(const ob::StateSpacePtr &stateSpace) : oc::RealVectorControlSpace(stateSpace, 2)
00162     {
00163     }
00164 };
00166 
00167 void planWithSimpleSetup(void)
00168 {
00170     ob::StateSpacePtr space(new ob::SE2StateSpace());
00171 
00173     ob::RealVectorBounds bounds(2);
00174     bounds.setLow(-1);
00175     bounds.setHigh(1);
00176 
00177     space->as<ob::SE2StateSpace>()->setBounds(bounds);
00178 
00179     // create a control space
00180     oc::ControlSpacePtr cspace(new DemoControlSpace(space));
00181 
00182     // set the bounds for the control space
00183     ob::RealVectorBounds cbounds(2);
00184     cbounds.setLow(-0.3);
00185     cbounds.setHigh(0.3);
00186 
00187     cspace->as<DemoControlSpace>()->setBounds(cbounds);
00188 
00189     // define a simple setup class
00190     oc::SimpleSetup ss(cspace);
00191 
00192     // set state validity checking for this space
00193     ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
00194 
00195     // Setting the propagation routine for this space:
00196     // KinematicCarModel does NOT use ODESolver
00197     //ss.setStatePropagator(oc::StatePropagatorPtr(new KinematicCarModel(ss.getSpaceInformation())));
00198 
00199     // Use the ODESolver to propagate the system.  Call KinematicCarPostIntegration
00200     // when integration has finished to normalize the orientation values.
00201     oc::ODESolverPtr odeSolver(new oc::ODEBasicSolver<> (ss.getSpaceInformation(), &KinematicCarODE));
00202     ss.setStatePropagator(oc::ODESolver::getStatePropagator(odeSolver, &KinematicCarPostIntegration));
00203 
00205     ob::ScopedState<ob::SE2StateSpace> start(space);
00206     start->setX(-0.5);
00207     start->setY(0.0);
00208     start->setYaw(0.0);
00209 
00211     ob::ScopedState<ob::SE2StateSpace> goal(space);
00212     goal->setX(0.0);
00213     goal->setY(0.5);
00214     goal->setYaw(0.0);
00215 
00217     ss.setStartAndGoalStates(start, goal, 0.05);
00218 
00220     ss.setup();
00221 
00223     ob::PlannerStatus solved = ss.solve(10.0);
00224 
00225     if (solved)
00226     {
00227         std::cout << "Found solution:" << std::endl;
00229 
00230         ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
00231     }
00232     else
00233         std::cout << "No solution found" << std::endl;
00234 }
00235 
00236 int main(int, char **)
00237 {
00238     std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
00239 
00240     planWithSimpleSetup();
00241 
00242     return 0;
00243 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines