1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, 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: Ryan Luna */
36
37 #include <ompl/control/SpaceInformation.h>
38 #include <ompl/base/spaces/SE2StateSpace.h>
39 #include <ompl/control/ODESolver.h>
40 #include <ompl/control/spaces/RealVectorControlSpace.h>
41 #include <ompl/control/SimpleSetup.h>
42 #include <ompl/config.h>
43 #include <iostream>
44 #include <valarray>
45 #include <limits>
46
47 namespace ob = ompl::base;
48 namespace oc = ompl::control;
49
50 // Kinematic car model object definition. This class does NOT use ODESolver to propagate the system.
51 class KinematicCarModel : public oc::StatePropagator
52 {
53 public:
KinematicCarModel(const oc::SpaceInformationPtr & si)54 KinematicCarModel(const oc::SpaceInformationPtr &si) : oc::StatePropagator(si)
55 {
56 space_ = si->getStateSpace();
57 carLength_ = 0.2;
58 timeStep_ = 0.01;
59 }
60
propagate(const ob::State * state,const oc::Control * control,const double duration,ob::State * result) const61 void propagate(const ob::State *state, const oc::Control* control, const double duration, ob::State *result) const override
62 {
63 EulerIntegration(state, control, duration, result);
64 }
65
66 protected:
67 // Explicit Euler Method for numerical integration.
EulerIntegration(const ob::State * start,const oc::Control * control,const double duration,ob::State * result) const68 void EulerIntegration(const ob::State *start, const oc::Control *control, const double duration, ob::State *result) const
69 {
70 double t = timeStep_;
71 std::valarray<double> dstate;
72 space_->copyState(result, start);
73 while (t < duration + std::numeric_limits<double>::epsilon())
74 {
75 ode(result, control, dstate);
76 update(result, timeStep_ * dstate);
77 t += timeStep_;
78 }
79 if (t + std::numeric_limits<double>::epsilon() > duration)
80 {
81 ode(result, control, dstate);
82 update(result, (t - duration) * dstate);
83 }
84 }
85
86 /// implement the function describing the robot motion: qdot = f(q, u)
ode(const ob::State * state,const oc::Control * control,std::valarray<double> & dstate) const87 void ode(const ob::State *state, const oc::Control *control, std::valarray<double> &dstate) const
88 {
89 const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;
90 const double theta = state->as<ob::SE2StateSpace::StateType>()->getYaw();
91
92 dstate.resize(3);
93 dstate[0] = u[0] * cos(theta);
94 dstate[1] = u[0] * sin(theta);
95 dstate[2] = u[0] * tan(u[1]) / carLength_;
96 }
97
98 /// implement y(n+1) = y(n) + d
update(ob::State * state,const std::valarray<double> & dstate) const99 void update(ob::State *state, const std::valarray<double> &dstate) const
100 {
101 ob::SE2StateSpace::StateType &s = *state->as<ob::SE2StateSpace::StateType>();
102 s.setX(s.getX() + dstate[0]);
103 s.setY(s.getY() + dstate[1]);
104 s.setYaw(s.getYaw() + dstate[2]);
105 space_->enforceBounds(state);
106 }
107
108 ob::StateSpacePtr space_;
109 double carLength_;
110 double timeStep_;
111 };
112
113 // Definition of the ODE for the kinematic car.
114 // This method is analogous to the above KinematicCarModel::ode function.
KinematicCarODE(const oc::ODESolver::StateType & q,const oc::Control * control,oc::ODESolver::StateType & qdot)115 void KinematicCarODE (const oc::ODESolver::StateType& q, const oc::Control* control, oc::ODESolver::StateType& qdot)
116 {
117 const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;
118 const double theta = q[2];
119 double carLength = 0.2;
120
121 // Zero out qdot
122 qdot.resize (q.size (), 0);
123
124 qdot[0] = u[0] * cos(theta);
125 qdot[1] = u[0] * sin(theta);
126 qdot[2] = u[0] * tan(u[1]) / carLength;
127 }
128
129 // This is a callback method invoked after numerical integration.
KinematicCarPostIntegration(const ob::State *,const oc::Control *,const double,ob::State * result)130 void KinematicCarPostIntegration (const ob::State* /*state*/, const oc::Control* /*control*/, const double /*duration*/, ob::State *result)
131 {
132 // Normalize orientation between 0 and 2*pi
133 ob::SO2StateSpace SO2;
134 SO2.enforceBounds(result->as<ob::SE2StateSpace::StateType>()->as<ob::SO2StateSpace::StateType>(1));
135 }
136
isStateValid(const oc::SpaceInformation * si,const ob::State * state)137 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
138 {
139 // ob::ScopedState<ob::SE2StateSpace>
140 /// cast the abstract state type to the type we expect
141 const auto *se2state = state->as<ob::SE2StateSpace::StateType>();
142
143 /// extract the first component of the state and cast it to what we expect
144 const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
145
146 /// extract the second component of the state and cast it to what we expect
147 const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
148
149 /// check validity of state defined by pos & rot
150
151
152 // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
153 return si->satisfiesBounds(state) && (const void*)rot != (const void*)pos;
154 }
155
156 /// @cond IGNORE
157 class DemoControlSpace : public oc::RealVectorControlSpace
158 {
159 public:
160
DemoControlSpace(const ob::StateSpacePtr & stateSpace)161 DemoControlSpace(const ob::StateSpacePtr &stateSpace) : oc::RealVectorControlSpace(stateSpace, 2)
162 {
163 }
164 };
165 /// @endcond
166
planWithSimpleSetup()167 void planWithSimpleSetup()
168 {
169 /// construct the state space we are planning in
170 auto space(std::make_shared<ob::SE2StateSpace>());
171
172 /// set the bounds for the R^2 part of SE(2)
173 ob::RealVectorBounds bounds(2);
174 bounds.setLow(-1);
175 bounds.setHigh(1);
176
177 space->setBounds(bounds);
178
179 // create a control space
180 auto cspace(std::make_shared<DemoControlSpace>(space));
181
182 // set the bounds for the control space
183 ob::RealVectorBounds cbounds(2);
184 cbounds.setLow(-0.3);
185 cbounds.setHigh(0.3);
186
187 cspace->setBounds(cbounds);
188
189 // define a simple setup class
190 oc::SimpleSetup ss(cspace);
191
192 // set state validity checking for this space
193 oc::SpaceInformation *si = ss.getSpaceInformation().get();
194 ss.setStateValidityChecker(
195 [si](const ob::State *state) { return isStateValid(si, state); });
196
197 // Setting the propagation routine for this space:
198 // KinematicCarModel does NOT use ODESolver
199 //ss.setStatePropagator(std::make_shared<KinematicCarModel>(ss.getSpaceInformation()));
200
201 // Use the ODESolver to propagate the system. Call KinematicCarPostIntegration
202 // when integration has finished to normalize the orientation values.
203 auto odeSolver(std::make_shared<oc::ODEBasicSolver<>>(ss.getSpaceInformation(), &KinematicCarODE));
204 ss.setStatePropagator(oc::ODESolver::getStatePropagator(odeSolver, &KinematicCarPostIntegration));
205
206 /// create a start state
207 ob::ScopedState<ob::SE2StateSpace> start(space);
208 start->setX(-0.5);
209 start->setY(0.0);
210 start->setYaw(0.0);
211
212 /// create a goal state; use the hard way to set the elements
213 ob::ScopedState<ob::SE2StateSpace> goal(space);
214 goal->setX(0.0);
215 goal->setY(0.5);
216 goal->setYaw(0.0);
217
218 /// set the start and goal states
219 ss.setStartAndGoalStates(start, goal, 0.05);
220
221 /// we want to have a reasonable value for the propagation step size
222 ss.setup();
223
224 /// attempt to solve the problem within one second of planning time
225 ob::PlannerStatus solved = ss.solve(10.0);
226
227 if (solved)
228 {
229 std::cout << "Found solution:" << std::endl;
230 /// print the path to screen
231
232 ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
233 }
234 else
235 std::cout << "No solution found" << std::endl;
236 }
237
main(int,char **)238 int main(int /*argc*/, char ** /*argv*/)
239 {
240 std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
241
242 planWithSimpleSetup();
243
244 return 0;
245 }
246