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_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 
200 int 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 }
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
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: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
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
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
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66