Loading...
Searching...
No Matches
RigidBodyPlanningWithControls.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/planners/est/EST.h>
44#include <ompl/control/planners/syclop/SyclopRRT.h>
45#include <ompl/control/planners/syclop/SyclopEST.h>
46#include <ompl/control/planners/pdst/PDST.h>
47#include <ompl/control/planners/syclop/GridDecomposition.h>
48#include <ompl/control/SimpleSetup.h>
49#include <ompl/config.h>
50#include <iostream>
51
52namespace ob = ompl::base;
53namespace oc = ompl::control;
54
55// a decomposition is only needed for SyclopRRT and SyclopEST
56class MyDecomposition : public oc::GridDecomposition
57{
58public:
59 MyDecomposition(const int length, const ob::RealVectorBounds& bounds)
60 : GridDecomposition(length, 2, bounds)
61 {
62 }
63 void project(const ob::State* s, std::vector<double>& coord) const override
64 {
65 coord.resize(2);
66 coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
67 coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
68 }
69
70 void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const override
71 {
72 sampler->sampleUniform(s);
73 s->as<ob::SE2StateSpace::StateType>()->setXY(coord[0], coord[1]);
74 }
75};
76
77bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
78{
79 // ob::ScopedState<ob::SE2StateSpace>
80 // cast the abstract state type to the type we expect
81 const auto *se2state = state->as<ob::SE2StateSpace::StateType>();
82
83 // extract the first component of the state and cast it to what we expect
84 const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
85
86 // extract the second component of the state and cast it to what we expect
87 const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
88
89 // check validity of state defined by pos & rot
90
91
92 // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
93 return si->satisfiesBounds(state) && (const void*)rot != (const void*)pos;
94}
95
96void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
97{
98 const auto *se2state = start->as<ob::SE2StateSpace::StateType>();
99 const double* pos = se2state->as<ob::RealVectorStateSpace::StateType>(0)->values;
100 const double rot = se2state->as<ob::SO2StateSpace::StateType>(1)->value;
101 const double* ctrl = control->as<oc::RealVectorControlSpace::ControlType>()->values;
102
103 result->as<ob::SE2StateSpace::StateType>()->setXY(
104 pos[0] + ctrl[0] * duration * cos(rot),
105 pos[1] + ctrl[0] * duration * sin(rot));
106 result->as<ob::SE2StateSpace::StateType>()->setYaw(
107 rot + ctrl[1] * duration);
108}
109
110void plan()
111{
112
113 // construct the state space we are planning in
114 auto space(std::make_shared<ob::SE2StateSpace>());
115
116 // set the bounds for the R^2 part of SE(2)
117 ob::RealVectorBounds bounds(2);
118 bounds.setLow(-1);
119 bounds.setHigh(1);
120
121 space->setBounds(bounds);
122
123 // create a control space
124 auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));
125
126 // set the bounds for the control space
127 ob::RealVectorBounds cbounds(2);
128 cbounds.setLow(-0.3);
129 cbounds.setHigh(0.3);
130
131 cspace->setBounds(cbounds);
132
133 // construct an instance of space information from this control space
134 auto si(std::make_shared<oc::SpaceInformation>(space, cspace));
135
136 // set state validity checking for this space
137 si->setStateValidityChecker(
138 [&si](const ob::State *state) { return isStateValid(si.get(), state); });
139
140 // set the state propagation routine
141 si->setStatePropagator(propagate);
142
143 // create a start state
145 start->setX(-0.5);
146 start->setY(0.0);
147 start->setYaw(0.0);
148
149 // create a goal state
151 goal->setX(0.5);
152
153 // create a problem instance
154 auto pdef(std::make_shared<ob::ProblemDefinition>(si));
155
156 // set the start and goal states
157 pdef->setStartAndGoalStates(start, goal, 0.1);
158
159 // create a planner for the defined space
160 //auto planner(std::make_shared<oc::RRT>(si));
161 //auto planner(std::make_shared<oc::EST>(si));
162 //auto planner(std::make_shared<oc::KPIECE1>(si));
163 auto decomp(std::make_shared<MyDecomposition>(32, bounds));
164 auto planner(std::make_shared<oc::SyclopEST>(si, decomp));
165 //auto planner(std::make_shared<oc::SyclopRRT>(si, decomp));
166
167 // set the problem we are trying to solve for the planner
168 planner->setProblemDefinition(pdef);
169
170 // perform setup steps for the planner
171 planner->setup();
172
173
174 // print the settings for this space
175 si->printSettings(std::cout);
176
177 // print the problem settings
178 pdef->print(std::cout);
179
180 // attempt to solve the problem within ten seconds of planning time
181 ob::PlannerStatus solved = planner->ob::Planner::solve(10.0);
182
183 if (solved)
184 {
185 // get the goal representation from the problem definition (not the same as the goal state)
186 // and inquire about the found path
187 ob::PathPtr path = pdef->getSolutionPath();
188 std::cout << "Found solution:" << std::endl;
189
190 // print the path to screen
191 path->print(std::cout);
192 }
193 else
194 std::cout << "No solution found" << std::endl;
195}
196
197
198void planWithSimpleSetup()
199{
200 // construct the state space we are planning in
201 auto space(std::make_shared<ob::SE2StateSpace>());
202
203 // set the bounds for the R^2 part of SE(2)
204 ob::RealVectorBounds bounds(2);
205 bounds.setLow(-1);
206 bounds.setHigh(1);
207
208 space->setBounds(bounds);
209
210 // create a control space
211 auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));
212
213 // set the bounds for the control space
214 ob::RealVectorBounds cbounds(2);
215 cbounds.setLow(-0.3);
216 cbounds.setHigh(0.3);
217
218 cspace->setBounds(cbounds);
219
220 // define a simple setup class
221 oc::SimpleSetup ss(cspace);
222
223 // set the state propagation routine
224 ss.setStatePropagator(propagate);
225
226 // set state validity checking for this space
227 ss.setStateValidityChecker(
228 [&ss](const ob::State *state) { return isStateValid(ss.getSpaceInformation().get(), state); });
229
230 // create a start state
232 start->setX(-0.5);
233 start->setY(0.0);
234 start->setYaw(0.0);
235
236 // create a goal state; use the hard way to set the elements
238 (*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
239 (*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.5;
240 (*goal)[1]->as<ob::SO2StateSpace::StateType>()->value = 0.0;
241
242
243 // set the start and goal states
244 ss.setStartAndGoalStates(start, goal, 0.05);
245
246 // ss.setPlanner(std::make_shared<oc::PDST>(ss.getSpaceInformation()));
247 // ss.getSpaceInformation()->setMinMaxControlDuration(1,100);
248 // attempt to solve the problem within one second of planning time
249 ob::PlannerStatus solved = ss.solve(10.0);
250
251 if (solved)
252 {
253 std::cout << "Found solution:" << std::endl;
254 // print the path to screen
255
256 ss.getSolutionPath().printAsMatrix(std::cout);
257 }
258 else
259 std::cout << "No solution found" << std::endl;
260}
261
262int main(int /*argc*/, char ** /*argv*/)
263{
264 std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
265
266 // plan();
267 //
268 // std::cout << std::endl << std::endl;
269 //
270 planWithSimpleSetup();
271
272 return 0;
273}
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
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
Definition of an abstract control.
Definition Control.h:48
const T * as() const
Cast this instance to a desired type.
Definition Control.h:64
A GridDecomposition is a Decomposition implemented using a grid.
GridDecomposition(int len, int dim, const base::RealVectorBounds &b)
Constructor. Creates a GridDecomposition as a hypercube with a given dimension, side length,...
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
A class to store the exit status of Planner::solve()