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