demos/HypercubeBenchmark.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, 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: Bryant Gipson, Mark Moll */
00036 
00037 #include <ompl/base/spaces/RealVectorStateSpace.h>
00038 #include <ompl/geometric/planners/rrt/RRT.h>
00039 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
00040 #include <ompl/geometric/planners/est/EST.h>
00041 #include <ompl/geometric/planners/prm/PRM.h>
00042 #include <ompl/geometric/planners/stride/STRIDE.h>
00043 #include <ompl/tools/benchmark/Benchmark.h>
00044 
00045 #include <boost/math/constants/constants.hpp>
00046 #include <boost/format.hpp>
00047 #include <fstream>
00048 
00049 unsigned ndim = 6;
00050 const double edgeWidth = 0.1;
00051 
00052 // Only states near some edges of a hypercube are valid. The valid edges form a
00053 // narrow passage from (0,...,0) to (1,...,1). A state s is valid if there exists
00054 // a k s.t. (a) 0<=s[k]<=1, (b) for all i<k s[i]<=edgeWidth, and (c) for all i>k
00055 // s[i]>=1-edgewidth.
00056 bool isStateValid(const ompl::base::State *state)
00057 {
00058     const ompl::base::RealVectorStateSpace::StateType *s
00059         = static_cast<const ompl::base::RealVectorStateSpace::StateType*>(state);
00060     bool foundMaxDim = false;
00061 
00062     for (int i = ndim - 1; i >= 0; i--)
00063         if (!foundMaxDim)
00064         {
00065             if ((*s)[i] > edgeWidth)
00066                 foundMaxDim = true;
00067         }
00068         else if ((*s)[i] < (1. - edgeWidth))
00069             return false;
00070         return true;
00071 }
00072 
00073 void addPlanner(ompl::tools::Benchmark& benchmark, ompl::base::PlannerPtr planner, double range)
00074 {
00075     ompl::base::ParamSet& params = planner->params();
00076     if (params.hasParam(std::string("range")))
00077         params.setParam(std::string("range"), boost::lexical_cast<std::string>(range));
00078     benchmark.addPlanner(planner);
00079 }
00080 
00081 int main(int argc, char **argv)
00082 {
00083     if(argc > 1)
00084         ndim = boost::lexical_cast<size_t>(argv[1]);
00085 
00086     double range = edgeWidth * 0.5;
00087     ompl::base::StateSpacePtr space(new ompl::base::RealVectorStateSpace(ndim));
00088     ompl::base::RealVectorBounds bounds(ndim);
00089     ompl::geometric::SimpleSetup ss(space);
00090     ompl::base::ScopedState<> start(space), goal(space);
00091 
00092     bounds.setLow(0.);
00093     bounds.setHigh(1.);
00094     space->as<ompl::base::RealVectorStateSpace>()->setBounds(bounds);
00095     ss.setStateValidityChecker(&isStateValid);
00096     ss.getSpaceInformation()->setStateValidityCheckingResolution(0.001);
00097     for(unsigned int i = 0; i < ndim; ++i)
00098     {
00099         start[i] = 0.;
00100         goal[i] = 1.;
00101     }
00102     ss.setStartAndGoalStates(start, goal);
00103 
00104     // by default, use the Benchmark class
00105     double runtime_limit = 1000, memory_limit = 4096;
00106     int run_count = 20;
00107     ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
00108     ompl::tools::Benchmark b(ss, "HyperCube");
00109 
00110     addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())), range);
00111     addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::EST(ss.getSpaceInformation())), range);
00112     addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::KPIECE1(ss.getSpaceInformation())), range);
00113     addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::RRT(ss.getSpaceInformation())), range);
00114     addPlanner(b, ompl::base::PlannerPtr(new ompl::geometric::PRM(ss.getSpaceInformation())), range);
00115     b.benchmark(request);
00116     b.saveResultsToFile(boost::str(boost::format("hypercube_%i.log") % ndim).c_str());
00117 
00118     exit(0);
00119 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines