demos/PlannerData.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2012, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ryan Luna, Luis G. Torres */ 00036 00037 #include <ompl/base/PlannerData.h> 00038 #include <ompl/base/PlannerDataStorage.h> 00039 #include <ompl/base/PlannerDataGraph.h> 00040 #include <ompl/base/spaces/SE3StateSpace.h> 00041 #include <ompl/base/objectives/PathLengthOptimizationObjective.h> 00042 #include <ompl/geometric/SimpleSetup.h> 00043 #include <ompl/base/goals/GoalState.h> 00044 00045 #include <boost/graph/astar_search.hpp> 00046 #include <iostream> 00047 00048 namespace ob = ompl::base; 00049 namespace og = ompl::geometric; 00050 00051 bool isStateValid(const ob::State *state) 00052 { 00053 // cast the abstract state type to the type we expect 00054 const ob::SE3StateSpace::StateType *se3state = state->as<ob::SE3StateSpace::StateType>(); 00055 00056 // extract the first component of the state and cast it to what we expect 00057 const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0); 00058 00059 // extract the second component of the state and cast it to what we expect 00060 const ob::SO3StateSpace::StateType *rot = se3state->as<ob::SO3StateSpace::StateType>(1); 00061 00062 // check validity of state defined by pos & rot 00063 00064 00065 // return a value that is always true but uses the two variables we define, so we avoid compiler warnings 00066 return (const void*)rot != (const void*)pos; 00067 } 00068 00069 void planWithSimpleSetup(void) 00070 { 00071 // construct the state space we are planning in 00072 ob::StateSpacePtr space(new ob::SE3StateSpace()); 00073 00074 // set the bounds for the R^3 part of SE(3) 00075 ob::RealVectorBounds bounds(3); 00076 bounds.setLow(-10); 00077 bounds.setHigh(10); 00078 00079 space->as<ob::SE3StateSpace>()->setBounds(bounds); 00080 00081 // define a simple setup class 00082 og::SimpleSetup ss(space); 00083 00084 // set state validity checking for this space 00085 ss.setStateValidityChecker(boost::bind(&isStateValid, _1)); 00086 00087 // create a random start state 00088 ob::ScopedState<> start(space); 00089 start.random(); 00090 00091 // create a random goal state 00092 ob::ScopedState<> goal(space); 00093 goal.random(); 00094 00095 // set the start and goal states 00096 ss.setStartAndGoalStates(start, goal); 00097 00098 // this call is optional, but we put it in to get more output information 00099 ss.setup(); 00100 ss.print(); 00101 00102 // attempt to find an exact solution within five seconds 00103 if (ss.solve(5.0) == ob::PlannerStatus::EXACT_SOLUTION) 00104 { 00105 og::PathGeometric slnPath = ss.getSolutionPath(); 00106 00107 std::cout << std::endl; 00108 std::cout << "Found solution with " << slnPath.getStateCount() << " states and length " << slnPath.length() << std::endl; 00109 // print the path to screen 00110 //slnPath.print(std::cout); 00111 00112 std::cout << "Writing PlannerData to file './myPlannerData'" << std::endl; 00113 ob::PlannerData data(ss.getSpaceInformation()); 00114 ss.getPlannerData(data); 00115 00116 ob::PlannerDataStorage dataStorage; 00117 dataStorage.store(data, "myPlannerData"); 00118 } 00119 else 00120 std::cout << "No solution found" << std::endl; 00121 } 00122 00123 // Used for A* search. Computes the heuristic distance from vertex v1 to the goal 00124 ob::Cost distanceHeuristic(ob::PlannerData::Graph::Vertex v1, 00125 const ob::GoalState* goal, 00126 const ob::OptimizationObjective* obj, 00127 const boost::property_map<ob::PlannerData::Graph::Type, 00128 vertex_type_t>::type& plannerDataVertices) 00129 { 00130 return ob::Cost(obj->costToGo(plannerDataVertices[v1]->getState(), goal)); 00131 } 00132 00133 void readPlannerData(void) 00134 { 00135 std::cout << std::endl; 00136 std::cout << "Reading PlannerData from './myPlannerData'" << std::endl; 00137 00138 // Recreating the space information from the stored planner data instance 00139 ob::StateSpacePtr space(new ob::SE3StateSpace()); 00140 ob::SpaceInformationPtr si(new ob::SpaceInformation(space)); 00141 00142 ob::PlannerDataStorage dataStorage; 00143 ob::PlannerData data(si); 00144 00145 // Loading an instance of PlannerData from disk. 00146 dataStorage.load("myPlannerData", data); 00147 00148 // Re-extract the shortest path from the loaded planner data 00149 if (data.numStartVertices() > 0 && data.numGoalVertices() > 0) 00150 { 00151 // Create an optimization objective for optimizing path length in A* 00152 ob::PathLengthOptimizationObjective opt(si); 00153 00154 // Computing the weights of all edges based on the state space distance 00155 // This is not done by default for efficiency 00156 data.computeEdgeWeights(opt); 00157 00158 // Getting a handle to the raw Boost.Graph data 00159 ob::PlannerData::Graph::Type& graph = data.toBoostGraph(); 00160 00161 // Now we can apply any Boost.Graph algorithm. How about A*! 00162 00163 // create a predecessor map to store A* results in 00164 boost::vector_property_map<ob::PlannerData::Graph::Vertex> prev(data.numVertices()); 00165 00166 // Retieve a property map with the PlannerDataVertex object pointers for quick lookup 00167 boost::property_map<ob::PlannerData::Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), graph); 00168 00169 // Run A* search over our planner data 00170 ob::GoalState goal(si); 00171 goal.setState(data.getGoalVertex(0).getState()); 00172 ob::PlannerData::Graph::Vertex start = boost::vertex(data.getStartIndex(0), graph); 00173 boost::astar_search(graph, start, 00174 boost::bind(&distanceHeuristic, _1, &goal, &opt, vertices), 00175 boost::predecessor_map(prev). 00176 distance_compare(boost::bind(&ob::OptimizationObjective:: 00177 isCostBetterThan, &opt, _1, _2)). 00178 distance_combine(boost::bind(&ob::OptimizationObjective:: 00179 combineCosts, &opt, _1, _2)). 00180 distance_inf(opt.infiniteCost()). 00181 distance_zero(opt.identityCost())); 00182 00183 // Extracting the path 00184 og::PathGeometric path(si); 00185 for (ob::PlannerData::Graph::Vertex pos = boost::vertex(data.getGoalIndex(0), graph); 00186 prev[pos] != pos; 00187 pos = prev[pos]) 00188 { 00189 path.append(vertices[pos]->getState()); 00190 } 00191 path.append(vertices[start]->getState()); 00192 path.reverse(); 00193 00194 // print the path to screen 00195 //path.print(std::cout); 00196 std::cout << "Found stored solution with " << path.getStateCount() << " states and length " << path.length() << std::endl; 00197 } 00198 } 00199 00200 int main(int, char **) 00201 { 00202 // Plan and save all of the planner data to disk 00203 planWithSimpleSetup(); 00204 00205 // Read in the saved planner data and extract the solution path 00206 readPlannerData(); 00207 00208 return 0; 00209 }