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
82 
83  // extract the first component of the state and cast it to what we expect
85 
86  // extract the second component of the state and cast it to what we expect
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 {
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
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 one second 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, char **)
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() const
Cast this instance to a desired type.
Definition: Control.h:64
Definition of a scoped state.
Definition: ScopedState.h:56
Definition of an abstract control.
Definition: Control.h:47
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
A shared pointer wrapper for ompl::base::StateSampler.
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
A GridDecomposition is a Decomposition implemented using a grid.
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:97
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.
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
void printSettings(std::ostream &out=std::cout) const override
Print information about the current instance of the state space.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
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...
GridDecomposition(int len, int dim, const base::RealVectorBounds &b)
Constructor. Creates a GridDecomposition as a hypercube with a given dimension, side length...
The lower and upper bounds for an Rn space.
void setStatePropagator(const StatePropagatorFn &fn)
Set the function that performs state propagation.
Space information containing necessary information for planning with controls. setup() needs to be ca...
A shared pointer wrapper for ompl::base::Path.