RigidBodyPlanningWithIntegrationAndControls.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: 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 
54 class KinematicCarModel
55 {
56 public:
57 
58  KinematicCarModel(const ob::StateSpace *space) : space_(space), carLength_(0.2)
59  {
60  }
61 
63  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 
75  void update(ob::State *state, const std::valarray<double> &dstate) const
76  {
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 
93 template<typename F>
94 class EulerIntegrator
95 {
96 public:
97 
98  EulerIntegrator(const ob::StateSpace *space, double timeStep) : space_(space), timeStep_(timeStep), ode_(space)
99  {
100  }
101 
102  void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result) const
103  {
104  double t = 0;
105  std::valarray<double> dstate;
106  space_->copyState(result, start);
107  while (t+timeStep_ < 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, (duration-t) * dstate);
117  }
118  }
119 
120  double getTimeStep() const
121  {
122  return timeStep_;
123  }
124 
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 
138 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
139 {
140  // ob::ScopedState<ob::SE2StateSpace>
142  const auto *se2state = state->as<ob::SE2StateSpace::StateType>();
143 
145  const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
146 
148  const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
149 
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 
158 class DemoControlSpace : public oc::RealVectorControlSpace
159 {
160 public:
161 
162  DemoControlSpace(const ob::StateSpacePtr &stateSpace) : oc::RealVectorControlSpace(stateSpace, 2)
163  {
164  }
165 };
166 
167 class DemoStatePropagator : public oc::StatePropagator
168 {
169 public:
170 
171  DemoStatePropagator(oc::SpaceInformation *si) : oc::StatePropagator(si),
172  integrator_(si->getStateSpace().get(), 0.0)
173  {
174  }
175 
176  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 
181  void setIntegrationTimeStep(double timeStep)
182  {
183  integrator_.setTimeStep(timeStep);
184  }
185 
186  double getIntegrationTimeStep() const
187  {
188  return integrator_.getTimeStep();
189  }
190 
191  EulerIntegrator<KinematicCarModel> integrator_;
192 };
193 
195 
196 void planWithSimpleSetup()
197 {
199  auto space(std::make_shared<ob::SE2StateSpace>());
200 
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 
222  oc::SpaceInformation *si = ss.getSpaceInformation().get();
223  ss.setStateValidityChecker(
224  [si](const ob::State *state) { return isStateValid(si, state); });
225 
227  auto propagator(std::make_shared<DemoStatePropagator>(si));
228  ss.setStatePropagator(propagator);
229 
232  start->setX(-0.5);
233  start->setY(0.0);
234  start->setYaw(0.0);
235 
238  goal->setX(0.0);
239  goal->setY(0.5);
240  goal->setYaw(0.0);
241 
243  ss.setStartAndGoalStates(start, goal, 0.05);
244 
246  ss.setup();
247  propagator->setIntegrationTimeStep(si->getPropagationStepSize());
248 
250  ob::PlannerStatus solved = ss.solve(10.0);
251 
252  if (solved)
253  {
254  std::cout << "Found solution:" << std::endl;
256 
257  ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
258  }
259  else
260  std::cout << "No solution found" << std::endl;
261 }
262 
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 }
Definition of an abstract control.
Definition: Control.h:111
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:159
Definition of an abstract state.
Definition: State.h:113
This namespace contains sampling based planning routines shared by both planning under geometric cons...
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...
Model the effect of controls on system states.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
ompl::control::Control ControlType
Define the type of control allocated by this control space.
Definition: ControlSpace.h:131
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:126
A class to store the exit status of Planner::solve()
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:76
Space information containing necessary information for planning with controls. setup() needs to be ca...
A control space representing Rn.
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:160
Definition of a scoped state.
Definition: ScopedState.h:120
The lower and upper bounds for an Rn space.