All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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  static double carLength = 20.;
55  int nsteps = ceil(duration / timeStep);
56  double dt = duration / nsteps;
57  const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;
58 
63 
64  si->getStateSpace()->copyState(result, state);
65  for(int i=0; i<nsteps; i++)
66  {
67  se2.setX(se2.getX() + timeStep * velocity.values[0] * cos(se2.getYaw()));
68  se2.setY(se2.getY() + timeStep * velocity.values[0] * sin(se2.getYaw()));
69  se2.setYaw(se2.getYaw() + timeStep * u[0]);
70  velocity.values[0] = velocity.values[0] + timeStep * (u[1]*gear.value);
71 
72  // 'guards' - conditions to change gears
73  if (gear.value > 0)
74  {
75  if (gear.value < 3 && velocity.values[0] > 10*(gear.value + 1))
76  gear.value++;
77  else if (gear.value > 1 && velocity.values[0] < 10*gear.value)
78  gear.value--;
79  }
80  if (!si->satisfiesBounds(result))
81  return;
82  }
83 }
84 
85 // The free space consists of two narrow corridors connected at right angle.
86 // To make the turn, the car will have to downshift.
87 bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
88 {
89  const ob::SE2StateSpace::StateType *se2 =
90  state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
91  return si->satisfiesBounds(state) && (se2->getX() < -80. || se2->getY() > 80.);
92 }
93 
94 
95 int main(int argc, char* argv[])
96 {
97  // plan for hybrid car in SE(2) with discrete gears
100  // set the range for gears: [-1,3] inclusive
102  ob::StateSpacePtr stateSpace = SE2 + velocity + gear;
103 
104  // set the bounds for the R^2 part of SE(2)
105  ob::RealVectorBounds bounds(2);
106  bounds.setLow(-100);
107  bounds.setHigh(100);
108  SE2->as<ob::SE2StateSpace>()->setBounds(bounds);
109 
110  // set the bounds for the velocity
111  ob::RealVectorBounds velocityBound(1);
112  velocityBound.setLow(0);
113  velocityBound.setHigh(60);
114  velocity->as<ob::RealVectorStateSpace>()->setBounds(velocityBound);
115 
116  // create start and goal states
117  ob::ScopedState<> start(stateSpace);
118  ob::ScopedState<> goal(stateSpace);
119 
120  // Both start and goal are states with high velocity with the car in third gear.
121  // However, to make the turn, the car cannot stay in third gear and will have to
122  // shift to first gear.
123  start[0] = start[1] = -90.; // position
124  start[2] = boost::math::constants::pi<double>()/2; // orientation
125  start[3] = 40.; // velocity
126  start->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear
127 
128  goal[0] = goal[1] = 90.; // position
129  goal[2] = 0.; // orientation
130  goal[3] = 40.; // velocity
131  goal->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear
132 
133  oc::ControlSpacePtr cmanifold(new oc::RealVectorControlSpace(stateSpace, 2));
134 
135  // set the bounds for the control manifold
136  ob::RealVectorBounds cbounds(2);
137  // bounds for steering input
138  cbounds.setLow(0, -1.);
139  cbounds.setHigh(0, 1.);
140  // bounds for brake/gas input
141  cbounds.setLow(1, -20.);
142  cbounds.setHigh(1, 20.);
143  cmanifold->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
144 
145  oc::SimpleSetup setup(cmanifold);
146  setup.setStartAndGoalStates(start, goal, 5.);
147  setup.setStateValidityChecker(boost::bind(
148  &isStateValid, setup.getSpaceInformation().get(), _1));
149  setup.setStatePropagator(boost::bind(
150  &propagate, setup.getSpaceInformation().get(), _1, _2, _3, _4));
151  setup.getSpaceInformation()->setPropagationStepSize(.1);
152  setup.getSpaceInformation()->setMinMaxControlDuration(2, 3);
153 
154  // try to solve the problem
155  if (setup.solve(30))
156  {
157  // print the (approximate) solution path: print states along the path
158  // and controls required to get from one state to the next
159  oc::PathControl& path(setup.getSolutionPath());
160 
161  // print out full state on solution path
162  // (format: x, y, theta, v, u0, u1, dt)
163  for(unsigned int i=0; i<path.getStateCount(); ++i)
164  {
165  const ob::State* state = path.getState(i);
166  const ob::SE2StateSpace::StateType *se2 =
167  state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
168  const ob::RealVectorStateSpace::StateType *velocity =
169  state->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(1);
171  state->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2);
172  std::cout << se2->getX() << ' ' << se2->getY() << ' ' << se2->getYaw()
173  << ' ' << velocity->values[0] << ' ' << gear->value << ' ';
174  if (i==0)
175  // null controls applied for zero seconds to get to start state
176  std::cout << "0 0 0";
177  else
178  {
179  // print controls and control duration needed to get from state i-1 to state i
180  const double* u =
181  path.getControl(i-1)->as<oc::RealVectorControlSpace::ControlType>()->values;
182  std::cout << u[0] << ' ' << u[1] << ' ' << path.getControlDuration(i-1);
183  }
184  std::cout << std::endl;
185  }
186  if (!setup.haveExactSolutionPath())
187  {
188  std::cout << "Solution is approximate. Distance to actual goal is " <<
189  setup.getProblemDefinition()->getSolutionDifference() << std::endl;
190  }
191  }
192 
193  return 0;
194 }