demos/RigidBodyPlanningWithIntegrationAndControls.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: Ioan Sucan */
00036 
00037 #include <ompl/control/SpaceInformation.h>
00038 #include <ompl/base/goals/GoalState.h>
00039 #include <ompl/base/spaces/SE2StateSpace.h>
00040 #include <ompl/control/spaces/RealVectorControlSpace.h>
00041 #include <ompl/control/planners/kpiece/KPIECE1.h>
00042 #include <ompl/control/planners/rrt/RRT.h>
00043 #include <ompl/control/SimpleSetup.h>
00044 #include <ompl/config.h>
00045 #include <iostream>
00046 #include <valarray>
00047 #include <limits>
00048 
00049 namespace ob = ompl::base;
00050 namespace oc = ompl::control;
00051 
00052 
00054 class KinematicCarModel
00055 {
00056 public:
00057 
00058     KinematicCarModel(const ob::StateSpace *space) : space_(space), carLength_(0.2)
00059     {
00060     }
00061 
00063     void operator()(const ob::State *state, const oc::Control *control, std::valarray<double> &dstate) const
00064     {
00065         const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;
00066         const double theta = state->as<ob::SE2StateSpace::StateType>()->getYaw();
00067 
00068         dstate.resize(3);
00069         dstate[0] = u[0] * cos(theta);
00070         dstate[1] = u[0] * sin(theta);
00071         dstate[2] = u[0] * tan(u[1]) / carLength_;
00072     }
00073 
00075     void update(ob::State *state, const std::valarray<double> &dstate) const
00076     {
00077         ob::SE2StateSpace::StateType &s = *state->as<ob::SE2StateSpace::StateType>();
00078         s.setX(s.getX() + dstate[0]);
00079         s.setY(s.getY() + dstate[1]);
00080         s.setYaw(s.getYaw() + dstate[2]);
00081         space_->enforceBounds(state);
00082     }
00083 
00084 private:
00085 
00086     const ob::StateSpace *space_;
00087     const double          carLength_;
00088 
00089 };
00090 
00091 
00093 template<typename F>
00094 class EulerIntegrator
00095 {
00096 public:
00097 
00098     EulerIntegrator(const ob::StateSpace *space, double timeStep) : space_(space), timeStep_(timeStep), ode_(space)
00099     {
00100     }
00101 
00102     void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result) const
00103     {
00104         double t = timeStep_;
00105         std::valarray<double> dstate;
00106         space_->copyState(result, start);
00107         while (t < duration + std::numeric_limits<double>::epsilon())
00108         {
00109             ode_(result, control, dstate);
00110             ode_.update(result, timeStep_ * dstate);
00111             t += timeStep_;
00112         }
00113         if (t + std::numeric_limits<double>::epsilon() > duration)
00114         {
00115             ode_(result, control, dstate);
00116             ode_.update(result, (t - duration) * dstate);
00117         }
00118     }
00119 
00120     double getTimeStep(void) const
00121     {
00122         return timeStep_;
00123     }
00124 
00125     void setTimeStep(double timeStep)
00126     {
00127         timeStep_ = timeStep;
00128     }
00129 
00130 private:
00131 
00132     const ob::StateSpace *space_;
00133     double                   timeStep_;
00134     F                        ode_;
00135 };
00136 
00137 
00138 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
00139 {
00140     //    ob::ScopedState<ob::SE2StateSpace>
00142     const ob::SE2StateSpace::StateType *se2state = state->as<ob::SE2StateSpace::StateType>();
00143 
00145     const ob::RealVectorStateSpace::StateType *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
00146 
00148     const ob::SO2StateSpace::StateType *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
00149 
00151 
00152 
00153     // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
00154     return si->satisfiesBounds(state) && (const void*)rot != (const void*)pos;
00155 }
00156 
00158 class DemoControlSpace : public oc::RealVectorControlSpace
00159 {
00160 public:
00161 
00162     DemoControlSpace(const ob::StateSpacePtr &stateSpace) : oc::RealVectorControlSpace(stateSpace, 2)
00163     {
00164     }
00165 };
00166 
00167 class DemoStatePropagator : public oc::StatePropagator
00168 {
00169 public:
00170 
00171     DemoStatePropagator(const oc::SpaceInformationPtr &si) : oc::StatePropagator(si),
00172                                                              integrator_(si->getStateSpace().get(), 0.0)
00173     {
00174     }
00175 
00176     virtual void propagate(const ob::State *state, const oc::Control* control, const double duration, ob::State *result) const
00177     {
00178         integrator_.propagate(state, control, duration, result);
00179     }
00180 
00181     void setIntegrationTimeStep(double timeStep)
00182     {
00183         integrator_.setTimeStep(timeStep);
00184     }
00185 
00186     double getIntegrationTimeStep(void) const
00187     {
00188         return integrator_.getTimeStep();
00189     }
00190 
00191     EulerIntegrator<KinematicCarModel> integrator_;
00192 };
00193 
00195 
00196 void planWithSimpleSetup(void)
00197 {
00199     ob::StateSpacePtr space(new ob::SE2StateSpace());
00200 
00202     ob::RealVectorBounds bounds(2);
00203     bounds.setLow(-1);
00204     bounds.setHigh(1);
00205 
00206     space->as<ob::SE2StateSpace>()->setBounds(bounds);
00207 
00208     // create a control space
00209     oc::ControlSpacePtr cspace(new DemoControlSpace(space));
00210 
00211     // set the bounds for the control space
00212     ob::RealVectorBounds cbounds(2);
00213     cbounds.setLow(-0.3);
00214     cbounds.setHigh(0.3);
00215 
00216     cspace->as<DemoControlSpace>()->setBounds(cbounds);
00217 
00218     // define a simple setup class
00219     oc::SimpleSetup ss(cspace);
00220 
00222     ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
00223 
00225     ss.setStatePropagator(oc::StatePropagatorPtr(new DemoStatePropagator(ss.getSpaceInformation())));
00226 
00228     ob::ScopedState<ob::SE2StateSpace> start(space);
00229     start->setX(-0.5);
00230     start->setY(0.0);
00231     start->setYaw(0.0);
00232 
00234     ob::ScopedState<ob::SE2StateSpace> goal(space);
00235     goal->setX(0.0);
00236     goal->setY(0.5);
00237     goal->setYaw(0.0);
00238 
00240     ss.setStartAndGoalStates(start, goal, 0.05);
00241 
00243     ss.setup();
00244     static_cast<DemoStatePropagator*>(ss.getStatePropagator().get())->setIntegrationTimeStep(ss.getSpaceInformation()->getPropagationStepSize());
00245 
00247     ob::PlannerStatus solved = ss.solve(10.0);
00248 
00249     if (solved)
00250     {
00251         std::cout << "Found solution:" << std::endl;
00253 
00254         ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
00255     }
00256     else
00257         std::cout << "No solution found" << std::endl;
00258 }
00259 
00260 int main(int, char **)
00261 {
00262     std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
00263 
00264     planWithSimpleSetup();
00265 
00266     return 0;
00267 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines