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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines