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 }