Bipedal-locomotion-framework: Investigate the possibility to use Automatic Differentiation in the Framework

[GiulioRomualdi]

Related issues: https://github.com/stack-of-tasks/pinocchio/pull/813 https://github.com/stack-of-tasks/pinocchio/issues/437 https://github.com/stack-of-tasks/pinocchio/issues/809

cc @traversaro @S-Dafarra

[GiulioRomualdi]

Today I tried to do test the CppAD in a simple toy problem with Eigen.

# include <cppad/cppad.hpp>
# include <cppad/example/cppad_eigen.hpp> // <--------- Required for Numtraits
# include <Eigen/Dense>


template<typename scalar>
using Vector = Eigen::Matrix<scalar, Eigen::Dynamic, 1>;

template<typename scalar>
using Matrix = Eigen::Matrix<scalar, Eigen::Dynamic, Eigen::Dynamic>;

using ADdouble = CppAD::AD<double>;

template <typename scalarVector, typename scalarMatrix>
class Foo
{
    Vector<scalarVector> x;
    Matrix<scalarMatrix> A;

public:

    void setA(const Matrix<scalarMatrix> A)
    {
        this->A = A;
    }

    void setX(const Eigen::Ref<const Vector<scalarVector>>& x)
    {
        this->x = x;
    }

    Vector<scalarVector> getMultiplication()
    {
        return A  * x;
    }
};

int main()
{

    constexpr int cols = 5;
    constexpr int rows = 5;

    Vector<ADdouble> x = Vector<ADdouble>::Ones(cols);
    CppAD::Independent(x);

    Matrix<double> A =  Eigen::Matrix<double, rows, cols>::Random();;

    Foo<ADdouble, ADdouble> foo;
    foo.setA(A.cast<ADdouble>());
    foo.setX(x);


    Vector<ADdouble> y = foo.getMultiplication();
    CppAD::ADFun<double> f(x, y);

    Vector<double> xNum = Vector<double>::Random(cols);
    Vector<double> jac = f.Jacobian(xNum);

    std::cout << "Expected Jacobian: " << std::endl << A << std::endl << std::endl;
    Eigen::Map<Eigen::Matrix<double, rows, cols, Eigen::RowMajor>> Jac(jac.data());
    std::cout << "CppAD Jacobian: " << std::endl << Jac << std::endl << std::endl;

    return 0;
}

The following code compiles and produces the expected output.

Expected Jacobian: 
  0.680375  -0.604897 -0.0452059    0.83239  -0.967399
 -0.211234  -0.329554   0.257742   0.271423  -0.514226
  0.566198   0.536459  -0.270431   0.434594  -0.725537
   0.59688  -0.444451  0.0268018  -0.716795   0.608354
  0.823295    0.10794   0.904459   0.213938  -0.686642

CppAD Jacobian: 
  0.680375  -0.604897 -0.0452059    0.83239  -0.967399
 -0.211234  -0.329554   0.257742   0.271423  -0.514226
  0.566198   0.536459  -0.270431   0.434594  -0.725537
   0.59688  -0.444451  0.0268018  -0.716795   0.608354
  0.823295    0.10794   0.904459   0.213938  -0.686642

Given the simplicity of the code, I was thinking that with some effort we can start supporting CppAD in our framework. This is also possible thanks to #63.

What do you think @traversaro @S-Dafarra?

It might be interesting also for @Giulero and @diegoferigo

[GiulioRomualdi]

was thinking that with some effort we can start supporting CppAD in our framework.

This should be read as: Moving to template metaprogramming

[GiulioRomualdi]

Actually checking https://github.com/stack-of-tasks/pinocchio/pull/813 I'm also really tempted to support casADi (we are already using casADi in #61).

[S-Dafarra]

Given the simplicity of the code, I was thinking that with some effort we can start supporting CppAD in our framework. This is also possible thanks to #63.

This is really cool @GiulioRomualdi, well done.

This should be read as: Moving to template metaprogramming

We are already moving to Eigen. If you are able to do these changes together, it is fine, otherwise, let's not mix changes too much. What would be your plan?

[GiulioRomualdi]

We are already moving to Eigen. If you are able to do these changes together, it is fine, otherwise, let's not mix changes too much. What would be your plan?

The first step to using Eigen with cppAD and CasADi is define the NumTraits for the scalars. Once this is done we can start to templatize the classes where we are interested in AD.

The first part is independent and it can be actually done in a few hours. The second part is more tricky. (I was thinking to open an Epic for this task and changing the title of this issue)

The task of converting the library to eigen is almost done. I should update the issue #63

[GiulioRomualdi]

The support of cppAD has been added #78. While regarding casadi we already started to use it in the #61.
For the time being, we can close this issue.