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