demos/RigidBodyPlanningWithControls.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/planners/est/EST.h>
00044 #include <ompl/control/planners/syclop/SyclopRRT.h>
00045 #include <ompl/control/planners/syclop/SyclopEST.h>
00046 #include <ompl/control/planners/pdst/PDST.h>
00047 #include <ompl/control/planners/syclop/GridDecomposition.h>
00048 #include <ompl/control/SimpleSetup.h>
00049 #include <ompl/config.h>
00050 #include <iostream>
00051 
00052 namespace ob = ompl::base;
00053 namespace oc = ompl::control;
00054 
00055 // a decomposition is only needed for SyclopRRT and SyclopEST
00056 class MyDecomposition : public oc::GridDecomposition
00057 {
00058 public:
00059     MyDecomposition(const int length, const ob::RealVectorBounds& bounds)
00060         : GridDecomposition(length, 2, bounds)
00061     {
00062     }
00063     virtual void project(const ob::State* s, std::vector<double>& coord) const
00064     {
00065         coord.resize(2);
00066         coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
00067         coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
00068     }
00069 
00070     virtual void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const
00071     {
00072         sampler->sampleUniform(s);
00073         s->as<ob::SE2StateSpace::StateType>()->setXY(coord[0], coord[1]);
00074     }
00075 };
00076 
00077 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
00078 {
00079     //    ob::ScopedState<ob::SE2StateSpace>
00080     // cast the abstract state type to the type we expect
00081     const ob::SE2StateSpace::StateType *se2state = state->as<ob::SE2StateSpace::StateType>();
00082 
00083     // extract the first component of the state and cast it to what we expect
00084     const ob::RealVectorStateSpace::StateType *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
00085 
00086     // extract the second component of the state and cast it to what we expect
00087     const ob::SO2StateSpace::StateType *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
00088 
00089     // check validity of state defined by pos & rot
00090 
00091 
00092     // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
00093     return si->satisfiesBounds(state) && (const void*)rot != (const void*)pos;
00094 }
00095 
00096 void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
00097 {
00098     const ob::SE2StateSpace::StateType *se2state = start->as<ob::SE2StateSpace::StateType>();
00099     const double* pos = se2state->as<ob::RealVectorStateSpace::StateType>(0)->values;
00100     const double rot = se2state->as<ob::SO2StateSpace::StateType>(1)->value;
00101     const double* ctrl = control->as<oc::RealVectorControlSpace::ControlType>()->values;
00102 
00103     result->as<ob::SE2StateSpace::StateType>()->setXY(
00104         pos[0] + ctrl[0] * duration * cos(rot),
00105         pos[1] + ctrl[0] * duration * sin(rot));
00106     result->as<ob::SE2StateSpace::StateType>()->setYaw(
00107         rot    + ctrl[1] * duration);
00108 }
00109 
00110 void plan(void)
00111 {
00112 
00113     // construct the state space we are planning in
00114     ob::StateSpacePtr space(new ob::SE2StateSpace());
00115 
00116     // set the bounds for the R^2 part of SE(2)
00117     ob::RealVectorBounds bounds(2);
00118     bounds.setLow(-1);
00119     bounds.setHigh(1);
00120 
00121     space->as<ob::SE2StateSpace>()->setBounds(bounds);
00122 
00123     // create a control space
00124     oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
00125 
00126     // set the bounds for the control space
00127     ob::RealVectorBounds cbounds(2);
00128     cbounds.setLow(-0.3);
00129     cbounds.setHigh(0.3);
00130 
00131     cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
00132 
00133     // construct an instance of  space information from this control space
00134     oc::SpaceInformationPtr si(new oc::SpaceInformation(space, cspace));
00135 
00136     // set state validity checking for this space
00137     si->setStateValidityChecker(boost::bind(&isStateValid, si.get(),  _1));
00138 
00139     // set the state propagation routine
00140     si->setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
00141 
00142     // create a start state
00143     ob::ScopedState<ob::SE2StateSpace> start(space);
00144     start->setX(-0.5);
00145     start->setY(0.0);
00146     start->setYaw(0.0);
00147 
00148     // create a goal state
00149     ob::ScopedState<ob::SE2StateSpace> goal(start);
00150     goal->setX(0.5);
00151 
00152     // create a problem instance
00153     ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
00154 
00155     // set the start and goal states
00156     pdef->setStartAndGoalStates(start, goal, 0.1);
00157 
00158     // create a planner for the defined space
00159     //ob::PlannerPtr planner(new oc::RRT(si));
00160     //ob::PlannerPtr planner(new oc::EST(si));
00161     //ob::PlannerPtr planner(new oc::KPIECE1(si));
00162     oc::DecompositionPtr decomp(new MyDecomposition(32, bounds));
00163     ob::PlannerPtr planner(new oc::SyclopEST(si, decomp));
00164     //ob::PlannerPtr planner(new oc::SyclopRRT(si, decomp));
00165 
00166     // set the problem we are trying to solve for the planner
00167     planner->setProblemDefinition(pdef);
00168 
00169     // perform setup steps for the planner
00170     planner->setup();
00171 
00172 
00173     // print the settings for this space
00174     si->printSettings(std::cout);
00175 
00176     // print the problem settings
00177     pdef->print(std::cout);
00178 
00179     // attempt to solve the problem within one second of planning time
00180     ob::PlannerStatus solved = planner->solve(10.0);
00181 
00182     if (solved)
00183     {
00184         // get the goal representation from the problem definition (not the same as the goal state)
00185         // and inquire about the found path
00186         ob::PathPtr path = pdef->getSolutionPath();
00187         std::cout << "Found solution:" << std::endl;
00188 
00189         // print the path to screen
00190         path->print(std::cout);
00191     }
00192     else
00193         std::cout << "No solution found" << std::endl;
00194 }
00195 
00196 
00197 void planWithSimpleSetup(void)
00198 {
00199     // construct the state space we are planning in
00200     ob::StateSpacePtr space(new ob::SE2StateSpace());
00201 
00202     // set the bounds for the R^2 part of SE(2)
00203     ob::RealVectorBounds bounds(2);
00204     bounds.setLow(-1);
00205     bounds.setHigh(1);
00206 
00207     space->as<ob::SE2StateSpace>()->setBounds(bounds);
00208 
00209     // create a control space
00210     oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
00211 
00212     // set the bounds for the control space
00213     ob::RealVectorBounds cbounds(2);
00214     cbounds.setLow(-0.3);
00215     cbounds.setHigh(0.3);
00216 
00217     cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
00218 
00219     // define a simple setup class
00220     oc::SimpleSetup ss(cspace);
00221 
00222     // set the state propagation routine
00223     ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
00224 
00225     // set state validity checking for this space
00226     ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
00227 
00228     // create a start state
00229     ob::ScopedState<ob::SE2StateSpace> start(space);
00230     start->setX(-0.5);
00231     start->setY(0.0);
00232     start->setYaw(0.0);
00233 
00234     // create a  goal state; use the hard way to set the elements
00235     ob::ScopedState<ob::SE2StateSpace> goal(space);
00236     (*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
00237     (*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.5;
00238     (*goal)[1]->as<ob::SO2StateSpace::StateType>()->value = 0.0;
00239 
00240 
00241     // set the start and goal states
00242     ss.setStartAndGoalStates(start, goal, 0.05);
00243 
00244     // ss.setPlanner(ob::PlannerPtr(new oc::PDST(ss.getSpaceInformation())));
00245     // ss.getSpaceInformation()->setMinMaxControlDuration(1,100);
00246     // attempt to solve the problem within one second of planning time
00247     ob::PlannerStatus solved = ss.solve(10.0);
00248 
00249     if (solved)
00250     {
00251         std::cout << "Found solution:" << std::endl;
00252         // print the path to screen
00253 
00254         ss.getSolutionPath().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     // plan();
00265     //
00266     // std::cout << std::endl << std::endl;
00267     //
00268     planWithSimpleSetup();
00269 
00270     return 0;
00271 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines