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 
52 namespace ob = ompl::base;
53 namespace oc = ompl::control;
54 
55 // a decomposition is only needed for SyclopRRT and SyclopEST
56 class MyDecomposition : public oc::GridDecomposition
57 {
58 public:
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 
77 bool 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 
96 void 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 
110 void 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 
198 void 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 
262 int 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 }
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
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.
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
virtual void sampleFullState(const base::StateSamplerPtr &sampler, const std::vector< double > &coord, base::State *s) const =0
Samples a State using a projected coordinate and a StateSampler.
virtual void project(const base::State *s, std::vector< double > &coord) const =0
Project a given State to a set of coordinates in R^k, where k is the dimension of this Decomposition.
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
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition: Time.h:55
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49