Lightning.cpp

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014, JSK, The University of Tokyo.
 *  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 JSK, The University of Tokyo 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: Dave Coleman */


#include "ompl/tools/lightning/Lightning.h"
#include "ompl/tools/lightning/LightningDB.h"

namespace og = ompl::geometric;
namespace ob = ompl::base;
namespace ot = ompl::tools;

ompl::tools::Lightning::Lightning(const base::SpaceInformationPtr &si) :
    ompl::tools::ExperienceSetup(si)
{
    initialize();
}

ompl::tools::Lightning::Lightning(const base::StateSpacePtr &space) :
    ompl::tools::ExperienceSetup(space)
{
    initialize();
}

void ompl::tools::Lightning::initialize()
{
    recallEnabled_ = true;
    scratchEnabled_ = true;

    // Load dynamic time warp
    dtw_.reset(new ot::DynamicTimeWarp(si_));

    // Load the experience database
    experienceDB_.reset(new ompl::tools::LightningDB(si_->getStateSpace()));

    // Load the Retrieve repair database. We do it here so that setRepairPlanner() works
    rrPlanner_ = ob::PlannerPtr(new og::LightningRetrieveRepair(si_, experienceDB_));

    OMPL_INFORM("Lightning Framework initialized.");
}

void ompl::tools::Lightning::setup()
{
    if (!configured_ || !si_->isSetup() || !planner_->isSetup() || !rrPlanner_->isSetup() )
    {
        OMPL_INFORM("Setting up the Lightning Framework");

        if (!configured_)
            OMPL_INFORM("  Setting up because not configured");
        else if(!si_->isSetup())
            OMPL_INFORM("  Setting up because not si->isSetup");
        else if(!planner_->isSetup())
            OMPL_INFORM("  Setting up because not planner->isSetup");
        else if(!rrPlanner_->isSetup())
            OMPL_INFORM("  Setting up because not rrPlanner->isSetup");

        // Setup Space Information if we haven't already done so
        if (!si_->isSetup())
            si_->setup();

        // Setup planning from scratch planner
        if (!planner_)
        {
            if (pa_)
                planner_ = pa_(si_);
            if (!planner_)
            {
                planner_ = tools::SelfConfig::getDefaultPlanner(pdef_->getGoal()); // we could use the repairProblemDef_ here but that isn't setup yet

                OMPL_INFORM("No planner specified. Using default: %s", planner_->getName().c_str() );
            }
        }
        planner_->setProblemDefinition(pdef_);
        if (!planner_->isSetup())
            planner_->setup();

        // Setup planning from experience planner
        rrPlanner_->setProblemDefinition(pdef_);

        if (!rrPlanner_->isSetup())
            rrPlanner_->setup();

        // Create the parallel component for splitting into two threads
        pp_ = ot::ParallelPlanPtr(new ot::ParallelPlan(pdef_) );
        if (!scratchEnabled_ && !recallEnabled_)
        {
            throw Exception("Both planning from scratch and experience have been disabled, unable to plan");
        }
        if (scratchEnabled_)
            pp_->addPlanner(planner_);   // Add the planning from scratch planner if desired
        if (recallEnabled_)
            pp_->addPlanner(rrPlanner_);  // Add the planning from experience planner if desired


        // Check if experience database is already loaded
        if (experienceDB_->isEmpty())
        {
            if (filePath_.empty())
            {
                OMPL_ERROR("No file path has been specified, unable to load experience DB");
            }
            else
            {
                experienceDB_->load(filePath_); // load from file
            }
        }
        else
            OMPL_ERROR("Attempting to load experience database when it is not empty");

        // Set the configured flag
        configured_ = true;
    }
}

void ompl::tools::Lightning::clear(void)
{
    if (planner_)
        planner_->clear();
    if (rrPlanner_)
        rrPlanner_->clear();
    if (pdef_)
        pdef_->clearSolutionPaths();
    if (pp_)
    {
        pp_->clearHybridizationPaths();
    }
}

