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 }