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 a 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 }
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:79
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:159
An optimization objective which corresponds to optimizing path length.
Definition of an abstract state.
Definition: State.h:113
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:126
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
Definition of a geometric path.
Definition: PathGeometric.h:97
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
Abstract definition of optimization objectives.
virtual void store(const PlannerData &pd, const char *filename)
Store (serialize) the PlannerData structure to the given filename.
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
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...
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
double length() const override
Compute the length of a geometric path (sum of lengths of segments that make up the path)
@ EXACT_SOLUTION
The planner found an exact solution.
Definition of a goal state.
Definition: GoalState.h:112
Definition of a scoped state.
Definition: ScopedState.h:120
The lower and upper bounds for an Rn space.