Koules.cpp
Go to the documentation of this file.
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: Beck Chen, Mark Moll */
36
66#include "KoulesConfig.h"
67#include "KoulesSetup.h"
68#include "KoulesStateSpace.h"
69#include <ompl/tools/benchmark/Benchmark.h>
70#include <ompl/config.h>
71#include <boost/program_options.hpp>
72#include <boost/format.hpp>
73#include <fstream>
74
75namespace ob = ompl::base;
76namespace oc = ompl::control;
77namespace ot = ompl::tools;
78namespace po = boost::program_options;
79
80void writeParams(std::ostream& out)
81{
82 out << sideLength << ' ' << shipRadius << ' ' << kouleRadius << ' ' << ' '
83 << propagationStepSize << ' ' << shipAcceleration << ' ' << shipRotVel << ' '
84 << shipDelta << ' ' << shipEps << std::endl;
85}
86
87void plan(KoulesSetup& ks, double maxTime, const std::string& outputFile)
88{
89 if (ks.solve(maxTime))
90 {
91 std::ofstream out(outputFile.c_str());
92 oc::PathControl path(ks.getSolutionPath());
93 path.interpolate();
94 if (!path.check())
95 OMPL_ERROR("Path is invalid");
96 writeParams(out);
97 path.printAsMatrix(out);
98 if (!ks.haveExactSolutionPath())
99 OMPL_INFORM("Solution is approximate. Distance to actual goal is %g",
100 ks.getProblemDefinition()->getSolutionDifference());
101 OMPL_INFORM("Output saved in %s", outputFile.c_str());
102 }
103
104#if 0
105 // Get the planner data, save the ship's (x,y) coordinates to one file and
106 // the edge information to another file. This can be used for debugging
107 // purposes; plotting the tree of states might give you some idea of
108 // a planner's strategy.
109 ob::PlannerData pd(ks.getSpaceInformation());
110 ks.getPlannerData(pd);
111 std::ofstream vertexFile((outputFile + "-vertices").c_str()), edgeFile((outputFile + "-edges").c_str());
112 double* coords;
113 unsigned numVerts = pd.numVertices();
114 std::vector<unsigned int> edgeList;
115
116 for (unsigned int i = 0; i < numVerts; ++i)
117 {
118 coords = pd.getVertex(i).getState()->as<KoulesStateSpace::StateType>()->values;
119 vertexFile << coords[0] << ' ' << coords[1] << '\n';
120
121 pd.getEdges(i, edgeList);
122 for (unsigned int j = 0; j < edgeList.size(); ++j)
123 edgeFile << i << ' ' << edgeList[j] << '\n';
124 }
125#endif
126}
127
128
129void benchmark(KoulesSetup& ks, ot::Benchmark::Request request,
130 const std::string& plannerName, const std::string& outputFile)
131{
132 // Create a benchmark class
133 ompl::tools::Benchmark b(ks, "Koules");
134 b.addExperimentParameter("num_koules", "INTEGER", std::to_string(
135 (ks.getStateSpace()->getDimension() - 5) / 4));
136 // Add the planner to evaluate
137 b.addPlanner(ks.getConfiguredPlannerInstance(plannerName));
138 // Start benchmark
139 b.benchmark(request);
140 // Save the results
141 b.saveResultsToFile(outputFile.c_str());
142 OMPL_INFORM("Output saved in %s", outputFile.c_str());
143}
144
145int main(int argc, char **argv)
146{
147 try
148 {
149 unsigned int numKoules, numRuns;
150 double maxTime, kouleVel;
151 std::string plannerName, outputFile;
152 po::options_description desc("Options");
153 desc.add_options()
154 ("help", "show help message")
155 ("plan", "solve the game of koules")
156 ("benchmark", "benchmark the game of koules")
157 ("numkoules", po::value<unsigned int>(&numKoules)->default_value(3),
158 "start from <numkoules> koules")
159 ("maxtime", po::value<double>(&maxTime)->default_value(10.),
160 "time limit in seconds")
161 ("output", po::value<std::string>(&outputFile), "output file name")
162 ("numruns", po::value<unsigned int>(&numRuns)->default_value(10),
163 "number of runs for each planner in benchmarking mode")
164 ("planner", po::value<std::string>(&plannerName)->default_value("kpiece"),
165 "planning algorithm to use (pdst, kpiece, sst, rrt, est, sycloprrt, or syclopest)")
166 ("velocity", po::value<double>(&kouleVel)->default_value(0.),
167 "initial velocity of each koule")
168 ;
169
170 po::variables_map vm;
171 po::store(po::parse_command_line(argc, argv, desc,
172 po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
173 po::notify(vm);
174
175 KoulesSetup ks(numKoules, plannerName, kouleVel);
176 if ((vm.count("help") != 0u) || argc == 1)
177 {
178 std::cout << "Solve the games of Koules.\nSelect one of these two options:\n"
179 << "\"--plan\", or \"--benchmark\"\n\n" << desc << "\n";
180 return 1;
181 }
182
183 if (outputFile.empty())
184 {
185 std::string prefix(vm.count("plan") != 0u ? "koules_" : "koulesBenchmark_");
186 outputFile = boost::str(boost::format("%1%%2%_%3%_%4%.dat")
187 % prefix % numKoules % plannerName % maxTime);
188 }
189 if (vm.count("plan") != 0u)
190 plan(ks, maxTime, outputFile);
191 else if (vm.count("benchmark") != 0u)
192 benchmark(ks, ot::Benchmark::Request(maxTime, 10000.0, numRuns),
193 plannerName, outputFile);
194 }
195 catch(std::exception& e) {
196 std::cerr << "Error: " << e.what() << "\n";
197 return 1;
198 }
199 catch(...) {
200 std::cerr << "Exception of unknown type!\n";
201 }
202
203 return 0;
204}
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Definition of a control path.
Definition: PathControl.h:61
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:49
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:45
Includes various tools such as self config, benchmarking, etc.