CostFunctionAutoDiffDerivativeBase.hpp

/***********************************************************************************
Copyright (c) 2017, Diego Pardo. 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 ETH ZURICH 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 ETH ZURICH 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.
***************************************************************************************/

/*
 * CostFunctionBase.hpp
 *
 *  created on 06.06.2015
 *
 *  Based on the original "CostFunctionBase.hpp" version of
 *
 *  Created on: 26.03.2014
 *      Author: neunertm
 */
#ifndef DOXYGEN_SHOULD_SKIP_THIS

#ifndef COSTFUNCTIONAUTODIFFDERIVATIVEBASE_HPP_
#define COSTFUNCTIONAUTODIFFDERIVATIVEBASE_HPP_

#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/AutoDiff>
#include <iostream>

template <class DIMENSIONS>
class CostFunctionAutoDiffDerivativeBase
{ 
public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    typedef typename DIMENSIONS::state_vector_t state_vector_t;
    typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
    typedef typename DIMENSIONS::state_matrix_t state_matrix_t;
    typedef typename DIMENSIONS::control_vector_t control_vector_t;
    typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
    typedef typename DIMENSIONS::control_matrix_t control_matrix_t;
    typedef typename DIMENSIONS::control_feedback_t control_feedback_t;
    typedef typename DIMENSIONS::scalar_t scalar_t;

    // Generic functor
    template<typename _Scalar>
    struct Functor
    {
        typedef _Scalar Scalar;
        enum {
            InputsAtCompileTime = Eigen::Dynamic,
            ValuesAtCompileTime = Eigen::Dynamic
                              };
        typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
        typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
        typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

        const int m_inputs, m_values;

        Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
        Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

        int inputs() const { return m_inputs; } // number of degree of freedom (= 2*nb_vertices)
        int values() const { return m_values; } // number of energy terms (= nb_vertices + nb_edges)


    };


    // Specialized functor warping the function to be derived
    struct iklike_functor : Functor<double>
    {
        typedef Eigen::AutoDiffScalar<Eigen::Matrix<double,Eigen::Dynamic,1> > ADS;
        typedef Eigen::Matrix<ADS, Eigen::Dynamic, 1> VectorXad;

        // pfirst and plast are the two extremities of the curve
        iklike_functor(const int inputs_size, const int values_size)
            :   Functor<double>(inputs_size,values_size), // and initialize stuff
                m_targetAngles(targetAngles), m_targetLengths(targetLengths), m_beta(beta),
                m_pfirst(pfirst), m_plast(plast)
        {}

        // input = x = {  ..., x_i, y_i, ....}
        // output = fvec = the value of each term
        int operator()(const Eigen::VectorXd &inputs, Eigen::VectorXd &val_of_function)
        {
            //I don't like this :using namespace Eigen;
            Eigen::VectorXd curves(this->inputs()+8); //so the curves are larger than the inputs

            curves.segment(4,this->inputs()) = inputs;
                        //what is with the other values

            Vector2d d(1,0);
            curves.segment<2>(0)                   = m_pfirst - d;
            curves.segment<2>(2)                   = m_pfirst;
            curves.segment<2>(this->inputs()+4)    = m_plast;
            curves.segment<2>(this->inputs()+6)    = m_plast + d;

            ikLikeCosts(curves, m_targetAngles, m_targetLengths, m_beta, fvec);
            return 0;
        }

