All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 */
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/geometric/SimpleSetup.h>
00042 
00043 #include <boost/graph/astar_search.hpp>
00044 #include <iostream>
00045 
00046 namespace ob = ompl::base;
00047 namespace og = ompl::geometric;
00048 
00049 bool isStateValid(const ob::State *state)
00050 {
00051     // cast the abstract state type to the type we expect
00052     const ob::SE3StateSpace::StateType *se3state = state->as<ob::SE3StateSpace::StateType>();
00053 
00054     // extract the first component of the state and cast it to what we expect
00055     const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
00056 
00057     // extract the second component of the state and cast it to what we expect
00058     const ob::SO3StateSpace::StateType *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
00059 
00060     // check validity of state defined by pos & rot
00061 
00062 
00063     // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
00064     return (void*)rot != (void*)pos;
00065 }
00066 
00067 void planWithSimpleSetup(void)
00068 {
00069     // construct the state space we are planning in
00070     ob::StateSpacePtr space(new ob::SE3StateSpace());
00071 
00072     // set the bounds for the R^3 part of SE(3)
00073     ob::RealVectorBounds bounds(3);
00074     bounds.setLow(-10);
00075     bounds.setHigh(10);
00076 
00077     space->as<ob::SE3StateSpace>()->setBounds(bounds);
00078 
00079     // define a simple setup class
00080     og::SimpleSetup ss(space);
00081 
00082     // set state validity checking for this space
00083     ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
00084 
00085     // create a random start state
00086     ob::ScopedState<> start(space);
00087     start.random();
00088 
00089     // create a random goal state
00090     ob::ScopedState<> goal(space);
00091     goal.random();
00092 
00093     // set the start and goal states
00094     ss.setStartAndGoalStates(start, goal);
00095 
00096     // this call is optional, but we put it in to get more output information
00097     ss.setup();
00098     ss.print();
00099 
00100     // attempt to find an exact solution within five seconds
00101     if (ss.solve(5.0) == ob::PlannerStatus::EXACT_SOLUTION)
00102     {
00103         og::PathGeometric slnPath = ss.getSolutionPath();
00104 
00105         std::cout << std::endl;
00106         std::cout << "Found solution with " << slnPath.getStateCount() << " states and length " << slnPath.length() << std::endl;
00107         // print the path to screen
00108         //slnPath.print(std::cout);
00109 
00110         std::cout << "Writing PlannerData to file './myPlannerData'" << std::endl;
00111         ob::PlannerData data(ss.getSpaceInformation());
00112         ss.getPlannerData(data);
00113 
00114         ob::PlannerDataStorage dataStorage;
00115         dataStorage.store(data, "myPlannerData");
00116     }
00117     else
00118         std::cout << "No solution found" << std::endl;
00119 }
00120 
00121 // Used for A* search.  Computes the heuristic distance from vertex v1 to the goal
00122 double distanceHeuristic(ob::PlannerData::Graph::Vertex v1, const ob::State* goal, const ob::StateSpacePtr& space,
00123                          const boost::property_map<ob::PlannerData::Graph::Type, vertex_type_t>::type& plannerDataVertices)
00124 {
00125     return space->distance(plannerDataVertices[v1]->getState(), goal);
00126 }
00127 
00128 void readPlannerData(void)
00129 {
00130     std::cout << std::endl;
00131     std::cout << "Reading PlannerData from './myPlannerData'" << std::endl;
00132 
00133     // Recreating the space information from the stored planner data instance
00134     ob::StateSpacePtr space(new ob::SE3StateSpace());
00135     ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
00136 
00137     ob::PlannerDataStorage dataStorage;
00138     ob::PlannerData data(si);
00139 
00140     // Loading an instance of PlannerData from disk.
00141     dataStorage.load("myPlannerData", data);
00142 
00143     // Re-extract the shortest path from the loaded planner data
00144     if (data.numStartVertices() > 0 && data.numGoalVertices() > 0)
00145     {
00146         // Computing the weights of all edges based on the state space distance
00147         // This is not done by default for efficiency
00148         data.computeEdgeWeights();
00149 
00150         // Getting a handle to the raw Boost.Graph data
00151         ob::PlannerData::Graph::Type& graph = data.toBoostGraph();
00152 
00153         // Now we can apply any Boost.Graph algorithm.  How about A*!
00154 
00155         // create a predecessor map to store A* results in
00156         boost::vector_property_map<ob::PlannerData::Graph::Vertex> prev(data.numVertices());
00157 
00158         // Retieve a property map with the PlannerDataVertex object pointers for quick lookup
00159         boost::property_map<ob::PlannerData::Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), graph);
00160 
00161         // Run A* search over our planner data
00162         ob::PlannerData::Graph::Vertex goal  = boost::vertex(data.getGoalIndex(0), graph);
00163         ob::PlannerData::Graph::Vertex start = boost::vertex(data.getStartIndex(0), graph);
00164         boost::astar_search(graph, start,
00165                             boost::bind(&distanceHeuristic, _1, vertices[goal]->getState(), space, vertices),
00166                             boost::predecessor_map(prev));
00167 
00168         // Extracting the path
00169         og::PathGeometric path(si);
00170         for (ob::PlannerData::Graph::Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
00171             path.append(vertices[pos]->getState());
00172         path.append(vertices[start]->getState());
00173         path.reverse();
00174 
00175         // print the path to screen
00176         //path.print(std::cout);
00177         std::cout << "Found stored solution with " << path.getStateCount() << " states and length " << path.length() << std::endl;
00178     }
00179 }
00180 
00181 int main(int, char **)
00182 {
00183     // Plan and save all of the planner data to disk
00184     planWithSimpleSetup();
00185 
00186     // Read in the saved planner data and extract the solution path
00187     readPlannerData();
00188 
00189     return 0;
00190 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines