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 
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:
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  {
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.
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.
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;
135 }
136 
137 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
138 {
139  // ob::ScopedState<ob::SE2StateSpace>
142 
145 
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 
157 class DemoControlSpace : public oc::RealVectorControlSpace
158 {
159 public:
160 
161  DemoControlSpace(const ob::StateSpacePtr &stateSpace) : oc::RealVectorControlSpace(stateSpace, 2)
162  {
163  }
164 };
166 
167 void 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();
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 
238 int main(int, char **)
239 {
240  std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
241 
242  planWithSimpleSetup();
243 
244  return 0;
245 }
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:64
Definition of a scoped state.
Definition: ScopedState.h:56
Definition of an abstract control.
Definition: Control.h:47
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
A shared pointer wrapper for ompl::base::StateSpace.
virtual void propagate(const base::State *state, const Control *control, const 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...
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:97
static StatePropagatorPtr getStatePropagator(ODESolverPtr solver, const PostPropagationEvent &postEvent=nullptr)
Retrieve a StatePropagator object that solves a system of ordinary differential equations defined by ...
Definition: ODESolver.h:123
Model the effect of controls on system states.
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
A control space representing Rn.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
std::vector< double > StateType
Portable data type for the state values.
Definition: ODESolver.h:73
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
A shared pointer wrapper for ompl::control::SpaceInformation.
void enforceBounds(State *state) const override
Normalize the value of the state to the interval [-Pi, Pi)
Basic solver for ordinary differential equations of the type q&#39; = f(q, u), where q is the current sta...
Definition: ODESolver.h:195
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:63
Space information containing necessary information for planning with controls. setup() needs to be ca...
StatePropagator(SpaceInformation *si)
Constructor.