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  virtual void project(const ob::State* s, std::vector<double>& coord) const
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  virtual void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const
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(void)
111 {
112 
113  // construct the state space we are planning in
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->as<ob::SE2StateSpace>()->setBounds(bounds);
122 
123  // create a control space
124  oc::ControlSpacePtr cspace(new 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->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
132 
133  // construct an instance of space information from this control space
134  oc::SpaceInformationPtr si(new oc::SpaceInformation(space, cspace));
135 
136  // set state validity checking for this space
137  si->setStateValidityChecker(boost::bind(&isStateValid, si.get(), _1));
138 
139  // set the state propagation routine
140  si->setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
141 
142  // create a start state
144  start->setX(-0.5);
145  start->setY(0.0);
146  start->setYaw(0.0);
147 
148  // create a goal state
150  goal->setX(0.5);
151 
152  // create a problem instance
154 
155  // set the start and goal states
156  pdef->setStartAndGoalStates(start, goal, 0.1);
157 
158  // create a planner for the defined space
159  //ob::PlannerPtr planner(new oc::RRT(si));
160  //ob::PlannerPtr planner(new oc::EST(si));
161  //ob::PlannerPtr planner(new oc::KPIECE1(si));
162  oc::DecompositionPtr decomp(new MyDecomposition(32, bounds));
163  ob::PlannerPtr planner(new oc::SyclopEST(si, decomp));
164  //ob::PlannerPtr planner(new oc::SyclopRRT(si, decomp));
165 
166  // set the problem we are trying to solve for the planner
167  planner->setProblemDefinition(pdef);
168 
169  // perform setup steps for the planner
170  planner->setup();
171 
172 
173  // print the settings for this space
174  si->printSettings(std::cout);
175 
176  // print the problem settings
177  pdef->print(std::cout);
178 
179  // attempt to solve the problem within one second of planning time
180  ob::PlannerStatus solved = planner->solve(10.0);
181 
182  if (solved)
183  {
184  // get the goal representation from the problem definition (not the same as the goal state)
185  // and inquire about the found path
186  ob::PathPtr path = pdef->getSolutionPath();
187  std::cout << "Found solution:" << std::endl;
188 
189  // print the path to screen
190  path->print(std::cout);
191  }
192  else
193  std::cout << "No solution found" << std::endl;
194 }
195 
196 
197 void planWithSimpleSetup(void)
198 {
199  // construct the state space we are planning in
201 
202  // set the bounds for the R^2 part of SE(2)
203  ob::RealVectorBounds bounds(2);
204  bounds.setLow(-1);
205  bounds.setHigh(1);
206 
207  space->as<ob::SE2StateSpace>()->setBounds(bounds);
208 
209  // create a control space
210  oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
211 
212  // set the bounds for the control space
213  ob::RealVectorBounds cbounds(2);
214  cbounds.setLow(-0.3);
215  cbounds.setHigh(0.3);
216 
217  cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
218 
219  // define a simple setup class
220  oc::SimpleSetup ss(cspace);
221 
222  // set the state propagation routine
223  ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
224 
225  // set state validity checking for this space
226  ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
227 
228  // create a start state
230  start->setX(-0.5);
231  start->setY(0.0);
232  start->setYaw(0.0);
233 
234  // create a goal state; use the hard way to set the elements
236  (*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
237  (*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.5;
238  (*goal)[1]->as<ob::SO2StateSpace::StateType>()->value = 0.0;
239 
240 
241  // set the start and goal states
242  ss.setStartAndGoalStates(start, goal, 0.05);
243 
244  // ss.setPlanner(ob::PlannerPtr(new oc::PDST(ss.getSpaceInformation())));
245  // ss.getSpaceInformation()->setMinMaxControlDuration(1,100);
246  // attempt to solve the problem within one second of planning time
247  ob::PlannerStatus solved = ss.solve(10.0);
248 
249  if (solved)
250  {
251  std::cout << "Found solution:" << std::endl;
252  // print the path to screen
253 
254  ss.getSolutionPath().printAsMatrix(std::cout);
255  }
256  else
257  std::cout << "No solution found" << std::endl;
258 }
259 
260 int main(int, char **)
261 {
262  std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
263 
264  // plan();
265  //
266  // std::cout << std::endl << std::endl;
267  //
268  planWithSimpleSetup();
269 
270  return 0;
271 }
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:72
Definition of a scoped state.
Definition: ScopedState.h:56
Definition of an abstract control.
Definition: Control.h:48
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::base::StateSampler.
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:64
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:549
A GridDecomposition is a Decomposition implemented using a grid.
State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
A boost shared pointer wrapper for ompl::control::ControlSpace.
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.
A boost shared pointer wrapper for ompl::base::Planner.
A control space representing Rn.
A state space representing SE(2)
Definition: SE2StateSpace.h:50
SyclopEST is Syclop with EST as its low-level tree planner.
Definition: SyclopEST.h:51
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
Definition of an abstract state.
Definition: State.h:50
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
A boost shared pointer wrapper for ompl::control::SpaceInformation.
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...
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
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.
A boost shared pointer wrapper for ompl::control::Decomposition.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
Space information containing necessary information for planning with controls. setup() needs to be ca...
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:109
A boost shared pointer wrapper for ompl::base::Path.