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 experiment");
134  // Add the planner to evaluate
135  b.addPlanner(ks.getConfiguredPlannerInstance(plannerName));
136  // Start benchmark
137  b.benchmark(request);
138  // Save the results
139  b.saveResultsToFile(outputFile.c_str());
140  OMPL_INFORM("Output saved in %s", outputFile.c_str());
141 }
142 
143 int main(int argc, char **argv)
144 {
145  try
146  {
147  unsigned int numKoules, numRuns;
148  double maxTime, kouleVel;
149  std::string plannerName, outputFile;
150  po::options_description desc("Options");
151  desc.add_options()
152  ("help", "show help message")
153  ("plan", "solve the game of koules")
154  ("benchmark", "benchmark the game of koules")
155  ("numkoules", po::value<unsigned int>(&numKoules)->default_value(3),
156  "start from <numkoules> koules")
157  ("maxtime", po::value<double>(&maxTime)->default_value(10.),
158  "time limit in seconds")
159  ("output", po::value<std::string>(&outputFile), "output file name")
160  ("numruns", po::value<unsigned int>(&numRuns)->default_value(10),
161  "number of runs for each planner in benchmarking mode")
162  ("planner", po::value<std::string>(&plannerName)->default_value("kpiece"),
163  "planning algorithm to use (pdst, kpiece, rrt, or est)")
164  ("velocity", po::value<double>(&kouleVel)->default_value(0.),
165  "initial velocity of each koule")
166  ;
167 
168  po::variables_map vm;
169  po::store(po::parse_command_line(argc, argv, desc,
170  po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
171  po::notify(vm);
172 
173  KoulesSetup ks(numKoules, plannerName, kouleVel);
174  if (vm.count("help") || argc == 1)
175  {
176  std::cout << "Solve the games of Koules.\nSelect one of these two options:\n"
177  << "\"--plan\", or \"--benchmark\"\n\n" << desc << "\n";
178  return 1;
179  }
180 
181  if (outputFile.size() == 0)
182  {
183  std::string prefix(vm.count("plan") ? "koules_" : "koulesBenchmark_");
184  outputFile = boost::str(boost::format("%1%%2%_%3%_%4%.dat")
185  % prefix % numKoules % plannerName % maxTime);
186  }
187  if (vm.count("plan"))
188  plan(ks, maxTime, outputFile);
189  else if (vm.count("benchmark"))
190  benchmark(ks, ot::Benchmark::Request(maxTime, 10000.0, numRuns),
191  plannerName, outputFile);
192  }
193  catch(std::exception& e) {
194  std::cerr << "Error: " << e.what() << "\n";
195  return 1;
196  }
197  catch(...) {
198  std::cerr << "Exception of unknown type!\n";
199  }
200 
201  return 0;
202 }
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
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.
Definition: Benchmark.h:45
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