// we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner termination condition
ompl::base::PlannerStatus ompl::tools::Lightning::solve(const base::PlannerTerminationCondition &ptc)
{
    OMPL_INFORM("Lightning Framework: Starting solve()");

    // Setup again in case it has not been done yet
    setup();

    lastStatus_ = base::PlannerStatus::UNKNOWN;
    time::point start = time::now();

    // Insertion time
    double insertionTime = 0.;

    // Start both threads
    bool hybridize = false;
    lastStatus_ = pp_->solve(ptc, hybridize);

    // Planning time
    planTime_ = time::seconds(time::now() - start);
    stats_.totalPlanningTime_ += planTime_; // used for averaging
    stats_.numProblems_++; // used for averaging

    // Create log
    ExperienceLog log;
    log.planning_time = planTime_;

    if (lastStatus_ == ompl::base::PlannerStatus::TIMEOUT)
    {
        // Skip further processing if absolutely no path is available
        OMPL_ERROR("Lightning Solve: No solution found after %f seconds", planTime_);
        stats_.numSolutionsTimedout_++;

        // Logging
        log.planner = "neither_planner";
        log.result = "timedout";
        log.is_saved = "not_saved";
    }
    else if (!lastStatus_)
    {
        // Skip further processing if absolutely no path is available
        OMPL_ERROR("Lightning Solve: Unknown failure, planner status: %s", lastStatus_.asString().c_str());
        stats_.numSolutionsFailed_++;

        // Logging
        log.planner = "neither_planner";
        log.result = "failed";
        log.is_saved = "not_saved";
    }
    else
    {
        OMPL_INFORM("Lightning Solve: Possible solution found in %f seconds", planTime_);

        // Smooth the result
        simplifySolution(ptc);

        og::PathGeometric solutionPath = getSolutionPath(); // copied so that it is non-const
        OMPL_INFORM("Solution path has %d states and was generated from planner %s", solutionPath.getStateCount(), getSolutionPlannerName().c_str());

        // Logging
        log.planner = getSolutionPlannerName();

        // Do not save if approximate
        if (!haveExactSolutionPath())
        {
            // Logging
            log.result = "not_exact_solution";
            log.is_saved = "not_saved";
            log.approximate = true;

            // Stats
            stats_.numSolutionsApproximate_++;

            // not sure what to do here, use case not tested
            OMPL_INFORM("NOT saving to database because the solution is APPROXIMATE");
        }
        // Use dynamic time warping to see if the repaired path is too similar to the original
        else if (getSolutionPlannerName() == rrPlanner_->getName())
        {
            // Stats
            stats_.numSolutionsFromRecall_++;

            // Logging
            log.result = "from_recall";

            // Make sure solution has at least 2 states
            if (solutionPath.getStateCount() < 2)
            {
                OMPL_INFORM("NOT saving to database because solution is less than 2 states long");
                stats_.numSolutionsTooShort_++;

                // Logging
                log.is_saved = "less_2_states";
                log.too_short = true;
            }
            else
            {
                // Benchmark runtime
                time::point startTime = time::now();

                // Convert the original recalled path to PathGeometric
                ob::PlannerDataPtr chosenRecallPathData = getLightningRetrieveRepairPlanner().getChosenRecallPath();
                og::PathGeometric chosenRecallPath(si_);
                convertPlannerData(chosenRecallPathData, chosenRecallPath);

                // Reverse path2 if necessary so that it matches path1 better
                reversePathIfNecessary(solutionPath, chosenRecallPath);

                double score = dtw_->getPathsScore( solutionPath, chosenRecallPath );
                log.score = score;

                if (score < 4)
                {
                    OMPL_INFORM("NOT saving to database because best solution was from database and is too similar (score %f)", score);

                    // Logging
                    log.insertion_failed = true;
                    log.is_saved = "score_too_similar";
                }
                else
                {
                    OMPL_INFORM("Adding path to database because repaired path is different enough from original recalled path (score %f)", score);

                    // Logging
                    log.insertion_failed = false;
                    log.is_saved = "score_different_enough";

                    // Stats
                    stats_.numSolutionsFromRecallSaved_++;

                    // Save to database
                    double dummyInsertionTime; // unused because does not include scoring function
                    experienceDB_->addPath(solutionPath, dummyInsertionTime);
                }
                insertionTime += time::seconds(time::now() - startTime);
            }
        }
        else
        {
            // Logging
            log.result = "from_scratch";

            // Stats
            stats_.numSolutionsFromScratch_++;

            // Make sure solution has at least 2 states
            if (solutionPath.getStateCount() < 2)
            {
                OMPL_INFORM("NOT saving to database because solution is less than 2 states long");

                // Logging
                log.is_saved = "less_2_states";
                log.too_short = true;

                // Stats
                stats_.numSolutionsTooShort_++;
            }
            else
            {
                OMPL_INFORM("Adding path to database because best solution was not from database");

                // Logging
                log.result = "from_scratch";
                log.is_saved = "saving";

                // Save to database
                experienceDB_->addPath(solutionPath, insertionTime);
            }
        }
    }

    stats_.totalInsertionTime_ += insertionTime; // used for averaging

    // Final log data
    log.insertion_time = insertionTime;
    log.num_vertices = experienceDB_->getStatesCount();
    log.num_edges = 0;
    log.num_connected_components = 0;

    // Flush the log to buffer
    convertLogToString(log);

    return lastStatus_;
}

