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 
48 namespace ob = ompl::base;
49 namespace og = ompl::geometric;
50 
51 bool 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 
69 void 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
124 ob::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 
133 void 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_search(graph, start,
174  [&goal, &opt, &vertices](ob::PlannerData::Graph::Vertex v1) { return distanceHeuristic(v1, &goal, &opt, vertices); },
175  boost::predecessor_map(prev).
176  distance_compare([&opt](ob::Cost c1, ob::Cost c2) { return opt.isCostBetterThan(c1, c2); }).
177  distance_combine([&opt](ob::Cost c1, ob::Cost c2) { return opt.combineCosts(c1, c2); }).
178  distance_inf(opt.infiniteCost()).
179  distance_zero(opt.identityCost()));
180 
181  // Extracting the path
182  og::PathGeometric path(si);
183  for (ob::PlannerData::Graph::Vertex pos = boost::vertex(data.getGoalIndex(0), graph);
184  prev[pos] != pos;
185  pos = prev[pos])
186  {
187  path.append(vertices[pos]->getState());
188  }
189  path.append(vertices[start]->getState());
190  path.reverse();
191 
192  // print the path to screen
193  //path.print(std::cout);
194  std::cout << "Found stored solution with " << path.getStateCount() << " states and length " << path.length() << std::endl;
195  }
196 }
197 
198 int main(int /*argc*/, char ** /*argv*/)
199 {
200  // Plan and save all of the planner data to disk
201  planWithSimpleSetup();
202 
203  // Read in the saved planner data and extract the solution path
204  readPlannerData();
205 
206  return 0;
207 }
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
double length() const override
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
Definition of a scoped state.
Definition: ScopedState.h:56
Definition of a goal state.
Definition: GoalState.h:48
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
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 ...
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...
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
The planner found an exact solution.
Definition: PlannerStatus.h:66
An optimization objective which corresponds to optimizing path length.
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
Abstract definition of optimization objectives.
The lower and upper bounds for an Rn space.
Definition of a geometric path.
Definition: PathGeometric.h:60
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:95
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:47
virtual void store(const PlannerData &pd, const char *filename)
Store (serialize) the PlannerData structure to the given filename.