00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00048
00049
00050
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
00079 og::SimpleSetup ss(space);
00080
00081
00082 ob::SpaceInformationPtr si(ss.getSpaceInformation());
00083 ss.setStateValidityChecker(boost::bind(
00084 easy ? &isStateValidEasy : &isStateValidHard, si.get(), _1));
00085
00086
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
00100 ss.getSpaceInformation()->setStateValidityCheckingResolution(0.005);
00101 ss.setup();
00102 ss.print();
00103
00104
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
00113
00114
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
00155
00156
00157
00158
00159
00160
00161
00162
00163
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 }