demos/Koules/Koules.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Beck Chen, Mark Moll */
00036 
00066 #include "KoulesConfig.h"
00067 #include "KoulesSetup.h"
00068 #include "KoulesStateSpace.h"
00069 #include <ompl/tools/benchmark/Benchmark.h>
00070 #include <ompl/config.h>
00071 #include <boost/program_options.hpp>
00072 #include <boost/format.hpp>
00073 #include <fstream>
00074 
00075 namespace ob = ompl::base;
00076 namespace oc = ompl::control;
00077 namespace ot = ompl::tools;
00078 namespace po = boost::program_options;
00079 
00080 void writeParams(std::ostream& out)
00081 {
00082     out << sideLength << ' ' << shipRadius << ' ' << kouleRadius << ' ' << ' '
00083         << propagationStepSize << ' ' << shipAcceleration << ' ' << shipRotVel << ' '
00084         << shipDelta << ' ' << shipEps << std::endl;
00085 }
00086 
00087 void plan(KoulesSetup& ks, double maxTime, const std::string& outputFile)
00088 {
00089     if (ks.solve(maxTime))
00090     {
00091         std::ofstream out(outputFile.c_str());
00092         oc::PathControl path(ks.getSolutionPath());
00093         path.interpolate();
00094         if (!path.check())
00095             OMPL_ERROR("Path is invalid");
00096         writeParams(out);
00097         path.printAsMatrix(out);
00098         if (!ks.haveExactSolutionPath())
00099             OMPL_INFORM("Solution is approximate. Distance to actual goal is %g",
00100                 ks.getProblemDefinition()->getSolutionDifference());
00101         OMPL_INFORM("Output saved in %s", outputFile.c_str());
00102     }
00103 
00104 #if 0
00105     // Get the planner data, save the ship's (x,y) coordinates to one file and
00106     // the edge information to another file. This can be used for debugging
00107     // purposes; plotting the tree of states might give you some idea of
00108     // a planner's strategy.
00109     ob::PlannerData pd(ks.getSpaceInformation());
00110     ks.getPlannerData(pd);
00111     std::ofstream vertexFile((outputFile + "-vertices").c_str()), edgeFile((outputFile + "-edges").c_str());
00112     double* coords;
00113     unsigned numVerts = pd.numVertices();
00114     std::vector<unsigned int> edgeList;
00115 
00116     for (unsigned int i = 0; i < numVerts; ++i)
00117     {
00118         coords = pd.getVertex(i).getState()->as<KoulesStateSpace::StateType>()->values;
00119         vertexFile << coords[0] << ' ' << coords[1] << '\n';
00120 
00121         pd.getEdges(i, edgeList);
00122         for (unsigned int j = 0; j < edgeList.size(); ++j)
00123             edgeFile << i << ' ' << edgeList[j] << '\n';
00124     }
00125 #endif
00126 }
00127 
00128 
00129 void benchmark(KoulesSetup& ks, ot::Benchmark::Request request,
00130     const std::string& plannerName, const std::string& outputFile)
00131 {
00132     // Create a benchmark class
00133     ompl::tools::Benchmark b(ks, "Koules experiment");
00134     // Add the planner to evaluate
00135     b.addPlanner(ks.getConfiguredPlannerInstance(plannerName));
00136     // Start benchmark
00137     b.benchmark(request);
00138     // Save the results
00139     b.saveResultsToFile(outputFile.c_str());
00140     OMPL_INFORM("Output saved in %s", outputFile.c_str());
00141 }
00142 
00143 int main(int argc, char **argv)
00144 {
00145     try
00146     {
00147         unsigned int numKoules, numRuns;
00148         double maxTime, kouleVel;
00149         std::string plannerName, outputFile;
00150         po::options_description desc("Options");
00151         desc.add_options()
00152             ("help", "show help message")
00153             ("plan", "solve the game of koules")
00154             ("benchmark", "benchmark the game of koules")
00155             ("numkoules", po::value<unsigned int>(&numKoules)->default_value(3),
00156                 "start from <numkoules> koules")
00157             ("maxtime", po::value<double>(&maxTime)->default_value(10.),
00158                 "time limit in seconds")
00159             ("output", po::value<std::string>(&outputFile), "output file name")
00160             ("numruns", po::value<unsigned int>(&numRuns)->default_value(10),
00161                 "number of runs for each planner in benchmarking mode")
00162             ("planner", po::value<std::string>(&plannerName)->default_value("kpiece"),
00163                 "planning algorithm to use (pdst, kpiece, rrt, or est)")
00164             ("velocity", po::value<double>(&kouleVel)->default_value(0.),
00165                 "initial velocity of each koule")
00166         ;
00167 
00168         po::variables_map vm;
00169         po::store(po::parse_command_line(argc, argv, desc,
00170             po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
00171         po::notify(vm);
00172 
00173         KoulesSetup ks(numKoules, plannerName, kouleVel);
00174         if (vm.count("help") || argc == 1)
00175         {
00176             std::cout << "Solve the games of Koules.\nSelect one of these two options:\n"
00177                       << "\"--plan\", or \"--benchmark\"\n\n" << desc << "\n";
00178             return 1;
00179         }
00180 
00181         if (outputFile.size() == 0)
00182         {
00183             std::string prefix(vm.count("plan") ? "koules_" : "koulesBenchmark_");
00184             outputFile = boost::str(boost::format("%1%%2%_%3%_%4%.dat")
00185                 % prefix % numKoules % plannerName % maxTime);
00186         }
00187         if (vm.count("plan"))
00188             plan(ks, maxTime, outputFile);
00189         else if (vm.count("benchmark"))
00190             benchmark(ks, ot::Benchmark::Request(maxTime, 10000.0, numRuns),
00191                 plannerName, outputFile);
00192     }
00193     catch(std::exception& e) {
00194         std::cerr << "Error: " << e.what() << "\n";
00195         return 1;
00196     }
00197     catch(...) {
00198         std::cerr << "Exception of unknown type!\n";
00199     }
00200 
00201     return 0;
00202 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines