Loading...
Searching...
No Matches
RigidBodyPlanningWithODESolverAndControls.cpp
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
47namespace ob = ompl::base;
48namespace oc = ompl::control;
49
50// Kinematic car model object definition. This class does NOT use ODESolver to propagate the system.
51class KinematicCarModel : public oc::StatePropagator
52{
53 public:
54 KinematicCarModel(const oc::SpaceInformationPtr &si) : oc::StatePropagator(si)
55 {
56 space_ = si->getStateSpace();
57 carLength_ = 0.2;
58 timeStep_ = 0.01;
59 }
60
61 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.
68 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
87 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
99 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.
115void 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.
130void KinematicCarPostIntegration (const ob::State* /*state*/, const oc::Control* /*control*/, const double /*duration*/, ob::State *result)
131{
132 // Normalize orientation between 0 and 2*pi
134 SO2.enforceBounds(result->as<ob::SE2StateSpace::StateType>()->as<ob::SO2StateSpace::StateType>(1));
135}
136
137bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
138{
139 // ob::ScopedState<ob::SE2StateSpace>
141 const auto *se2state = state->as<ob::SE2StateSpace::StateType>();
142
144 const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
145
147 const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
148
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
157class DemoControlSpace : public oc::RealVectorControlSpace
158{
159public:
160
161 DemoControlSpace(const ob::StateSpacePtr &stateSpace) : oc::RealVectorControlSpace(stateSpace, 2)
162 {
163 }
164};
166
167void planWithSimpleSetup()
168{
170 auto space(std::make_shared<ob::SE2StateSpace>());
171
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
208 start->setX(-0.5);
209 start->setY(0.0);
210 start->setYaw(0.0);
211
214 goal->setX(0.0);
215 goal->setY(0.5);
216 goal->setYaw(0.0);
217
219 ss.setStartAndGoalStates(start, goal, 0.05);
220
222 ss.setup();
223
225 ob::PlannerStatus solved = ss.solve(10.0);
226
227 if (solved)
228 {
229 std::cout << "Found solution:" << std::endl;
231
232 ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
233 }
234 else
235 std::cout << "No solution found" << std::endl;
236}
237
238int main(int /*argc*/, char ** /*argv*/)
239{
240 std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
241
242 planWithSimpleSetup();
243
244 return 0;
245}
The lower and upper bounds for an Rn space.
A state space representing SO(2). The distance function and interpolation take into account angle wra...
Definition: SO2StateSpace.h:64
void enforceBounds(State *state) const override
Normalize the value of the state to the interval [-Pi, Pi)
Definition of a scoped state.
Definition: ScopedState.h:57
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 an abstract control.
Definition: Control.h:48
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:64
Basic solver for ordinary differential equations of the type q' = f(q, u), where q is the current sta...
Definition: ODESolver.h:200
A control space representing Rn.
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
Space information containing necessary information for planning with controls. setup() needs to be ca...
Model the effect of controls on system states.
virtual void propagate(const base::State *state, const Control *control, double duration, base::State *result) const =0
Propagate from a state, given a control, for some specified amount of time (the amount of time can al...
StatePropagator(SpaceInformation *si)
Constructor.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:45
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49