HybridSystemPlanning.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: Elizabeth Fudge */
36 
37 #include <ompl/base/goals/GoalState.h>
38 #include <ompl/base/spaces/SE2StateSpace.h>
39 #include <ompl/base/spaces/DiscreteStateSpace.h>
40 #include <ompl/control/spaces/RealVectorControlSpace.h>
41 #include <ompl/control/SimpleSetup.h>
42 #include <ompl/config.h>
43 #include <iostream>
44 #include <limits>
45 #include <boost/math/constants/constants.hpp>
46 
47 namespace ob = ompl::base;
48 namespace oc = ompl::control;
49 
50 void propagate(const oc::SpaceInformation *si, const ob::State *state,
51  const oc::Control* control, const double duration, ob::State *result)
52 {
53  static double timeStep = .01;
54  int nsteps = ceil(duration / timeStep);
55  double dt = duration / nsteps;
56  const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;
57 
62 
63  si->getStateSpace()->copyState(result, state);
64  for(int i=0; i<nsteps; i++)
65  {
66  se2.setX(se2.getX() + dt * velocity.values[0] * cos(se2.getYaw()));
67  se2.setY(se2.getY() + dt * velocity.values[0] * sin(se2.getYaw()));
68  se2.setYaw(se2.getYaw() + dt * u[0]);
69  velocity.values[0] = velocity.values[0] + dt * (u[1]*gear.value);
70 
71  // 'guards' - conditions to change gears
72  if (gear.value > 0)
73  {
74  if (gear.value < 3 && velocity.values[0] > 10*(gear.value + 1))
75  gear.value++;
76  else if (gear.value > 1 && velocity.values[0] < 10*gear.value)
77  gear.value--;
78  }
79  if (!si->satisfiesBounds(result))
80  return;
81  }
82 }
83 
84 // The free space consists of two narrow corridors connected at right angle.
85 // To make the turn, the car will have to downshift.
86 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
87 {
88  const auto *se2 =
89  state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
90  return si->satisfiesBounds(state) && (se2->getX() < -80. || se2->getY() > 80.);
91 }
92 
93 
94 int main(int /*argc*/, char** /*argv*/)
95 {
96  // plan for hybrid car in SE(2) with discrete gears
97  auto SE2(std::make_shared<ob::SE2StateSpace>());
98  auto velocity(std::make_shared<ob::RealVectorStateSpace>(1));
99  // set the range for gears: [-1,3] inclusive
100  auto gear(std::make_shared<ob::DiscreteStateSpace>(-1,3));
101  ob::StateSpacePtr stateSpace = SE2 + velocity + gear;
102 
103  // set the bounds for the R^2 part of SE(2)
104  ob::RealVectorBounds bounds(2);
105  bounds.setLow(-100);
106  bounds.setHigh(100);
107  SE2->setBounds(bounds);
108 
109  // set the bounds for the velocity
110  ob::RealVectorBounds velocityBound(1);
111  velocityBound.setLow(0);
112  velocityBound.setHigh(60);
113  velocity->setBounds(velocityBound);
114 
115  // create start and goal states
116  ob::ScopedState<> start(stateSpace);
117  ob::ScopedState<> goal(stateSpace);
118 
119  // Both start and goal are states with high velocity with the car in third gear.
120  // However, to make the turn, the car cannot stay in third gear and will have to
121  // shift to first gear.
122  start[0] = start[1] = -90.; // position
123  start[2] = boost::math::constants::pi<double>()/2; // orientation
124  start[3] = 40.; // velocity
125  start->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear
126 
127  goal[0] = goal[1] = 90.; // position
128  goal[2] = 0.; // orientation
129  goal[3] = 40.; // velocity
130  goal->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear
131 
132  oc::ControlSpacePtr cmanifold(std::make_shared<oc::RealVectorControlSpace>(stateSpace, 2));
133 
134  // set the bounds for the control manifold
135  ob::RealVectorBounds cbounds(2);
136  // bounds for steering input
137  cbounds.setLow(0, -1.);
138  cbounds.setHigh(0, 1.);
139  // bounds for brake/gas input
140  cbounds.setLow(1, -20.);
141  cbounds.setHigh(1, 20.);
142  cmanifold->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
143 
144  oc::SimpleSetup setup(cmanifold);
145  const oc::SpaceInformation *si = setup.getSpaceInformation().get();
146  setup.setStartAndGoalStates(start, goal, 5.);
147  setup.setStateValidityChecker([si](const ob::State *state)
148  {
149  return isStateValid(si, state);
150  });
151  setup.setStatePropagator([si](const ob::State *state, const oc::Control* control,
152  const double duration, ob::State *result)
153  {
154  propagate(si, state, control, duration, result);
155  });
156  setup.getSpaceInformation()->setPropagationStepSize(.1);
157  setup.getSpaceInformation()->setMinMaxControlDuration(2, 3);
158 
159  // try to solve the problem
160  if (setup.solve(30))
161  {
162  // print the (approximate) solution path: print states along the path
163  // and controls required to get from one state to the next
164  oc::PathControl& path(setup.getSolutionPath());
165 
166  // print out full state on solution path
167  // (format: x, y, theta, v, u0, u1, dt)
168  for(unsigned int i=0; i<path.getStateCount(); ++i)
169  {
170  const ob::State* state = path.getState(i);
171  const auto *se2 =
172  state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
173  const auto *velocity =
174  state->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(1);
175  const auto *gear =
176  state->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2);
177  std::cout << se2->getX() << ' ' << se2->getY() << ' ' << se2->getYaw()
178  << ' ' << velocity->values[0] << ' ' << gear->value << ' ';
179  if (i==0)
180  // null controls applied for zero seconds to get to start state
181  std::cout << "0 0 0";
182  else
183  {
184  // print controls and control duration needed to get from state i-1 to state i
185  const double* u =
186  path.getControl(i-1)->as<oc::RealVectorControlSpace::ControlType>()->values;
187  std::cout << u[0] << ' ' << u[1] << ' ' << path.getControlDuration(i-1);
188  }
189  std::cout << std::endl;
190  }
191  if (!setup.haveExactSolutionPath())
192  {
193  std::cout << "Solution is approximate. Distance to actual goal is " <<
194  setup.getProblemDefinition()->getSolutionDifference() << std::endl;
195  }
196  }
197 
198  return 0;
199 }
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:64
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
Definition of a compound state.
Definition: State.h:86
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.
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
Definition of a control path.
Definition: PathControl.h:60
A shared pointer wrapper for ompl::control::ControlSpace.
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
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
The lower and upper bounds for an Rn space.
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:95
Space information containing necessary information for planning with controls. setup() needs to be ca...