demos/KinematicChainBenchmark.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/SO2StateSpace.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 // a 2D line segment
00050 struct Segment
00051 {
00052     Segment(double p0_x, double p0_y, double p1_x, double p1_y)
00053         : x0(p0_x), y0(p0_y), x1(p1_x), y1(p1_y)
00054     {
00055     }
00056     double x0, y0, x1, y1;
00057 };
00058 
00059 // the robot and environment are modeled both as a vector of segments.
00060 typedef std::vector<Segment> Environment;
00061 
00062 // simply use a random projection
00063 class KinematicChainProjector : public ompl::base::ProjectionEvaluator
00064 {
00065 public:
00066     KinematicChainProjector(const ompl::base::StateSpace *space)
00067         : ompl::base::ProjectionEvaluator(space)
00068     {
00069         int dimension = std::max(2, (int)ceil(log((double) space->getDimension())));
00070         projectionMatrix_.computeRandom(space->getDimension(), dimension);
00071     }
00072     virtual unsigned int getDimension(void) const
00073     {
00074         return projectionMatrix_.mat.size1();
00075     }
00076     void project(const ompl::base::State *state, ompl::base::EuclideanProjection &projection) const
00077     {
00078         std::vector<double> v(space_->getDimension());
00079         space_->copyToReals(v, state);
00080         projectionMatrix_.project(&v[0], projection);
00081     }
00082 protected:
00083     ompl::base::ProjectionMatrix projectionMatrix_;
00084 };
00085 
00086 
00087 class KinematicChainSpace : public ompl::base::CompoundStateSpace
00088 {
00089 public:
00090     KinematicChainSpace(unsigned int numLinks, double linkLength, Environment *env = NULL)
00091         : ompl::base::CompoundStateSpace(), linkLength_(linkLength), environment_(env)
00092     {
00093         for (unsigned int i = 0; i < numLinks; ++i)
00094             addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO2StateSpace()), 1.);
00095         lock();
00096     }
00097 
00098     void registerProjections()
00099     {
00100         registerDefaultProjection(ompl::base::ProjectionEvaluatorPtr(
00101             new KinematicChainProjector(this)));
00102     }
00103 
00104     double distance(const ompl::base::State *state1, const ompl::base::State *state2) const
00105     {
00106         const StateType *cstate1 = state1->as<StateType>();
00107         const StateType *cstate2 = state2->as<StateType>();
00108         double theta1 = 0., theta2 = 0., dx = 0., dy = 0., dist = 0.;
00109 
00110         for (unsigned int i = 0; i < getSubspaceCount(); ++i)
00111         {
00112             theta1 += cstate1->as<ompl::base::SO2StateSpace::StateType>(i)->value;
00113             theta2 += cstate2->as<ompl::base::SO2StateSpace::StateType>(i)->value;
00114             dx += cos(theta1) - cos(theta2);
00115             dy += sin(theta1) - sin(theta2);
00116             dist += sqrt(dx * dx + dy * dy);
00117         }
00118         return dist * linkLength_;
00119     }
00120     double linkLength() const
00121     {
00122         return linkLength_;
00123     }
00124     const Environment* environment() const
00125     {
00126         return environment_;
00127     }
00128 
00129 protected:
00130     double linkLength_;
00131     Environment* environment_;
00132 };
00133 
00134 
00135 class KinematicChainValidityChecker : public ompl::base::StateValidityChecker
00136 {
00137 public:
00138     KinematicChainValidityChecker(const ompl::base::SpaceInformationPtr &si)
00139         : ompl::base::StateValidityChecker(si)
00140     {
00141     }
00142 
00143     bool isValid(const ompl::base::State *state) const
00144     {
00145         const KinematicChainSpace* space = si_->getStateSpace()->as<KinematicChainSpace>();
00146         const KinematicChainSpace::StateType *s = state->as<KinematicChainSpace::StateType>();
00147         unsigned int n = si_->getStateDimension();
00148         Environment segments;
00149         double linkLength = space->linkLength();
00150         double theta = 0., x = 0., y = 0., xN, yN;
00151 
00152         segments.reserve(n + 1);
00153         for(unsigned int i = 0; i < n; ++i)
00154         {
00155             theta += s->as<ompl::base::SO2StateSpace::StateType>(i)->value;
00156             xN = x + cos(theta) * linkLength;
00157             yN = y + sin(theta) * linkLength;
00158             segments.push_back(Segment(x, y, xN, yN));
00159             x = xN;
00160             y = yN;
00161         }
00162         xN = x + cos(theta) * 0.001;
00163         yN = y + sin(theta) * 0.001;
00164         segments.push_back(Segment(x, y, xN, yN));
00165         return selfIntersectionTest(segments)
00166             && environmentIntersectionTest(segments, *space->environment());
00167     }
00168 
00169 protected:
00170     // return true iff env does *not* include a pair of intersecting segments
00171     bool selfIntersectionTest(const Environment& env) const
00172     {
00173         for (unsigned int i = 0; i < env.size(); ++i)
00174             for (unsigned int j = i + 1; j < env.size(); ++j)
00175                 if (intersectionTest(env[i], env[j]))
00176                     return false;
00177         return true;
00178     }
00179     // return true iff no segment in env0 intersects any segment in env1
00180     bool environmentIntersectionTest(const Environment& env0, const Environment& env1) const
00181     {
00182         for (unsigned int i = 0; i < env0.size(); ++i)
00183             for (unsigned int j = 0; j < env1.size(); ++j)
00184                 if (intersectionTest(env0[i], env1[j]))
00185                     return false;
00186         return true;
00187     }
00188     // return true iff segment s0 intersects segment s1
00189     bool intersectionTest(const Segment& s0, const Segment& s1) const
00190     {
00191         // adopted from:
00192         // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect/1201356#1201356
00193         double s10_x = s0.x1 - s0.x0;
00194         double s10_y = s0.y1 - s0.y0;
00195         double s32_x = s1.x1 - s1.x0;
00196         double s32_y = s1.y1 - s1.y0;
00197         double denom = s10_x * s32_y - s32_x * s10_y;
00198         if (fabs(denom) < std::numeric_limits<double>::epsilon())
00199             return false; // Collinear
00200         bool denomPositive = denom > 0;
00201 
00202         double s02_x = s0.x0 - s1.x0;
00203         double s02_y = s0.y0 - s1.y0;
00204         double s_numer = s10_x * s02_y - s10_y * s02_x;
00205         if ((s_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
00206             return false; // No collision
00207         double t_numer = s32_x * s02_y - s32_y * s02_x;
00208         if ((t_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
00209             return false; // No collision
00210         if (((s_numer - denom > -std::numeric_limits<float>::epsilon()) == denomPositive)
00211             || ((t_numer - denom > std::numeric_limits<float>::epsilon()) == denomPositive))
00212             return false; // No collision
00213         return true;
00214     }
00215 };
00216 
00217 
00218 Environment createHornEnvironment(unsigned int d, double eps)
00219 {
00220     std::ofstream envFile("environment.dat");
00221     std::vector<Segment> env;
00222     double w = 1. / (double)d, x = w, y = -eps, xN, yN, theta = 0.,
00223         scale = w * (1. + boost::math::constants::pi<double>() * eps);
00224 
00225     envFile << x << " " << y << std::endl;
00226     for(unsigned int i = 0; i < d - 1; ++i)
00227     {
00228         theta += boost::math::constants::pi<double>() / (double) d;
00229         xN = x + cos(theta) * scale;
00230         yN = y + sin(theta) * scale;
00231         env.push_back(Segment(x, y, xN, yN));
00232         x = xN;
00233         y = yN;
00234         envFile << x << " " << y << std::endl;
00235     }
00236 
00237     theta = 0.;
00238     x = w;
00239     y = eps;
00240     envFile << x << " " << y << std::endl;
00241     scale = w * (1.0 - boost::math::constants::pi<double>() * eps);
00242     for(unsigned int i = 0; i < d - 1; ++i)
00243     {
00244         theta += boost::math::constants::pi<double>() / d;
00245         xN = x + cos(theta) * scale;
00246         yN = y + sin(theta) * scale;
00247         env.push_back(Segment(x, y, xN, yN));
00248         x = xN;
00249         y = yN;
00250         envFile << x << " " << y << std::endl;
00251     }
00252     envFile.close();
00253     return env;
00254 }
00255 
00256 
00257 int main(int argc, char **argv)
00258 {
00259     if (argc < 2)
00260     {
00261         std::cout << "Usage:\n" << argv[0] << " <num_links>\n";
00262         exit(0);
00263     }
00264 
00265     unsigned int numLinks = boost::lexical_cast<unsigned int>(std::string(argv[1]));
00266     Environment env = createHornEnvironment(numLinks, log((double)numLinks) / (double)numLinks);
00267     ompl::base::StateSpacePtr chain(new KinematicChainSpace(numLinks, 1. / (double)numLinks, &env));
00268     ompl::geometric::SimpleSetup ss(chain);
00269 
00270     ss.setStateValidityChecker(ompl::base::StateValidityCheckerPtr(
00271         new KinematicChainValidityChecker(ss.getSpaceInformation())));
00272 
00273     ompl::base::ScopedState<> start(chain), goal(chain);
00274     std::vector<double> startVec(numLinks, boost::math::constants::pi<double>() / (double)numLinks);
00275     std::vector<double> goalVec(numLinks, 0.);
00276 
00277     startVec[0] = 0.;
00278     goalVec[0] = boost::math::constants::pi<double>() - .001;
00279     chain->setup();
00280     chain->copyFromReals(start.get(), startVec);
00281     chain->copyFromReals(goal.get(), goalVec);
00282     ss.setStartAndGoalStates(start, goal);
00283 
00284     // SEKRIT BONUS FEATURE:
00285     // if you specify a second command line argument, it will solve the
00286     // problem just once with STRIDE and print out the solution path.
00287     if (argc > 2)
00288     {
00289         ss.setPlanner(ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())));
00290         ss.setup();
00291         ss.print();
00292         ss.solve(3600);
00293         ss.simplifySolution();
00294 
00295         ompl::geometric::PathGeometric path = ss.getSolutionPath();
00296         std::vector<double> v;
00297         for(unsigned int i = 0; i < path.getStateCount(); ++i)
00298         {
00299             chain->copyToReals(v, path.getState(i));
00300             std::copy(v.begin(), v.end(), std::ostream_iterator<double>(std::cout, " "));
00301             std::cout << std::endl;
00302         }
00303         exit(0);
00304     }
00305 
00306     // by default, use the Benchmark class
00307     double runtime_limit = 60, memory_limit = 1024;
00308     int run_count = 20;
00309     ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count, 0.5);
00310     ompl::tools::Benchmark b(ss, boost::str(boost::format("KinematicChain%i") % numLinks));
00311 
00312     b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())));
00313     b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::EST(ss.getSpaceInformation())));
00314     b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::KPIECE1(ss.getSpaceInformation())));
00315     b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::RRT(ss.getSpaceInformation())));
00316     b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::PRM(ss.getSpaceInformation())));
00317     b.benchmark(request);
00318     b.saveResultsToFile(boost::str(boost::format("kinematic_%i.log") % numLinks).c_str());
00319 
00320     exit(0);
00321 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines