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
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.
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::chrono::system_clock::duration duration
Representation of a time duration.
Definition: Time.h:67
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The lower and upper bounds for an Rn space.
Space information containing necessary information for planning with controls. setup() needs to be ca...