KinematicChainBenchmark.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Bryant Gipson, Mark Moll */
36 
37 #include "KinematicChain.h"
38 
39 int main(int argc, char **argv)
40 {
41  if (argc < 2)
42  {
43  std::cout << "Usage:\n" << argv[0] << " <num_links>\n";
44  exit(0);
45  }
46 
47  auto numLinks = boost::lexical_cast<unsigned int>(std::string(argv[1]));
48  Environment env = createHornEnvironment(numLinks, log((double)numLinks) / (double)numLinks);
49  auto chain(std::make_shared<KinematicChainSpace>(numLinks, 1. / (double)numLinks, &env));
51 
52  ss.setStateValidityChecker(std::make_shared<KinematicChainValidityChecker>(ss.getSpaceInformation()));
53 
54  ompl::base::ScopedState<> start(chain), goal(chain);
55  std::vector<double> startVec(numLinks, boost::math::constants::pi<double>() / (double)numLinks);
56  std::vector<double> goalVec(numLinks, 0);
57 
58  startVec[0] = 0.;
59  goalVec[0] = boost::math::constants::pi<double>() - .001;
60  chain->setup();
61  chain->copyFromReals(start.get(), startVec);
62  chain->copyFromReals(goal.get(), goalVec);
63  ss.setStartAndGoalStates(start, goal);
64 
65  // SEKRIT BONUS FEATURE:
66  // if you specify a second command line argument, it will solve the
67  // problem just once with STRIDE and print out the solution path.
68  if (argc > 2)
69  {
70  ss.setPlanner(std::make_shared<ompl::geometric::STRIDE>(ss.getSpaceInformation()));
71  ss.setup();
72  ss.print();
73  ss.solve(3600);
74  ss.simplifySolution();
75 
76  std::ofstream pathfile(boost::str(boost::format("kinematic_path_%i.dat") % numLinks).c_str());
77  ss.getSolutionPath().printAsMatrix(pathfile);
78  exit(0);
79  }
80 
81  // by default, use the Benchmark class
82  double runtime_limit = 60, memory_limit = 1024;
83  int run_count = 20;
84  ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count, 0.5);
85  ompl::tools::Benchmark b(ss, "KinematicChain");
86  b.addExperimentParameter("num_links", "INTEGER", std::to_string(numLinks));
87 
88  b.addPlanner(std::make_shared<ompl::geometric::STRIDE>(ss.getSpaceInformation()));
89  b.addPlanner(std::make_shared<ompl::geometric::EST>(ss.getSpaceInformation()));
90  b.addPlanner(std::make_shared<ompl::geometric::KPIECE1>(ss.getSpaceInformation()));
91  b.addPlanner(std::make_shared<ompl::geometric::RRT>(ss.getSpaceInformation()));
92  b.addPlanner(std::make_shared<ompl::geometric::PRM>(ss.getSpaceInformation()));
93  b.benchmark(request);
94  b.saveResultsToFile(boost::str(boost::format("kinematic_%i.log") % numLinks).c_str());
95 
96  exit(0);
97 }
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
Definition of a scoped state.
Definition: ScopedState.h:56
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
Representation of a benchmark request.
Definition: Benchmark.h:156