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