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: Ioan Sucan */
36
37 #include <ompl/control/SpaceInformation.h>
38 #include <ompl/base/goals/GoalState.h>
39 #include <ompl/base/spaces/SE2StateSpace.h>
40 #include <ompl/control/spaces/RealVectorControlSpace.h>
41 #include <ompl/control/planners/kpiece/KPIECE1.h>
42 #include <ompl/control/planners/rrt/RRT.h>
43 #include <ompl/control/SimpleSetup.h>
44 #include <ompl/config.h>
45 #include <iostream>
46 #include <valarray>
47 #include <limits>
48
49 namespace ob = ompl::base;
50 namespace oc = ompl::control;
51
52
53 /// Model defining the motion of the robot
54 class KinematicCarModel
55 {
56 public:
57
KinematicCarModel(const ob::StateSpace * space)58 KinematicCarModel(const ob::StateSpace *space) : space_(space), carLength_(0.2)
59 {
60 }
61
62 /// implement the function describing the robot motion: qdot = f(q, u)
operator ()(const ob::State * state,const oc::Control * control,std::valarray<double> & dstate) const63 void operator()(const ob::State *state, const oc::Control *control, std::valarray<double> &dstate) const
64 {
65 const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;
66 const double theta = state->as<ob::SE2StateSpace::StateType>()->getYaw();
67
68 dstate.resize(3);
69 dstate[0] = u[0] * cos(theta);
70 dstate[1] = u[0] * sin(theta);
71 dstate[2] = u[0] * tan(u[1]) / carLength_;
72 }
73
74 /// implement y(n+1) = y(n) + d
update(ob::State * state,const std::valarray<double> & dstate) const75 void update(ob::State *state, const std::valarray<double> &dstate) const
76 {
77 ob::SE2StateSpace::StateType &s = *state->as<ob::SE2StateSpace::StateType>();
78 s.setX(s.getX() + dstate[0]);
79 s.setY(s.getY() + dstate[1]);
80 s.setYaw(s.getYaw() + dstate[2]);
81 space_->enforceBounds(state);
82 }
83
84 private:
85
86 const ob::StateSpace *space_;
87 const double carLength_;
88
89 };
90
91
92 /// Simple integrator: Euclidean method
93 template<typename F>
94 class EulerIntegrator
95 {
96 public:
97
EulerIntegrator(const ob::StateSpace * space,double timeStep)98 EulerIntegrator(const ob::StateSpace *space, double timeStep) : space_(space), timeStep_(timeStep), ode_(space)
99 {
100 }
101
propagate(const ob::State * start,const oc::Control * control,const double duration,ob::State * result) const102 void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result) const
103 {
104 double t = timeStep_;
105 std::valarray<double> dstate;
106 space_->copyState(result, start);
107 while (t < duration + std::numeric_limits<double>::epsilon())
108 {
109 ode_(result, control, dstate);
110 ode_.update(result, timeStep_ * dstate);
111 t += timeStep_;
112 }
113 if (t + std::numeric_limits<double>::epsilon() > duration)
114 {
115 ode_(result, control, dstate);
116 ode_.update(result, (t - duration) * dstate);
117 }
118 }
119
getTimeStep() const120 double getTimeStep() const
121 {
122 return timeStep_;
123 }
124
setTimeStep(double timeStep)125 void setTimeStep(double timeStep)
126 {
127 timeStep_ = timeStep;
128 }
129
130 private:
131
132 const ob::StateSpace *space_;
133 double timeStep_;
134 F ode_;
135 };
136
137
isStateValid(const oc::SpaceInformation * si,const ob::State * state)138 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
139 {
140 // ob::ScopedState<ob::SE2StateSpace>
141 /// cast the abstract state type to the type we expect
142 const auto *se2state = state->as<ob::SE2StateSpace::StateType>();
143
144 /// extract the first component of the state and cast it to what we expect
145 const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
146
147 /// extract the second component of the state and cast it to what we expect
148 const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
149
150 /// check validity of state defined by pos & rot
151
152
153 // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
154 return si->satisfiesBounds(state) && (const void*)rot != (const void*)pos;
155 }
156
157 /// @cond IGNORE
158 class DemoControlSpace : public oc::RealVectorControlSpace
159 {
160 public:
161
DemoControlSpace(const ob::StateSpacePtr & stateSpace)162 DemoControlSpace(const ob::StateSpacePtr &stateSpace) : oc::RealVectorControlSpace(stateSpace, 2)
163 {
164 }
165 };
166
167 class DemoStatePropagator : public oc::StatePropagator
168 {
169 public:
170
DemoStatePropagator(oc::SpaceInformation * si)171 DemoStatePropagator(oc::SpaceInformation *si) : oc::StatePropagator(si),
172 integrator_(si->getStateSpace().get(), 0.0)
173 {
174 }
175
propagate(const ob::State * state,const oc::Control * control,const double duration,ob::State * result) const176 void propagate(const ob::State *state, const oc::Control* control, const double duration, ob::State *result) const override
177 {
178 integrator_.propagate(state, control, duration, result);
179 }
180
setIntegrationTimeStep(double timeStep)181 void setIntegrationTimeStep(double timeStep)
182 {
183 integrator_.setTimeStep(timeStep);
184 }
185
getIntegrationTimeStep() const186 double getIntegrationTimeStep() const
187 {
188 return integrator_.getTimeStep();
189 }
190
191 EulerIntegrator<KinematicCarModel> integrator_;
192 };
193
194 /// @endcond
195
planWithSimpleSetup()196 void planWithSimpleSetup()
197 {
198 /// construct the state space we are planning in
199 auto space(std::make_shared<ob::SE2StateSpace>());
200
201 /// set the bounds for the R^2 part of SE(2)
202 ob::RealVectorBounds bounds(2);
203 bounds.setLow(-1);
204 bounds.setHigh(1);
205
206 space->setBounds(bounds);
207
208 // create a control space
209 auto cspace(std::make_shared<DemoControlSpace>(space));
210
211 // set the bounds for the control space
212 ob::RealVectorBounds cbounds(2);
213 cbounds.setLow(-0.3);
214 cbounds.setHigh(0.3);
215
216 cspace->setBounds(cbounds);
217
218 // define a simple setup class
219 oc::SimpleSetup ss(cspace);
220
221 /// set state validity checking for this space
222 oc::SpaceInformation *si = ss.getSpaceInformation().get();
223 ss.setStateValidityChecker(
224 [si](const ob::State *state) { return isStateValid(si, state); });
225
226 /// set the propagation routine for this space
227 auto propagator(std::make_shared<DemoStatePropagator>(si));
228 ss.setStatePropagator(propagator);
229
230 /// create a start state
231 ob::ScopedState<ob::SE2StateSpace> start(space);
232 start->setX(-0.5);
233 start->setY(0.0);
234 start->setYaw(0.0);
235
236 /// create a goal state; use the hard way to set the elements
237 ob::ScopedState<ob::SE2StateSpace> goal(space);
238 goal->setX(0.0);
239 goal->setY(0.5);
240 goal->setYaw(0.0);
241
242 /// set the start and goal states
243 ss.setStartAndGoalStates(start, goal, 0.05);
244
245 /// we want to have a reasonable value for the propagation step size
246 ss.setup();
247 propagator->setIntegrationTimeStep(si->getPropagationStepSize());
248
249 /// attempt to solve the problem within one second of planning time
250 ob::PlannerStatus solved = ss.solve(10.0);
251
252 if (solved)
253 {
254 std::cout << "Found solution:" << std::endl;
255 /// print the path to screen
256
257 ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
258 }
259 else
260 std::cout << "No solution found" << std::endl;
261 }
262
main(int,char **)263 int main(int /*argc*/, char ** /*argv*/)
264 {
265 std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
266
267 planWithSimpleSetup();
268
269 return 0;
270 }
271