        // Compute the jacobian into fjac for the current solution x
        int df(const Eigen::VectorXd &x, Eigen::MatrixXd &fjac)
        {
            using namespace Eigen;
            VectorXad curves(this->inputs()+8);

            // Compute the derivatives of each degree of freedom
            // -> grad( x_i ) = (0, ..., 0, 1, 0, ..., 0) ; 1 is in position i
            for(int i=0; i<this->inputs();++i)
                curves(4+i) = ADS(x(i), this->inputs(), i);

            Vector2d d(1,0);
            curves.segment<2>(0)                   = (m_pfirst - d).cast<ADS>();
            curves.segment<2>(2)                   = (m_pfirst).cast<ADS>();
            curves.segment<2>(this->inputs()+4)    = (m_plast).cast<ADS>();
            curves.segment<2>(this->inputs()+6)    = (m_plast + d).cast<ADS>();

            VectorXad v(this->values());

            ikLikeCosts(curves, m_targetAngles, m_targetLengths, m_beta, v);

            // copy the gradient of each energy term into the Jacobian
            for(int i=0; i<this->values();++i)
                fjac.row(i) = v(i).derivatives();

            return 0;
        }

        const Eigen::VectorXd& m_targetAngles;
        const Eigen::VectorXd& m_targetLengths;
        double m_beta;
        Eigen::Vector2d m_pfirst, m_plast;
    };


































// END OF FILE

    // Specialized functor warping the ikLikeCosts function
    struct iklike_functor : Functor<double>
    {
        typedef Eigen::AutoDiffScalar<Eigen::Matrix<Scalar,Eigen::Dynamic,1> > ADS;
        typedef Eigen::Matrix<ADS, Eigen::Dynamic, 1> VectorXad;

        // pfirst and plast are the two extremities of the curve
        iklike_functor(const Eigen::VectorXd& targetAngles, const Eigen::VectorXd& targetLengths, double beta, const Eigen::Vector2d pfirst, const Eigen::Vector2d plast)
            :   Functor<double>(targetAngles.size()*2-4,targetAngles.size()*2-1),
                m_targetAngles(targetAngles), m_targetLengths(targetLengths), m_beta(beta),
                m_pfirst(pfirst), m_plast(plast)
        {}

        // input = x = {  ..., x_i, y_i, ....}
        // output = fvec = the value of each term
        int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec)
        {
            using namespace Eigen;
            VectorXd curves(this->inputs()+8);

            curves.segment(4,this->inputs()) = x;

            Vector2d d(1,0);
            curves.segment<2>(0)                   = m_pfirst - d;
            curves.segment<2>(2)                   = m_pfirst;
            curves.segment<2>(this->inputs()+4)    = m_plast;
            curves.segment<2>(this->inputs()+6)    = m_plast + d;

            ikLikeCosts(curves, m_targetAngles, m_targetLengths, m_beta, fvec);
            return 0;
        }

        // Compute the jacobian into fjac for the current solution x
        int df(const Eigen::VectorXd &x, Eigen::MatrixXd &fjac)
        {
            using namespace Eigen;
            VectorXad curves(this->inputs()+8);

            // Compute the derivatives of each degree of freedom
            // -> grad( x_i ) = (0, ..., 0, 1, 0, ..., 0) ; 1 is in position i
            for(int i=0; i<this->inputs();++i)
                curves(4+i) = ADS(x(i), this->inputs(), i);

            Vector2d d(1,0);
            curves.segment<2>(0)                   = (m_pfirst - d).cast<ADS>();
            curves.segment<2>(2)                   = (m_pfirst).cast<ADS>();
            curves.segment<2>(this->inputs()+4)    = (m_plast).cast<ADS>();
            curves.segment<2>(this->inputs()+6)    = (m_plast + d).cast<ADS>();

            VectorXad v(this->values());

            ikLikeCosts(curves, m_targetAngles, m_targetLengths, m_beta, v);

            // copy the gradient of each energy term into the Jacobian
            for(int i=0; i<this->values();++i)
                fjac.row(i) = v(i).derivatives();

            return 0;
        }

        const Eigen::VectorXd& m_targetAngles;
        const Eigen::VectorXd& m_targetLengths;
        double m_beta;
        Eigen::Vector2d m_pfirst, m_plast;
    };


    CostFunctionAutoDiffDerivativeBase() {};
    virtual ~CostFunctionAutoDiffDerivativeBase() {};

    virtual void setCurrentStateAndControl(const state_vector_t& x, const control_vector_t& u) {
        x_ = x;
        u_ = u;
    }

    virtual void setCurrentTrajectory(const state_vector_array_t & x_traj, const control_vector_array_t & u_traj, const Eigen::VectorXd & h_traj)
    {};

    virtual double evaluate() = 0;

    virtual int getNumberOfDependentVariables() = 0 ;
    /*
    virtual state_vector_t stateDerivative() = 0;
    virtual state_matrix_t stateSecondDerivative() = 0;
    virtual control_vector_t controlDerivative() = 0;
    virtual control_matrix_t controlSecondDerivative() = 0;
    */

    virtual void gradient_xuh(Eigen::VectorXd & f0_gradient) = 0;

protected:
    state_vector_t x_;
    control_vector_t u_;
};


// Computes the energy terms into costs. The first part contains termes related to edge length variation,
// and the second to angle variations.
template<typename Scalar>
void ikLikeCosts(const Eigen::Matrix<Scalar,Eigen::Dynamic,1>& curve, const Eigen::VectorXd& targetAngles, const Eigen::VectorXd& targetLengths, double beta,
                 Eigen::Matrix<Scalar,Eigen::Dynamic,1>& costs)
{
    using namespace Eigen;
    using std::atan2;

    typedef Matrix<Scalar,2,1> Vec2;
    int nb = curve.size()/2;

    costs.setZero();
    for(int k=1;k<nb-1;++k)
    {
        Vec2 pk0 = curve.template segment<2>(2*(k-1));
        Vec2 pk  = curve.template segment<2>(2*k);
        Vec2 pk1 = curve.template segment<2>(2*(k+1));

        if(k+1<nb-1) {
            costs((nb-2)+(k-1)) = (pk1-pk).norm() - targetLengths(k-1);
        }


        Vec2 v0 = (pk-pk0).normalized();
        Vec2 v1 = (pk1-pk).normalized();

        costs(k-1) = beta * (atan2(-v0.y() * v1.x() + v0.x() * v1.y(), v0.x() * v1.x() + v0.y() * v1.y()) - targetAngles(k-1));
    }
}

void draw_vecX(const Eigen::VectorXd& res)
{
  for( int i = 0; i < (res.size()/2) ; i++ )
  {
    std::cout << res.segment<2>(2*i).transpose() << "\n";
  }
}



int main()
{
    Eigen::Vector2d pfirst(-5., 0.);
    Eigen::Vector2d plast ( 5., 0.);

    // rest pose is a straight line starting between first and last point
    const int nb_points = 30;
    Eigen::VectorXd targetAngles (nb_points);
    targetAngles.fill(0);

    Eigen::VectorXd targetLengths(nb_points-1);
    double val = (pfirst-plast).norm() / (double)(nb_points-1);
    targetLengths.fill(val);


    // get initial solution
    Eigen::VectorXd x((nb_points-2)*2);
    for(int i = 1; i < (nb_points - 1); i++)
    {
        double s = (double)i / (double)(nb_points-1);
        x.segment<2>((i-1)*2) = plast * s + pfirst * (1. - s);
    }

    // move last point
    plast = Eigen::Vector2d(4., 1.);

    // Create the functor object
    iklike_functor func(targetAngles, targetLengths, 0.1, pfirst, plast);

    // construct the solver
    Eigen::LevenbergMarquardt<iklike_functor> lm(func);

    // adjust tolerance
    lm.parameters.ftol *= 1e-2;
    lm.parameters.xtol *= 1e-2;
    lm.parameters.maxfev = 2000;


    int a = lm.minimize(x);
    std::cerr << "info = " << a  << " " << lm.nfev << " " << lm.njev << " "  <<  "\n";

    std::cout << pfirst.transpose() << "\n";
    draw_vecX( x);
    std::cout << plast.transpose() << "\n";
}


#endif /* COSTFUNCTIONAUTODIFFDERIVATIVEBASE_HPP_ */
#endif /* DOXYGEN_SHOULD_SKIP_THIS */