ompl::base::PlannerStatus ompl::tools::Lightning::solve(double time)
{
    ob::PlannerTerminationCondition ptc = ob::timedPlannerTerminationCondition(time);
    return solve(ptc);
}

bool ompl::tools::Lightning::save()
{
    if (filePath_.empty())
    {
        OMPL_ERROR("No file path has been specified, unable to save experience DB");
        return false;
    }
    return experienceDB_->save(filePath_);
}

bool ompl::tools::Lightning::saveIfChanged()
{
    if (filePath_.empty())
    {
        OMPL_ERROR("No file path has been specified, unable to save experience DB");
        return false;
    }
    return experienceDB_->saveIfChanged(filePath_);
}

void ompl::tools::Lightning::printResultsInfo(std::ostream &out) const
{
    for (std::size_t i = 0; i < pdef_->getSolutionCount(); ++i)
    {
        out << "Solution " << i
            << " | Length: " << pdef_->getSolutions()[i].length_
            << " | Approximate: " << (pdef_->getSolutions()[i].approximate_ ? "true" : "false")
            << " | Planner: " << pdef_->getSolutions()[i].plannerName_
            << std::endl;
    }
}

void ompl::tools::Lightning::print(std::ostream &out) const
{
    if (si_)
    {
        si_->printProperties(out);
        si_->printSettings(out);
    }
    if (planner_)
    {
        planner_->printProperties(out);
        planner_->printSettings(out);
    }
    if (rrPlanner_)
    {
        rrPlanner_->printProperties(out);
        rrPlanner_->printSettings(out);
    }
    if (pdef_)
        pdef_->print(out);
}

void ompl::tools::Lightning::printLogs(std::ostream &out) const
{
    out << "Lightning Framework Logging Results" << std::endl;
    out << "  Solutions Attempted:           " << stats_.numProblems_ << std::endl;
    out << "     Solved from scratch:        " << stats_.numSolutionsFromScratch_ << " (" << stats_.numSolutionsFromScratch_/stats_.numProblems_*100.0 << "%)" << std::endl;
    out << "     Solved from recall:         " << stats_.numSolutionsFromRecall_  << " (" << stats_.numSolutionsFromRecall_/stats_.numProblems_*100.0 << "%)" << std::endl;
    out << "        That were saved:         " << stats_.numSolutionsFromRecallSaved_ << std::endl;
    out << "        That were discarded:     " << stats_.numSolutionsFromRecall_ - stats_.numSolutionsFromRecallSaved_ << std::endl;
    out << "        Less than 2 states:      " << stats_.numSolutionsTooShort_ << std::endl;
    out << "     Failed:                     " << stats_.numSolutionsFailed_ << std::endl;
    out << "     Timedout:                   " << stats_.numSolutionsTimedout_ << std::endl;
    out << "     Approximate:                " << stats_.numSolutionsApproximate_ << std::endl;
    out << "  LightningDb                    " << std::endl;
    out << "     Total paths:                " << experienceDB_->getExperiencesCount() << std::endl;
    out << "     Vertices (states):          " << experienceDB_->getStatesCount() << std::endl;
    out << "     Unsaved solutions:          " << experienceDB_->getNumUnsavedPaths() << std::endl;
    out << "  Average planning time:         " << stats_.getAveragePlanningTime() << std::endl;
    out << "  Average insertion time:        " << stats_.getAverageInsertionTime() << std::endl;
}

std::size_t ompl::tools::Lightning::getExperiencesCount() const
{
    return experienceDB_->getExperiencesCount();
}

void ompl::tools::Lightning::getAllPlannerDatas(std::vector<ob::PlannerDataPtr> &plannerDatas) const
{
    experienceDB_->getAllPlannerDatas(plannerDatas);
}

void ompl::tools::Lightning::convertPlannerData(const ob::PlannerDataPtr plannerData, og::PathGeometric &path)
{
    // Convert the planner data verticies into a vector of states
    for (std::size_t i = 0; i < plannerData->numVertices(); ++i)
        path.append(plannerData->getVertex(i).getState());
}

bool ompl::tools::Lightning::reversePathIfNecessary(og::PathGeometric &path1, og::PathGeometric &path2)
{
    // Reverse path2 if it matches better
    const ob::State* s1 = path1.getState(0);
    const ob::State* s2 = path2.getState(0);
    const ob::State* g1 = path1.getState(path1.getStateCount()-1);
    const ob::State* g2 = path2.getState(path2.getStateCount()-1);

    double regularDistance  = si_->distance(s1, s2) + si_->distance(g1, g2);
    double reversedDistance = si_->distance(s1, g2) + si_->distance(s2, g1);

    // Check if path is reversed from normal [start->goal] direction
    if ( regularDistance > reversedDistance )
    {
        // needs to be reversed
        path2.reverse();
        return true;
    }

    return false;
}