Loading...
Searching...
No Matches
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
45namespace ob = ompl::base;
46namespace og = ompl::geometric;
47
48bool 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
66void 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
131void 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
178int 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}
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Definition ScopedState.h:57
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
Create the set of classes typically needed to solve a geometric problem.
Definition SimpleSetup.h:63
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
A class to store the exit status of Planner::solve()