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 
75 namespace ob = ompl::base;
76 namespace oc = ompl::control;
77 namespace ot = ompl::tools;
78 namespace po = boost::program_options;
79 
80 void writeParams(std::ostream& out)
81 {
82  out << sideLength << ' ' << shipRadius << ' ' << kouleRadius << ' ' << ' '
83  << propagationStepSize << ' ' << shipAcceleration << ' ' << shipRotVel << ' '
84  << shipDelta << ' ' << shipEps << std::endl;
85 }
86 
87 void 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 
129 void 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 
145 int 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, rrt, or est)")
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") || 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.size() == 0)
184  {
185  std::string prefix(vm.count("plan") ? "koules_" : "koulesBenchmark_");
186  outputFile = boost::str(boost::format("%1%%2%_%3%_%4%.dat")
187  % prefix % numKoules % plannerName % maxTime);
188  }
189  if (vm.count("plan"))
190  plan(ks, maxTime, outputFile);
191  else if (vm.count("benchmark"))
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:174
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
Definition of a control path.
Definition: PathControl.h:60
Includes various tools such as self config, benchmarking, etc.
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
#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...
Definition: Cost.h:44
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68