demos/GeometricCarPlanning.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: Mark Moll */ 00036 00037 #include <ompl/base/spaces/DubinsStateSpace.h> 00038 #include <ompl/base/spaces/ReedsSheppStateSpace.h> 00039 #include <ompl/base/ScopedState.h> 00040 #include <ompl/geometric/SimpleSetup.h> 00041 #include <boost/program_options.hpp> 00042 00043 namespace ob = ompl::base; 00044 namespace og = ompl::geometric; 00045 namespace po = boost::program_options; 00046 00047 // The easy problem is the standard narrow passage problem: two big open 00048 // spaces connected by a narrow passage. The hard problem is essentially 00049 // one long narrow passage with the robot facing towards the long walls 00050 // in both the start and goal configurations. 00051 00052 bool isStateValidEasy(const ob::SpaceInformation *si, const ob::State *state) 00053 { 00054 const ob::SE2StateSpace::StateType *s = state->as<ob::SE2StateSpace::StateType>(); 00055 double x=s->getX(), y=s->getY(); 00056 return si->satisfiesBounds(s) && (x<5 || x>13 || (y>8.5 && y<9.5)); 00057 } 00058 00059 bool isStateValidHard(const ob::SpaceInformation *si, const ob::State *state) 00060 { 00061 return si->satisfiesBounds(state); 00062 } 00063 00064 void plan(ob::StateSpacePtr space, bool easy) 00065 { 00066 ob::ScopedState<> start(space), goal(space); 00067 ob::RealVectorBounds bounds(2); 00068 bounds.setLow(0); 00069 if (easy) 00070 bounds.setHigh(18); 00071 else 00072 { 00073 bounds.high[0] = 6; 00074 bounds.high[1] = .6; 00075 } 00076 space->as<ob::SE2StateSpace>()->setBounds(bounds); 00077 00078 // define a simple setup class 00079 og::SimpleSetup ss(space); 00080 00081 // set state validity checking for this space 00082 ob::SpaceInformationPtr si(ss.getSpaceInformation()); 00083 ss.setStateValidityChecker(boost::bind( 00084 easy ? &isStateValidEasy : &isStateValidHard, si.get(), _1)); 00085 00086 // set the start and goal states 00087 if (easy) 00088 { 00089 start[0] = start[1] = 1.; start[2] = 0.; 00090 goal[0] = goal[1] = 17; goal[2] = -.99*boost::math::constants::pi<double>(); 00091 } 00092 else 00093 { 00094 start[0] = start[1] = .5; start[2] = .5*boost::math::constants::pi<double>();; 00095 goal[0] = 5.5; goal[1] = .5; goal[2] = .5*boost::math::constants::pi<double>(); 00096 } 00097 ss.setStartAndGoalStates(start, goal); 00098 00099 // this call is optional, but we put it in to get more output information 00100 ss.getSpaceInformation()->setStateValidityCheckingResolution(0.005); 00101 ss.setup(); 00102 ss.print(); 00103 00104 // attempt to solve the problem within 30 seconds of planning time 00105 ob::PlannerStatus solved = ss.solve(30.0); 00106 00107 if (solved) 00108 { 00109 std::vector<double> reals; 00110 00111 std::cout << "Found solution:" << std::endl; 00112 ss.simplifySolution(); 00113 og::PathGeometric path = ss.getSolutionPath(); 00114 path.interpolate(1000); 00115 path.printAsMatrix(std::cout); 00116 } 00117 else 00118 std::cout << "No solution found" << std::endl; 00119 } 00120 00121 void printTrajectory(ob::StateSpacePtr space, const std::vector<double>& pt) 00122 { 00123 if (pt.size()!=3) throw ompl::Exception("3 arguments required for trajectory option"); 00124 const unsigned int num_pts = 50; 00125 ob::ScopedState<> from(space), to(space), s(space); 00126 std::vector<double> reals; 00127 00128 from[0] = from[1] = from[2] = 0.; 00129 00130 to[0] = pt[0]; 00131 to[1] = pt[1]; 00132 to[2] = pt[2]; 00133 00134 std::cout << "distance: " << space->distance(from(), to()) << "\npath:\n"; 00135 for (unsigned int i=0; i<=num_pts; ++i) 00136 { 00137 space->interpolate(from(), to(), (double)i/num_pts, s()); 00138 reals = s.reals(); 00139 std::cout << "path " << reals[0] << ' ' << reals[1] << ' ' << reals[2] << ' ' << std::endl; 00140 } 00141 } 00142 00143 void printDistanceGrid(ob::StateSpacePtr space) 00144 { 00145 // print the distance for (x,y,theta) for all points in a 3D grid in SE(2) 00146 // over [-5,5) x [-5, 5) x [-pi,pi). 00147 // 00148 // The output should be redirected to a file, say, distance.txt. This 00149 // can then be read and plotted in Matlab like so: 00150 // x = reshape(load('distance.txt'),200,200,200); 00151 // for i=1:200, 00152 // contourf(squeeze(x(i,:,:)),30); 00153 // axis equal; axis tight; colorbar; pause; 00154 // end; 00155 const unsigned int num_pts = 200; 00156 ob::ScopedState<> from(space), to(space); 00157 from[0] = from[1] = from[2] = 0.; 00158 00159 for (unsigned int i=0; i<num_pts; ++i) 00160 for (unsigned int j=0; j<num_pts; ++j) 00161 for (unsigned int k=0; k<num_pts; ++k) 00162 { 00163 to[0] = 5. * (2. * (double)i/num_pts - 1.); 00164 to[1] = 5. * (2. * (double)j/num_pts - 1.); 00165 to[2] = boost::math::constants::pi<double>() * (2. * (double)k/num_pts - 1.); 00166 std::cout << space->distance(from(), to()) << '\n'; 00167 } 00168 00169 } 00170 00171 int main(int argc, char* argv[]) 00172 { 00173 try 00174 { 00175 po::options_description desc("Options"); 00176 desc.add_options() 00177 ("help", "show help message") 00178 ("dubins", "use Dubins state space") 00179 ("dubinssym", "use symmetrized Dubins state space") 00180 ("reedsshepp", "use Reeds-Shepp state space (default)") 00181 ("easyplan", "solve easy planning problem and print path") 00182 ("hardplan", "solve hard planning problem and print path") 00183 ("trajectory", po::value<std::vector<double > >()->multitoken(), 00184 "print trajectory from (0,0,0) to a user-specified x, y, and theta") 00185 ("distance", "print distance grid") 00186 ; 00187 00188 po::variables_map vm; 00189 po::store(po::parse_command_line(argc, argv, desc, 00190 po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm); 00191 po::notify(vm); 00192 00193 if (vm.count("help") || argc==1) 00194 { 00195 std::cout << desc << "\n"; 00196 return 1; 00197 } 00198 00199 ob::StateSpacePtr space(new ob::ReedsSheppStateSpace); 00200 00201 if (vm.count("dubins")) 00202 space = ob::StateSpacePtr(new ob::DubinsStateSpace); 00203 if (vm.count("dubinssym")) 00204 space = ob::StateSpacePtr(new ob::DubinsStateSpace(1., true)); 00205 if (vm.count("easyplan")) 00206 plan(space, true); 00207 if (vm.count("hardplan")) 00208 plan(space, false); 00209 if (vm.count("trajectory")) 00210 printTrajectory(space, vm["trajectory"].as<std::vector<double> >()); 00211 if (vm.count("distance")) 00212 printDistanceGrid(space); 00213 } 00214 catch(std::exception& e) { 00215 std::cerr << "error: " << e.what() << "\n"; 00216 return 1; 00217 } 00218 catch(...) { 00219 std::cerr << "Exception of unknown type!\n"; 00220 } 00221 00222 return 0; 00223 }