Loading...
Searching...
No Matches
GeometricCarPlanning.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2010, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Mark Moll */
36
37#include <ompl/base/spaces/DubinsStateSpace.h>
38#include <ompl/base/spaces/ReedsSheppStateSpace.h>
39#include <ompl/base/ScopedState.h>
40#include <ompl/geometric/SimpleSetup.h>
41#include <boost/program_options.hpp>
42
43namespace ob = ompl::base;
44namespace og = ompl::geometric;
45namespace po = boost::program_options;
46
47// The easy problem is the standard narrow passage problem: two big open
48// spaces connected by a narrow passage. The hard problem is essentially
49// one long narrow passage with the robot facing towards the long walls
50// in both the start and goal configurations.
51
52bool isStateValidEasy(const ob::SpaceInformation *si, const ob::State *state)
53{
54 const auto *s = state->as<ob::SE2StateSpace::StateType>();
55 double x=s->getX(), y=s->getY();
56 return si->satisfiesBounds(s) && (x<5 || x>13 || (y>8.5 && y<9.5));
57}
58
59bool isStateValidHard(const ob::SpaceInformation *si, const ob::State *state)
60{
61 return si->satisfiesBounds(state);
62}
63
64void plan(const ob::StateSpacePtr& space, bool easy)
65{
66 ob::ScopedState<> start(space), goal(space);
67 ob::RealVectorBounds bounds(2);
68 bounds.setLow(0);
69 if (easy)
70 bounds.setHigh(18);
71 else
72 {
73 bounds.high[0] = 6;
74 bounds.high[1] = .6;
75 }
76 space->as<ob::SE2StateSpace>()->setBounds(bounds);
77
78 // define a simple setup class
79 og::SimpleSetup ss(space);
80
81 // set state validity checking for this space
82 const ob::SpaceInformation *si = ss.getSpaceInformation().get();
83 auto isStateValid = easy ? isStateValidEasy : isStateValidHard;
84 ss.setStateValidityChecker([isStateValid, si](const ob::State *state)
85 {
86 return isStateValid(si, state);
87 });
88
89 // set the start and goal states
90 if (easy)
91 {
92 start[0] = start[1] = 1.; start[2] = 0.;
93 goal[0] = goal[1] = 17; goal[2] = -.99*boost::math::constants::pi<double>();
94 }
95 else
96 {
97 start[0] = start[1] = .5; start[2] = .5*boost::math::constants::pi<double>();;
98 goal[0] = 5.5; goal[1] = .5; goal[2] = .5*boost::math::constants::pi<double>();
99 }
100 ss.setStartAndGoalStates(start, goal);
101
102 // this call is optional, but we put it in to get more output information
103 ss.getSpaceInformation()->setStateValidityCheckingResolution(0.005);
104 ss.setup();
105 ss.print();
106
107 // attempt to solve the problem within 30 seconds of planning time
108 ob::PlannerStatus solved = ss.solve(30.0);
109
110 if (solved)
111 {
112 std::vector<double> reals;
113
114 std::cout << "Found solution:" << std::endl;
115 ss.simplifySolution();
116 og::PathGeometric path = ss.getSolutionPath();
117 path.interpolate(1000);
118 path.printAsMatrix(std::cout);
119 }
120 else
121 std::cout << "No solution found" << std::endl;
122}
123
124void printTrajectory(const ob::StateSpacePtr& space, const std::vector<double>& pt)
125{
126 if (pt.size()!=3) throw ompl::Exception("3 arguments required for trajectory option");
127 const unsigned int num_pts = 50;
128 ob::ScopedState<> from(space), to(space), s(space);
129 std::vector<double> reals;
130
131 from[0] = from[1] = from[2] = 0.;
132
133 to[0] = pt[0];
134 to[1] = pt[1];
135 to[2] = pt[2];
136
137 std::cout << "distance: " << space->distance(from(), to()) << "\npath:\n";
138 for (unsigned int i=0; i<=num_pts; ++i)
139 {
140 space->interpolate(from(), to(), (double)i/num_pts, s());
141 reals = s.reals();
142 std::cout << "path " << reals[0] << ' ' << reals[1] << ' ' << reals[2] << ' ' << std::endl;
143 }
144}
145
146void printDistanceGrid(const ob::StateSpacePtr& space)
147{
148 // print the distance for (x,y,theta) for all points in a 3D grid in SE(2)
149 // over [-5,5) x [-5, 5) x [-pi,pi).
150 //
151 // The output should be redirected to a file, say, distance.txt. This
152 // can then be read and plotted in Matlab like so:
153 // x = reshape(load('distance.txt'),200,200,200);
154 // for i=1:200,
155 // contourf(squeeze(x(i,:,:)),30);
156 // axis equal; axis tight; colorbar; pause;
157 // end;
158 const unsigned int num_pts = 200;
159 ob::ScopedState<> from(space), to(space);
160 from[0] = from[1] = from[2] = 0.;
161
162 for (unsigned int i=0; i<num_pts; ++i)
163 for (unsigned int j=0; j<num_pts; ++j)
164 for (unsigned int k=0; k<num_pts; ++k)
165 {
166 to[0] = 5. * (2. * (double)i/num_pts - 1.);
167 to[1] = 5. * (2. * (double)j/num_pts - 1.);
168 to[2] = boost::math::constants::pi<double>() * (2. * (double)k/num_pts - 1.);
169 std::cout << space->distance(from(), to()) << '\n';
170 }
171
172}
173
174int main(int argc, char* argv[])
175{
176 try
177 {
178 po::options_description desc("Options");
179 desc.add_options()
180 ("help", "show help message")
181 ("dubins", "use Dubins state space")
182 ("dubinssym", "use symmetrized Dubins state space")
183 ("reedsshepp", "use Reeds-Shepp state space (default)")
184 ("easyplan", "solve easy planning problem and print path")
185 ("hardplan", "solve hard planning problem and print path")
186 ("trajectory", po::value<std::vector<double > >()->multitoken(),
187 "print trajectory from (0,0,0) to a user-specified x, y, and theta")
188 ("distance", "print distance grid")
189 ;
190
191 po::variables_map vm;
192 po::store(po::parse_command_line(argc, argv, desc,
193 po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
194 po::notify(vm);
195
196 if ((vm.count("help") != 0u) || argc==1)
197 {
198 std::cout << desc << "\n";
199 return 1;
200 }
201
202 ob::StateSpacePtr space(std::make_shared<ob::ReedsSheppStateSpace>());
203
204 if (vm.count("dubins") != 0u)
205 space = std::make_shared<ob::DubinsStateSpace>();
206 if (vm.count("dubinssym") != 0u)
207 space = std::make_shared<ob::DubinsStateSpace>(1., true);
208 if (vm.count("easyplan") != 0u)
209 plan(space, true);
210 if (vm.count("hardplan") != 0u)
211 plan(space, false);
212 if (vm.count("trajectory") != 0u)
213 printTrajectory(space, vm["trajectory"].as<std::vector<double> >());
214 if (vm.count("distance") != 0u)
215 printDistanceGrid(space);
216 }
217 catch(std::exception& e) {
218 std::cerr << "error: " << e.what() << "\n";
219 return 1;
220 }
221 catch(...) {
222 std::cerr << "Exception of unknown type!\n";
223 }
224
225 return 0;
226}
The exception type for ompl.
Definition: Exception.h:47
The lower and upper bounds for an Rn space.
A state space representing SE(2)
Definition: SE2StateSpace.h:50
Definition of a scoped state.
Definition: ScopedState.h:57
The base class for space information. This contains all the information about the space planning is d...
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
Definition of a geometric path.
Definition: PathGeometric.h:66
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:48
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49