Loading...
Searching...
No Matches
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
39int 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 = std::stoul(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}
Definition of a scoped state.
Definition ScopedState.h:57
Create the set of classes typically needed to solve a geometric problem.
Definition SimpleSetup.h:63
Benchmark a set of planners on a problem instance.
Definition Benchmark.h:49
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
Representation of a benchmark request.
Definition Benchmark.h:157