Loading...
Searching...
No Matches
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
49namespace ob = ompl::base;
50namespace oc = ompl::control;
51
52
54class KinematicCarModel
55{
56public:
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
84private:
85
86 const ob::StateSpace *space_;
87 const double carLength_;
88
89};
90
91
93template<typename F>
94class EulerIntegrator
95{
96public:
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 = 0;
105 std::valarray<double> dstate;
106 space_->copyState(result, start);
107 while (t+timeStep_ < 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, (duration-t) * 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
130private:
131
132 const ob::StateSpace *space_;
133 double timeStep_;
134 F ode_;
135};
136
137
138bool 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
158class DemoControlSpace : public oc::RealVectorControlSpace
159{
160public:
161
162 DemoControlSpace(const ob::StateSpacePtr &stateSpace) : oc::RealVectorControlSpace(stateSpace, 2)
163 {
164 }
165};
166
167class DemoStatePropagator : public oc::StatePropagator
168{
169public:
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
196void 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();
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
263int main(int /*argc*/, char ** /*argv*/)
264{
265 std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
266
267 planWithSimpleSetup();
268
269 return 0;
270}
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.
A state in SE(2): (x, y, yaw)
The definition of a state in SO(2)
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...
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
ompl::control::Control ControlType
Define the type of control allocated by this control space.
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()