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 = 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 
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 }
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:95
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Definition: ScopedState.h:57
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:71
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
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
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...
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