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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines