<commit_before>/*
 *  Created on: 19 Apr 2014
 *      Author: <NAME>
 *
 *  This code is based on algorithm developed by <NAME>
 *  <NAME>: Robot Trajectory Optimization using Approximate Inference. In Proc. of the Int. Conf. on Machine Learning (ICML 2009), 1049-1056, ACM, 2009.
 *  http://ipvs.informatik.uni-stuttgart.de/mlr/papers/09-toussaint-ICML.pdf
 *  Original code available at http://ipvs.informatik.uni-stuttgart.de/mlr/marc/source-code/index.html
 * 
 * Copyright (c) 2016, University Of Edinburgh 
 * 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  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. 
 *
 */

/** \file AICOsolver.h
 \brief Approximate Inference Control */

#include "aico/AICOsolver.h"

REGISTER_MOTIONSOLVER_TYPE("AICOsolver", exotica::AICOsolver)

namespace exotica
{
void AICOsolver::Instantiate(AICOsolverInitializer& init)
{
    std::string mode = init.SweepMode;
    if (mode == "Forwardly")
        sweepMode = smForwardly;
    else if (mode == "Symmetric")
        sweepMode = smSymmetric;
    else if (mode == "LocalGaussNewton")
        sweepMode = smLocalGaussNewton;
    else if (mode == "LocalGaussNewtonDamped")
        sweepMode = smLocalGaussNewtonDamped;
    else
    {
        throw_named("Unknown sweep mode '" << init.SweepMode << "'");
    }
    max_iterations = init.MaxIterations;
    tolerance = init.Tolerance;
    damping_init = init.Damping;
    useBwdMsg = init.UseBackwardMessage;
}

void AICOsolver::saveCosts(std::string file_name)
{
    std::ofstream myfile;
    myfile.open(file_name.c_str());
    myfile << "Control Task";
    for (int t = 0; t < prob_->getT(); t++)
    {
        myfile << "\n"
               << costControl(t);
        myfile << " " << costTask(t);
    }
    myfile.close();
}

AICOsolver::AICOsolver()
    : damping(0.01), tolerance(1e-2), max_iterations(100), useBwdMsg(false), bwdMsg_v(), bwdMsg_Vinv(), s(), Sinv(), v(), Vinv(), r(), R(), rhat(), b(), Binv(), q(), qhat(), s_old(), Sinv_old(), v_old(), Vinv_old(), r_old(), R_old(), rhat_old(), b_old(), Binv_old(), q_old(), qhat_old(), dampingReference(), cost(0.0), cost_old(0.0), b_step(0.0), Winv(), sweep(0), sweepMode(0), W(), n(0), updateCount(0), damping_init(0.0), q_stat()
{
}

void AICOsolver::getStats(std::vector<SinglePassMeanCoviariance>& q_stat_)
{
    q_stat_ = q_stat;
}

AICOsolver::~AICOsolver()
{
    // If this is not empty, your code is bad and you should feel bad!
    // Whoop whoop whoop whoop ...
}

void AICOsolver::specifyProblem(PlanningProblem_ptr problem)
{
    if (problem->type() != "exotica::UnconstrainedTimeIndexedProblem")
    {
        throw_named("This solver can't use problem of type '" << problem->type() << "'!");
    }
    MotionSolver::specifyProblem(problem);
    prob_ = std::static_pointer_cast<UnconstrainedTimeIndexedProblem>(problem);

    initMessages();
}

void AICOsolver::Solve(Eigen::MatrixXd& solution)
{
    prob_->preupdate();
    prob_->resetCostEvolution(max_iterations + 1);

    Eigen::VectorXd q0 = prob_->applyStartState();
    std::vector<Eigen::VectorXd> q_init = prob_->getInitialTrajectory();

    // If the initial value of the initial trajectory does not equal the start
    // state, assume that no initial guess is provided and fill the trajectory
    // with the start state
    if (!(q0 - q_init[0]).isMuchSmallerThan(1e-6))
    {
        q_init.resize(prob_->getT(), Eigen::VectorXd::Zero(q0.rows()));
        for (int i = 0; i < q_init.size(); i++) q_init[i] = q0;
    }
    else
    {
        HIGHLIGHT("AICO::Solve called with initial trajectory guess");
    }

    prob_->setStartState(q_init[0]);
    prob_->applyStartState();

    // Check if the trajectory length has changed, if so update the messages.
    if (prob_->getT() != lastT) initMessages();

    Timer timer;
    if (debug_) ROS_WARN_STREAM("AICO: Setting up the solver");
    updateCount = 0;
    sweep = -1;
    damping = damping_init;
    double d;
    if (!(prob_->getT() > 0))
    {
        throw_named("Problem has not been initialized properly: T=0!");
    }
    initTrajectory(q_init);
    sweep = 0;
    if (debug_) ROS_WARN_STREAM("AICO: Solving");
    for (int k = 0; k < max_iterations && !(Server::isRos() && !ros::ok()); k++)
    {
        d = step();
        if (d < 0)
        {
            throw_named("Negative step size!");
        }
        if (k && d < tolerance) break;
    }
    Eigen::MatrixXd sol(prob_->getT(), n);
    for (int tt = 0; tt < prob_->getT(); tt++)
    {
        sol.row(tt) = q[tt];
    }
    solution = sol;
    planning_time_ = timer.getDuration();
}<commit_msg>AICO: Only check tolerance (here: between two subsequent joint value sets) after the first step, avoid early exit<commit_after>/*
 *  Created on: 19 Apr 2014
 *      Author: <NAME>
 *
 *  This code is based on algorithm developed by <NAME>
 *  <NAME>: Robot Trajectory Optimization using Approximate Inference. In Proc. of the Int. Conf. on Machine Learning (ICML 2009), 1049-1056, ACM, 2009.
 *  http://ipvs.informatik.uni-stuttgart.de/mlr/papers/09-toussaint-ICML.pdf
 *  Original code available at http://ipvs.informatik.uni-stuttgart.de/mlr/marc/source-code/index.html
 * 
 * Copyright (c) 2016, University Of Edinburgh 
 * 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  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. 
 *
 */

/** \file AICOsolver.h
 \brief Approximate Inference Control */

#include "aico/AICOsolver.h"

REGISTER_MOTIONSOLVER_TYPE("AICOsolver", exotica::AICOsolver)

namespace exotica
{
void AICOsolver::Instantiate(AICOsolverInitializer& init)
{
    std::string mode = init.SweepMode;
    if (mode == "Forwardly")
        sweepMode = smForwardly;
    else if (mode == "Symmetric")
        sweepMode = smSymmetric;
    else if (mode == "LocalGaussNewton")
        sweepMode = smLocalGaussNewton;
    else if (mode == "LocalGaussNewtonDamped")
        sweepMode = smLocalGaussNewtonDamped;
    else
    {
        throw_named("Unknown sweep mode '" << init.SweepMode << "'");
    }
    max_iterations = init.MaxIterations;
    tolerance = init.Tolerance;
    damping_init = init.Damping;
    useBwdMsg = init.UseBackwardMessage;
}

void AICOsolver::saveCosts(std::string file_name)
{
    std::ofstream myfile;
    myfile.open(file_name.c_str());
    myfile << "Control Task";
    for (int t = 0; t < prob_->getT(); t++)
    {
        myfile << "\n"
               << costControl(t);
        myfile << " " << costTask(t);
    }
    myfile.close();
}

AICOsolver::AICOsolver()
    : damping(0.01), tolerance(1e-2), max_iterations(100), useBwdMsg(false), bwdMsg_v(), bwdMsg_Vinv(), s(), Sinv(), v(), Vinv(), r(), R(), rhat(), b(), Binv(), q(), qhat(), s_old(), Sinv_old(), v_old(), Vinv_old(), r_old(), R_old(), rhat_old(), b_old(), Binv_old(), q_old(), qhat_old(), dampingReference(), cost(0.0), cost_old(0.0), b_step(0.0), Winv(), sweep(0), sweepMode(0), W(), n(0), updateCount(0), damping_init(0.0), q_stat()
{
}

void AICOsolver::getStats(std::vector<SinglePassMeanCoviariance>& q_stat_)
{
    q_stat_ = q_stat;
}

AICOsolver::~AICOsolver()
{
    // If this is not empty, your code is bad and you should feel bad!
    // Whoop whoop whoop whoop ...
}

void AICOsolver::specifyProblem(PlanningProblem_ptr problem)
{
    if (problem->type() != "exotica::UnconstrainedTimeIndexedProblem")
    {
        throw_named("This solver can't use problem of type '" << problem->type() << "'!");
    }
    MotionSolver::specifyProblem(problem);
    prob_ = std::static_pointer_cast<UnconstrainedTimeIndexedProblem>(problem);

    initMessages();
}

void AICOsolver::Solve(Eigen::MatrixXd& solution)
{
    prob_->preupdate();
    prob_->resetCostEvolution(max_iterations + 1);

    Eigen::VectorXd q0 = prob_->applyStartState();
    std::vector<Eigen::VectorXd> q_init = prob_->getInitialTrajectory();

    // If the initial value of the initial trajectory does not equal the start
    // state, assume that no initial guess is provided and fill the trajectory
    // with the start state
    if (!(q0 - q_init[0]).isMuchSmallerThan(1e-6))
    {
        q_init.resize(prob_->getT(), Eigen::VectorXd::Zero(q0.rows()));
        for (int i = 0; i < q_init.size(); i++) q_init[i] = q0;
    }
    else
    {
        HIGHLIGHT("AICO::Solve called with initial trajectory guess");
    }

    prob_->setStartState(q_init[0]);
    prob_->applyStartState();

    // Check if the trajectory length has changed, if so update the messages.
    if (prob_->getT() != lastT) initMessages();

    Timer timer;
    if (debug_) ROS_WARN_STREAM("AICO: Setting up the solver");
    updateCount = 0;
    sweep = -1;
    damping = damping_init;
    double d;
    if (!(prob_->getT() > 0))
    {
        throw_named("Problem has not been initialized properly: T=0!");
    }
    initTrajectory(q_init);
    sweep = 0;
    if (debug_) ROS_WARN_STREAM("AICO: Solving");
    for (int k = 0; k < max_iterations && !(Server::isRos() && !ros::ok()); k++)
    {
        d = step();
        if (d < 0)
        {
            throw_named("Negative step size!");
        }
        if (k > 1 && d < tolerance)
        {
            if (debug_) HIGHLIGHT("Satisfied tolerance\tk=" << k << "\td=" << d << "\ttolerance=" << tolerance);
            break;
        }
    }
    Eigen::MatrixXd sol(prob_->getT(), n);
    for (int tt = 0; tt < prob_->getT(); tt++)
    {
        sol.row(tt) = q[tt];
    }
    solution = sol;
    planning_time_ = timer.getDuration();
}
{'fraction_non_alphanumeric': 0.12710417174920713, 'fraction_numerical': 0.010978287387167602, 'mean_word_length': 3.384313725490196, 'pattern_counts': {'":': 0, '<': 73, '<?xml version=': 0, '>': 46, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2355232', 'n_tokens_mistral': 4123, 'n_tokens_neox': 3801, 'n_words': 1175}