Koules.cpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2013, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
 
/* Author: Beck Chen, Mark Moll */
 
#include "KoulesConfig.h"
#include "KoulesSetup.h"
#include "KoulesStateSpace.h"
#include <ompl/tools/benchmark/Benchmark.h>
#include <ompl/config.h>
#include <boost/program_options.hpp>
#include <boost/format.hpp>
#include <fstream>
 
namespace ob = ompl::base;
namespace oc = ompl::control;
namespace ot = ompl::tools;
namespace po = boost::program_options;
 
void writeParams(std::ostream& out)
{
    out << sideLength << ' ' << shipRadius << ' ' << kouleRadius << ' ' << ' '
        << propagationStepSize << ' ' << shipAcceleration << ' ' << shipRotVel << ' '
        << shipDelta << ' ' << shipEps << std::endl;
}
 
void plan(KoulesSetup& ks, double maxTime, const std::string& outputFile)
{
    if (ks.solve(maxTime))
    {
        std::ofstream out(outputFile.c_str());
        oc::PathControl path(ks.getSolutionPath());
        path.interpolate();
        if (!path.check())
            OMPL_ERROR("Path is invalid");
        writeParams(out);
        path.printAsMatrix(out);
        if (!ks.haveExactSolutionPath())
            OMPL_INFORM("Solution is approximate. Distance to actual goal is %g",
                ks.getProblemDefinition()->getSolutionDifference());
        OMPL_INFORM("Output saved in %s", outputFile.c_str());
    }
 
#if 0
    // Get the planner data, save the ship's (x,y) coordinates to one file and
    // the edge information to another file. This can be used for debugging
    // purposes; plotting the tree of states might give you some idea of
    // a planner's strategy.
    ob::PlannerData pd(ks.getSpaceInformation());
    ks.getPlannerData(pd);
    std::ofstream vertexFile((outputFile + "-vertices").c_str()), edgeFile((outputFile + "-edges").c_str());
    double* coords;
    unsigned numVerts = pd.numVertices();
    std::vector<unsigned int> edgeList;
 
    for (unsigned int i = 0; i < numVerts; ++i)
    {
        coords = pd.getVertex(i).getState()->as<KoulesStateSpace::StateType>()->values;
        vertexFile << coords[0] << ' ' << coords[1] << '\n';
 
        pd.getEdges(i, edgeList);
        for (unsigned int j = 0; j < edgeList.size(); ++j)
            edgeFile << i << ' ' << edgeList[j] << '\n';
    }
#endif
}
 
 
void benchmark(KoulesSetup& ks, ot::Benchmark::Request request,
    const std::string& plannerName, const std::string& outputFile)
{
    // Create a benchmark class
    ompl::tools::Benchmark b(ks, "Koules");
    b.addExperimentParameter("num_koules", "INTEGER", std::to_string(
        (ks.getStateSpace()->getDimension() - 5) / 4));
    // Add the planner to evaluate
    b.addPlanner(ks.getConfiguredPlannerInstance(plannerName));
    // Start benchmark
    b.benchmark(request);
    // Save the results
    b.saveResultsToFile(outputFile.c_str());
    OMPL_INFORM("Output saved in %s", outputFile.c_str());
}
 
int main(int argc, char **argv)
{
    try
    {
        unsigned int numKoules, numRuns;
        double maxTime, kouleVel;
        std::string plannerName, outputFile;
        po::options_description desc("Options");
        desc.add_options()
            ("help", "show help message")
            ("plan", "solve the game of koules")
            ("benchmark", "benchmark the game of koules")
            ("numkoules", po::value<unsigned int>(&numKoules)->default_value(3),
                "start from <numkoules> koules")
            ("maxtime", po::value<double>(&maxTime)->default_value(10.),
                "time limit in seconds")
            ("output", po::value<std::string>(&outputFile), "output file name")
            ("numruns", po::value<unsigned int>(&numRuns)->default_value(10),
                "number of runs for each planner in benchmarking mode")
            ("planner", po::value<std::string>(&plannerName)->default_value("kpiece"),
                "planning algorithm to use (pdst, kpiece, sst, rrt, est, sycloprrt, or syclopest)")
            ("velocity", po::value<double>(&kouleVel)->default_value(0.),
                "initial velocity of each koule")
        ;
 
        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc,
            po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
        po::notify(vm);
 
        KoulesSetup ks(numKoules, plannerName, kouleVel);
        if ((vm.count("help") != 0u) || argc == 1)
        {
            std::cout << "Solve the games of Koules.\nSelect one of these two options:\n"
                      << "\"--plan\", or \"--benchmark\"\n\n" << desc << "\n";
            return 1;
        }
 
        if (outputFile.empty())
        {
            std::string prefix(vm.count("plan") != 0u ? "koules_" : "koulesBenchmark_");
            outputFile = boost::str(boost::format("%1%%2%_%3%_%4%.dat")
                % prefix % numKoules % plannerName % maxTime);
        }
        if (vm.count("plan") != 0u)
            plan(ks, maxTime, outputFile);
        else if (vm.count("benchmark") != 0u)
            benchmark(ks, ot::Benchmark::Request(maxTime, 10000.0, numRuns),
                plannerName, outputFile);
    }
    catch(std::exception& e) {
        std::cerr << "Error: " << e.what() << "\n";
        return 1;
    }
    catch(...) {
        std::cerr << "Exception of unknown type!\n";
    }
 
    return 0;
}