All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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         // We can't use regular simplify because it also tries to use spline interpolation,
00113         // which doesn't work for Dubins curves.
00114         //ss.simplifySolution();
00115         og::PathGeometric path = ss.getSolutionPath();
00116         og::PathSimplifierPtr ps = ss.getPathSimplifier();
00117         ps->reduceVertices(path);
00118         ps->collapseCloseVertices(path);
00119         path.interpolate(1000);
00120         for (unsigned int i=0; i < path.getStateCount(); ++i)
00121         {
00122             reals = ob::ScopedState<>(space, path.getState(i)).reals();
00123             std::cout << "path " << reals[0] <<' '<< reals[1] << ' ' << reals[2] << std::endl;
00124         }
00125     }
00126     else
00127         std::cout << "No solution found" << std::endl;
00128 }
00129 
00130 void printTrajectory(ob::StateSpacePtr space, const std::vector<double>& pt)
00131 {
00132     if (pt.size()!=3) throw ompl::Exception("3 arguments required for trajectory option");
00133     const unsigned int num_pts = 50;
00134     ob::ScopedState<> from(space), to(space), s(space);
00135     std::vector<double> reals;
00136 
00137     from[0] = from[1] = from[2] = 0.;
00138 
00139     to[0] = pt[0];
00140     to[1] = pt[1];
00141     to[2] = pt[2];
00142 
00143     std::cout << "distance: " << space->distance(from(), to()) << "\npath:\n";
00144     for (unsigned int i=0; i<=num_pts; ++i)
00145     {
00146         space->interpolate(from(), to(), (double)i/num_pts, s());
00147         reals = s.reals();
00148         std::cout << "path " << reals[0] << ' ' << reals[1] << ' ' << reals[2] << ' ' << std::endl;
00149     }
00150 }
00151 
00152 void printDistanceGrid(ob::StateSpacePtr space)
00153 {
00154     // print the distance for (x,y,theta) for all points in a 3D grid in SE(2)
00155     // over [-5,5) x [-5, 5) x [-pi,pi).
00156     //
00157     // The output should be redirected to a file, say, distance.txt. This
00158     // can then be read and plotted in Matlab like so:
00159     //     x = reshape(load('distance.txt'),200,200,200);
00160     //     for i=1:200,
00161     //         contourf(squeeze(x(i,:,:)),30);
00162     //         axis equal; axis tight; colorbar; pause;
00163     //     end;
00164     const unsigned int num_pts = 200;
00165     ob::ScopedState<> from(space), to(space);
00166     from[0] = from[1] = from[2] = 0.;
00167 
00168     for (unsigned int i=0; i<num_pts; ++i)
00169         for (unsigned int j=0; j<num_pts; ++j)
00170             for (unsigned int k=0; k<num_pts; ++k)
00171             {
00172                 to[0] = 5. * (2. * (double)i/num_pts - 1.);
00173                 to[1] = 5. * (2. * (double)j/num_pts - 1.);
00174                 to[2] = boost::math::constants::pi<double>() * (2. * (double)k/num_pts - 1.);
00175                 std::cout << space->distance(from(), to()) << '\n';
00176             }
00177 
00178 }
00179 
00180 int main(int argc, char* argv[])
00181 {
00182     try
00183     {
00184         po::options_description desc("Options");
00185         desc.add_options()
00186             ("help", "show help message")
00187             ("dubins", "use Dubins state space")
00188             ("dubinssym", "use symmetrized Dubins state space")
00189             ("reedsshepp", "use Reeds-Shepp state space (default)")
00190             ("easyplan", "solve easy planning problem and print path")
00191             ("hardplan", "solve hard planning problem and print path")
00192             ("trajectory", po::value<std::vector<double > >()->multitoken(),
00193                 "print trajectory from (0,0,0) to a user-specified x, y, and theta")
00194             ("distance", "print distance grid")
00195         ;
00196 
00197         po::variables_map vm;
00198         po::store(po::parse_command_line(argc, argv, desc,
00199             po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
00200         po::notify(vm);
00201 
00202         if (vm.count("help") || argc==1)
00203         {
00204             std::cout << desc << "\n";
00205             return 1;
00206         }
00207 
00208         ob::StateSpacePtr space(new ob::ReedsSheppStateSpace);
00209 
00210         if (vm.count("dubins"))
00211             space = ob::StateSpacePtr(new ob::DubinsStateSpace);
00212         if (vm.count("dubinssym"))
00213             space = ob::StateSpacePtr(new ob::DubinsStateSpace(1., true));
00214         if (vm.count("easyplan"))
00215             plan(space, true);
00216         if (vm.count("hardplan"))
00217             plan(space, false);
00218         if (vm.count("trajectory"))
00219             printTrajectory(space, vm["trajectory"].as<std::vector<double> >());
00220         if (vm.count("distance"))
00221             printDistanceGrid(space);
00222     }
00223     catch(std::exception& e) {
00224         std::cerr << "error: " << e.what() << "\n";
00225         return 1;
00226     }
00227     catch(...) {
00228         std::cerr << "Exception of unknown type!\n";
00229     }
00230 
00231     return 0;
00232 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines