Loading...
Searching...
No Matches
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
47namespace ob = ompl::base;
48namespace oc = ompl::control;
49
50void 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.
86bool 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
94int 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}
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition StateSpace.h:577
Definition of a compound state.
Definition State.h:87
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition State.h:95
The definition of a discrete state.
The lower and upper bounds for an Rn space.
A state in SE(2): (x, y, yaw)
Definition of a scoped state.
Definition ScopedState.h:57
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
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
Definition of a control path.
Definition PathControl.h:61
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...
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
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition Time.h:55