RigidBodyPlanning.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/base/SpaceInformation.h>
38 #include <ompl/base/spaces/SE3StateSpace.h>
39 #include <ompl/geometric/planners/rrt/RRTConnect.h>
40 #include <ompl/geometric/SimpleSetup.h>
41 
42 #include <ompl/config.h>
43 #include <iostream>
44 
45 namespace ob = ompl::base;
46 namespace og = ompl::geometric;
47 
48 bool isStateValid(const ob::State *state)
49 {
50  // cast the abstract state type to the type we expect
51  const auto *se3state = state->as<ob::SE3StateSpace::StateType>();
52 
53  // extract the first component of the state and cast it to what we expect
54  const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
55 
56  // extract the second component of the state and cast it to what we expect
57  const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
58 
59  // check validity of state defined by pos & rot
60 
61 
62  // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
63  return (const void*)rot != (const void*)pos;
64 }
65 
66 void plan()
67 {
68  // construct the state space we are planning in
69  auto space(std::make_shared<ob::SE3StateSpace>());
70 
71  // set the bounds for the R^3 part of SE(3)
72  ob::RealVectorBounds bounds(3);
73  bounds.setLow(-1);
74  bounds.setHigh(1);
75 
76  space->setBounds(bounds);
77 
78  // construct an instance of space information from this state space
79  auto si(std::make_shared<ob::SpaceInformation>(space));
80 
81  // set state validity checking for this space
82  si->setStateValidityChecker(isStateValid);
83 
84  // create a random start state
85  ob::ScopedState<> start(space);
86  start.random();
87 
88  // create a random goal state
89  ob::ScopedState<> goal(space);
90  goal.random();
91 
92  // create a problem instance
93  auto pdef(std::make_shared<ob::ProblemDefinition>(si));
94 
95  // set the start and goal states
96  pdef->setStartAndGoalStates(start, goal);
97 
98  // create a planner for the defined space
99  auto planner(std::make_shared<og::RRTConnect>(si));
100 
101  // set the problem we are trying to solve for the planner
102  planner->setProblemDefinition(pdef);
103 
104  // perform setup steps for the planner
105  planner->setup();
106 
107 
108  // print the settings for this space
109  si->printSettings(std::cout);
110 
111  // print the problem settings
112  pdef->print(std::cout);
113 
114  // attempt to solve the problem within one second of planning time
115  ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);
116 
117  if (solved)
118  {
119  // get the goal representation from the problem definition (not the same as the goal state)
120  // and inquire about the found path
121  ob::PathPtr path = pdef->getSolutionPath();
122  std::cout << "Found solution:" << std::endl;
123 
124  // print the path to screen
125  path->print(std::cout);
126  }
127  else
128  std::cout << "No solution found" << std::endl;
129 }
130 
131 void planWithSimpleSetup()
132 {
133  // construct the state space we are planning in
134  auto space(std::make_shared<ob::SE3StateSpace>());
135 
136  // set the bounds for the R^3 part of SE(3)
137  ob::RealVectorBounds bounds(3);
138  bounds.setLow(-1);
139  bounds.setHigh(1);
140 
141  space->setBounds(bounds);
142 
143  // define a simple setup class
144  og::SimpleSetup ss(space);
145 
146  // set state validity checking for this space
147  ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });
148 
149  // create a random start state
150  ob::ScopedState<> start(space);
151  start.random();
152 
153  // create a random goal state
154  ob::ScopedState<> goal(space);
155  goal.random();
156 
157  // set the start and goal states
158  ss.setStartAndGoalStates(start, goal);
159 
160  // this call is optional, but we put it in to get more output information
161  ss.setup();
162  ss.print();
163 
164  // attempt to solve the problem within one second of planning time
165  ob::PlannerStatus solved = ss.solve(1.0);
166 
167  if (solved)
168  {
169  std::cout << "Found solution:" << std::endl;
170  // print the path to screen
171  ss.simplifySolution();
172  ss.getSolutionPath().print(std::cout);
173  }
174  else
175  std::cout << "No solution found" << std::endl;
176 }
177 
178 int main(int /*argc*/, char ** /*argv*/)
179 {
180  std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
181 
182  plan();
183 
184  std::cout << std::endl << std::endl;
185 
186  planWithSimpleSetup();
187 
188  return 0;
189 }
Definition of a scoped state.
Definition: ScopedState.h:56
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
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
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
The lower and upper bounds for an Rn space.
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:95
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:47
A shared pointer wrapper for ompl::base::Path.