Loading...
Searching...
No Matches
PlannerData.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2012, 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: Ryan Luna, Luis G. Torres */
36
37#include <ompl/base/PlannerData.h>
38#include <ompl/base/PlannerDataStorage.h>
39#include <ompl/base/PlannerDataGraph.h>
40#include <ompl/base/spaces/SE3StateSpace.h>
41#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
42#include <ompl/geometric/SimpleSetup.h>
43#include <ompl/base/goals/GoalState.h>
44
45#include <boost/graph/astar_search.hpp>
46#include <iostream>
47
48namespace ob = ompl::base;
49namespace og = ompl::geometric;
50
51bool isStateValid(const ob::State *state)
52{
53 // cast the abstract state type to the type we expect
54 const auto *se3state = state->as<ob::SE3StateSpace::StateType>();
55
56 // extract the first component of the state and cast it to what we expect
57 const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
58
59 // extract the second component of the state and cast it to what we expect
60 const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
61
62 // check validity of state defined by pos & rot
63
64
65 // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
66 return (const void*)rot != (const void*)pos;
67}
68
69void planWithSimpleSetup()
70{
71 // construct the state space we are planning in
72 auto space(std::make_shared<ob::SE3StateSpace>());
73
74 // set the bounds for the R^3 part of SE(3)
75 ob::RealVectorBounds bounds(3);
76 bounds.setLow(-10);
77 bounds.setHigh(10);
78
79 space->setBounds(bounds);
80
81 // define a simple setup class
82 og::SimpleSetup ss(space);
83
84 // set state validity checking for this space
85 ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });
86
87 // create a random start state
88 ob::ScopedState<> start(space);
89 start.random();
90
91 // create a random goal state
92 ob::ScopedState<> goal(space);
93 goal.random();
94
95 // set the start and goal states
96 ss.setStartAndGoalStates(start, goal);
97
98 // this call is optional, but we put it in to get more output information
99 ss.setup();
100 ss.print();
101
102 // attempt to find an exact solution within five seconds
103 if (ss.solve(5.0) == ob::PlannerStatus::EXACT_SOLUTION)
104 {
105 og::PathGeometric slnPath = ss.getSolutionPath();
106
107 std::cout << std::endl;
108 std::cout << "Found solution with " << slnPath.getStateCount() << " states and length " << slnPath.length() << std::endl;
109 // print the path to screen
110 //slnPath.print(std::cout);
111
112 std::cout << "Writing PlannerData to file './myPlannerData'" << std::endl;
113 ob::PlannerData data(ss.getSpaceInformation());
114 ss.getPlannerData(data);
115
116 ob::PlannerDataStorage dataStorage;
117 dataStorage.store(data, "myPlannerData");
118 }
119 else
120 std::cout << "No solution found" << std::endl;
121}
122
123// Used for A* search. Computes the heuristic distance from vertex v1 to the goal
124ob::Cost distanceHeuristic(ob::PlannerData::Graph::Vertex v1,
125 const ob::GoalState* goal,
126 const ob::OptimizationObjective* obj,
127 const boost::property_map<ob::PlannerData::Graph::Type,
128 vertex_type_t>::type& plannerDataVertices)
129{
130 return ob::Cost(obj->costToGo(plannerDataVertices[v1]->getState(), goal));
131}
132
133void readPlannerData()
134{
135 std::cout << std::endl;
136 std::cout << "Reading PlannerData from './myPlannerData'" << std::endl;
137
138 // Recreating the space information from the stored planner data instance
139 auto space(std::make_shared<ob::SE3StateSpace>());
140 auto si(std::make_shared<ob::SpaceInformation>(space));
141
142 ob::PlannerDataStorage dataStorage;
143 ob::PlannerData data(si);
144
145 // Loading an instance of PlannerData from disk.
146 dataStorage.load("myPlannerData", data);
147
148 // Re-extract the shortest path from the loaded planner data
149 if (data.numStartVertices() > 0 && data.numGoalVertices() > 0)
150 {
151 // Create an optimization objective for optimizing path length in A*
153
154 // Computing the weights of all edges based on the state space distance
155 // This is not done by default for efficiency
156 data.computeEdgeWeights(opt);
157
158 // Getting a handle to the raw Boost.Graph data
159 ob::PlannerData::Graph::Type& graph = data.toBoostGraph();
160
161 // Now we can apply any Boost.Graph algorithm. How about A*!
162
163 // create a predecessor map to store A* results in
164 boost::vector_property_map<ob::PlannerData::Graph::Vertex> prev(data.numVertices());
165
166 // Retrieve a property map with the PlannerDataVertex object pointers for quick lookup
167 boost::property_map<ob::PlannerData::Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), graph);
168
169 // Run A* search over our planner data
170 ob::GoalState goal(si);
171 goal.setState(data.getGoalVertex(0).getState());
172 ob::PlannerData::Graph::Vertex start = boost::vertex(data.getStartIndex(0), graph);
173 boost::astar_visitor<boost::null_visitor> dummy_visitor;
174 boost::astar_search(graph, start,
175 [&goal, &opt, &vertices](ob::PlannerData::Graph::Vertex v1) { return distanceHeuristic(v1, &goal, &opt, vertices); },
176 boost::predecessor_map(prev).
177 distance_compare([&opt](ob::Cost c1, ob::Cost c2) { return opt.isCostBetterThan(c1, c2); }).
178 distance_combine([&opt](ob::Cost c1, ob::Cost c2) { return opt.combineCosts(c1, c2); }).
179 distance_inf(opt.infiniteCost()).
180 distance_zero(opt.identityCost()).
181 visitor(dummy_visitor));
182
183 // Extracting the path
184 og::PathGeometric path(si);
185 for (ob::PlannerData::Graph::Vertex pos = boost::vertex(data.getGoalIndex(0), graph);
186 prev[pos] != pos;
187 pos = prev[pos])
188 {
189 path.append(vertices[pos]->getState());
190 }
191 path.append(vertices[start]->getState());
192 path.reverse();
193
194 // print the path to screen
195 //path.print(std::cout);
196 std::cout << "Found stored solution with " << path.getStateCount() << " states and length " << path.length() << std::endl;
197 }
198}
199
200int main(int /*argc*/, char ** /*argv*/)
201{
202 // Plan and save all of the planner data to disk
203 planWithSimpleSetup();
204
205 // Read in the saved planner data and extract the solution path
206 readPlannerData();
207
208 return 0;
209}
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Definition of a goal state.
Definition: GoalState.h:49
Abstract definition of optimization objectives.
Cost costToGo(const State *state, const Goal *goal) const
Uses a cost-to-go heuristic to calculate an admissible estimate of the optimal cost from a given stat...
An optimization objective which corresponds to optimizing path length.
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
virtual void store(const PlannerData &pd, const char *filename)
Store (serialize) the PlannerData structure to the given filename.
virtual void load(const char *filename, PlannerData &pd)
Load the PlannerData structure from the given stream. The StateSpace that was used to store the data ...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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
Definition of a geometric path.
Definition: PathGeometric.h:66
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
double length() const override
Compute the length of a geometric path (sum of lengths of segments that make up the path)
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.
Definition: GeneticSearch.h:48