All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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/GoalState.h>
00039 #include <ompl/base/manifolds/SE2StateManifold.h>
00040 #include <ompl/control/manifolds/RealVectorControlManifold.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::StateManifold *manifold) : manifold_(manifold), 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::RealVectorControlManifold::ControlType>()->values;
00066         const double theta = state->as<ob::SE2StateManifold::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::SE2StateManifold::StateType &s = *state->as<ob::SE2StateManifold::StateType>();
00078         s.setX(s.getX() + dstate[0]);
00079         s.setY(s.getY() + dstate[1]);
00080         s.setYaw(s.getYaw() + dstate[2]);
00081         manifold_->enforceBounds(state);
00082     }
00083 
00084 private:
00085 
00086     const ob::StateManifold *manifold_;
00087     const double             carLength_;
00088 
00089 };
00090 
00091 
00093 template<typename F>
00094 class EulerIntegrator
00095 {
00096 public:
00097 
00098     EulerIntegrator(const ob::StateManifold *manifold, double timeStep) : manifold_(manifold), timeStep_(timeStep), ode_(manifold)
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         manifold_->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::StateManifold *manifold_;
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::SE2StateManifold>
00142     const ob::SE2StateManifold::StateType *se2state = state->as<ob::SE2StateManifold::StateType>();
00143 
00145     const ob::RealVectorStateManifold::StateType *pos = se2state->as<ob::RealVectorStateManifold::StateType>(0);
00146 
00148     const ob::SO2StateManifold::StateType *rot = se2state->as<ob::SO2StateManifold::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) && (void*)rot != (void*)pos;
00155 }
00156 
00158 class DemoControlManifold : public oc::RealVectorControlManifold
00159 {
00160 public:
00161 
00162     DemoControlManifold(const ob::StateManifoldPtr &stateManifold) : oc::RealVectorControlManifold(stateManifold, 2),
00163                                                                      integrator_(stateManifold.get(), 0.0)
00164     {
00165     }
00166 
00167     virtual void propagate(const ob::State *state, const oc::Control* control, const double duration, ob::State *result) const
00168     {
00169         integrator_.propagate(state, control, duration, result);
00170     }
00171 
00172     void setIntegrationTimeStep(double timeStep)
00173     {
00174         integrator_.setTimeStep(timeStep);
00175     }
00176 
00177     double getIntegrationTimeStep(void) const
00178     {
00179         return integrator_.getTimeStep();
00180     }
00181 
00182     EulerIntegrator<KinematicCarModel> integrator_;
00183 
00184 };
00186 
00187 void planWithSimpleSetup(void)
00188 {
00190     ob::StateManifoldPtr manifold(new ob::SE2StateManifold());
00191 
00193     ob::RealVectorBounds bounds(2);
00194     bounds.setLow(-1);
00195     bounds.setHigh(1);
00196 
00197     manifold->as<ob::SE2StateManifold>()->setBounds(bounds);
00198 
00199     // create a control manifold
00200     oc::ControlManifoldPtr cmanifold(new DemoControlManifold(manifold));
00201 
00202     // set the bounds for the control manifold
00203     ob::RealVectorBounds cbounds(2);
00204     cbounds.setLow(-0.3);
00205     cbounds.setHigh(0.3);
00206 
00207     cmanifold->as<DemoControlManifold>()->setBounds(cbounds);
00208 
00209     // define a simple setup class
00210     oc::SimpleSetup ss(cmanifold);
00211 
00213     ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
00214 
00216     ob::ScopedState<ob::SE2StateManifold> start(manifold);
00217     start->setX(-0.5);
00218     start->setY(0.0);
00219     start->setYaw(0.0);
00220 
00222     ob::ScopedState<ob::SE2StateManifold> goal(manifold);
00223     goal->setX(0.0);
00224     goal->setY(0.5);
00225     goal->setYaw(0.0);
00226 
00228     ss.setStartAndGoalStates(start, goal, 0.05);
00229 
00231     ss.setup();
00232     cmanifold->as<DemoControlManifold>()->setIntegrationTimeStep(ss.getSpaceInformation()->getPropagationStepSize());
00233 
00235     bool solved = ss.solve(10.0);
00236 
00237     if (solved)
00238     {
00239         std::cout << "Found solution:" << std::endl;
00241 
00242         ss.getSolutionPath().asGeometric().print(std::cout);
00243     }
00244     else
00245         std::cout << "No solution found" << std::endl;
00246 }
00247 
00248 int main(int, char **)
00249 {
00250     std::cout << "ompl version: " << OMPL_VERSION << std::endl;
00251 
00252     planWithSimpleSetup();
00253 
00254     return 0;
00255 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator