Using CasADi automatic differentiation is not possible

[mkatliar]

The paper says:

In addition to analytical derivatives, Pinocchio supports
automatic differentiation. This is made possible through the
full scalar templatization of the whole C++ code and the use
of any external library that does automatic differentiation:
ADOL-C [23], CasADi [24], CppAD [25] and others.

I tried to use casadi::SX type as a scalar type for Pinocchio algorithms, but the code does not compile. Here is a code snippet:

pinocchio::ModelTpl<casadi::SX> model;
pinocchio::DataTpl<casadi::SX> data(model);

Eigen::Matrix<casadi::SX, N_JOINTS, 1> q;
Eigen::Matrix<casadi::SX, N_JOINTS, 1> v;
Eigen::Matrix<casadi::SX, N_JOINTS, 1> tau;

aba(model, data, q, v, tau);

Here is the relevant compiler output: make.log

[jcarpent]

Nice to try with Casadi. Before starting with Pinocchio, I would suggest to test with Eigen first and to make sure that everything works well. Do you have a unitary example of Eigen + Casadi?

From what I've seen from your log, the problem comes from there.

[mkatliar]

I see at least two sources of the problem:

1. The non-specialized implementation of SINCOSAlgo calls std::sin() and std::cos():

   template<typename Scalar>
   struct SINCOSAlgo
   {
     static void run(const Scalar & a, Scalar * sa, Scalar * ca) 
     {   
       (*sa) = std::sin(a); (*ca) = std::cos(a);
     }   
   };

   It would be more appropriate to just do sin(a), cos(a) and let argument-dependent lookup do the job. For casadi::SX argument type, the calls would resolve to casadi::sin() and casadi::cos(). Notice that if we rely on ADL, the following code fragment becomes unnecessary:

   #ifdef PINOCCHIO_WITH_CPPAD_SUPPORT
   
   /// Implementation for CppAD scalar types.
   template<typename Scalar>
   struct SINCOSAlgo< CppAD::AD<Scalar> >
   {
     static void run(const CppAD::AD<Scalar> & a, CppAD::AD<Scalar> * sa, CppAD::AD<Scalar> * ca)
     {
       (*sa) = CppAD::sin(a); (*ca) = CppAD::cos(a);
     }
   };
   
   #endif

2. The matrix decomposition algorithms in Eigen::LLT do inequality comparisons between scalar variables. If those variables are of type casadi::SX, the operator > is not defined (and can not be defined, because the variables are symbolic variables).

[mkatliar]

Before starting with Pinocchio, I would suggest to test with Eigen first and to make sure that everything works well. Do you have a unitary example of Eigen + Casadi?

@jcarpent it is this line

jmodel.jointVelocitySelector(data.ddq).noalias() =
      jdata.Dinv() * jmodel.jointVelocitySelector(data.u) - jdata.UDinv().transpose() * data.a[i].toVector();

which produces the error message

/usr/include/eigen3/Eigen/src/Core/GeneralProduct.h:255:18: error: cannot convert ‘casadi::Matrix<casadi::SXElem>’ to ‘const bool’ in initialization
       const bool alphaIsCompatible = (!ComplexByReal) || (numext::imag(actualAlpha)==RealScalar(0));

I was unable to reproduce the same error with a simplified example so far. For example, the following code compiles without errors:

Eigen::Matrix<casadi::SX, 3, 3> A, B;
Eigen::Matrix<casadi::SX, 3, 1> a, b;
Eigen::Matrix<casadi::SX, 3, 1> c = A * a - B.transpose() * b;

[cmastalli]

Are you sure if this possible? So far I know, CasADi has its own algebra software.

I think what you need is to pass derivatives from Pinocchio + CppAD to the interface of your solver. CasADi has several interfaces such as Ipopt. But some of them you can use without the CasADi framework

[mkatliar]

@cmastalli what do you mean by "solver" here? You mentioned Ipopt -- yes, CasADi does provide interface to NLP solvers, but we are not speaking about optimization here. We are discussing automatic differentiation issues. And yes, CasADi does provide means for automatic differentiation:

CasADi's backbone is a symbolic framework implementing forward and reverse mode of AD on expression graphs

See https://web.casadi.org/ for more details.

[jmirabel]

Could you try:

Eigen::Matrix<casadi::SX, 3, 3> A, B;
Eigen::Matrix<casadi::SX, -1, 1> a (4), c (4);
Eigen::Matrix<casadi::SX, 3, 1> b;
c.segment(1, 3) = A * a.segment(1, 3) - B.transpose() * b;

@mkatliar, notice I edited the snippet.

[mkatliar]

@jmirabel your (the edited and the original) snippet compiles without errors.

[mkatliar]

@jmirabel can it have something to do with .noalias()?

[jmirabel]

It could but my guesses are:

• either one of typedef defined by Pinocchio does not handle properly the Scalar,
• or casADI wasn't meant to be used the way you use it...

From the log you provided, I can see that the matrices in the expression are of type Eigen::Matrix<casadi::Matrix<casadi::SXElem>, ...> and scalar is casadi::Matrix<casadi::SXElem>. Does this make sense ?

/usr/local/include/pinocchio/algorithm/aba.hxx:250:17:   required from ‘const typename pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::aba(const pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl>&, pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>&, const Eigen::MatrixBase<Mat>&, const Eigen::MatrixBase<MatRet>&, const Eigen::MatrixBase<TangentVectorType>&)
[with Scalar = casadi::Matrix<casadi::SXElem>; int Options = 0; JointCollectionTpl = pinocchio::JointCollectionDefaultTpl; ConfigVectorType = Eigen::Matrix<casadi::Matrix<casadi::SXElem>, 6, 1, 0, 6, 1>; TangentVectorType1 = Eigen::Matrix<casadi::Matrix<casadi::SXElem>, 6, 1, 0, 6, 1>; TangentVectorType2 = Eigen::Matrix<casadi::Matrix<casadi::SXElem>, 6, 1, 0, 6, 1>; typename pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType = Eigen::Matrix<casadi::Matrix<casadi::SXElem>, -1, 1, 0, -1, 1>]’

[mkatliar]

Hello @jmirabel , to address your second point, I wrote a working example which demonstrates the use of CasADi AD in combination with functions working with Eigen matrices:

#include <casadi/casadi.hpp>
#include <Eigen/Dense>

namespace Eigen
{
    template<> struct NumTraits<casadi::SX>
    {
        using Real = casadi::SX;
        using NonInteger = casadi::SX;
        using Literal = casadi::SX;
        using Nested = casadi::SX;

        static bool constexpr IsComplex = false;
        static bool constexpr IsInteger = false;
        static int constexpr ReadCost = 1;
        static int constexpr AddCost = 1;
        static int constexpr MulCost = 1;
        static bool constexpr IsSigned = true;
        static bool constexpr RequireInitialization = true;

        static double epsilon()
        {
            return std::numeric_limits<double>::epsilon();
        }

        static double dummy_precision()
        {
            return 1e-10;
        }

        static double hightest()
        {
            return std::numeric_limits<double>::max();
        }

        static double lowest()
        {
            return std::numeric_limits<double>::min();
        }

        static int digits10()
        {
            return std::numeric_limits<double>::digits10;
        }
    };

    // A function working with Eigen::Matrix'es parameterized by the Scalar type
    template <typename Scalar, typename T1, typename T2, typename T3, typename T4>
    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> eigenFun(
        Eigen::MatrixBase<T1> const& A, Eigen::MatrixBase<T2> const& a, Eigen::MatrixBase<T3> const& B, Eigen::MatrixBase<T4> const& b)
    {
        Eigen::Matrix<Scalar, Eigen::Dynamic, 1> c(4);
        c.segment(1, 3) = A * a.segment(1, 3) - B.transpose() * b;
        c[0] = 0.;

        return c;
    }
}

    int main(int, char **)
    {
        // Declare casadi symbolic matrix arguments
        // casadi::SX cs_A = casadi::SX::sym("A", 3, 3);
        // casadi::SX cs_B = casadi::SX::sym("B", 3, 3);
        casadi::SX cs_a = casadi::SX::sym("a", 4);
        casadi::SX cs_b = casadi::SX::sym("b", 3);

        // Declare Eigen matrices
        Eigen::Matrix<casadi::SX, 3, 3> A, B;
        Eigen::Matrix<casadi::SX, -1, 1> a (4), c (4);
        Eigen::Matrix<casadi::SX, 3, 1> b;

        // Let A, B be some numeric matrices
        for (Eigen::Index i = 0; i < A.rows(); ++i)
        {
            for (Eigen::Index j = 0; j < A.cols(); ++j)
            {
                A(i, j) = 10 * i + j;
                B(i, j) = -10 * i - j;
            }

            b[i] = cs_b(i);
        }

        // Let a, b be symbolic arguments of a function
        for (Eigen::Index i = 0; i < b.size(); ++i)
            b[i] = cs_b(i);

        for (Eigen::Index i = 0; i < a.size(); ++i)
            a[i] = cs_a(i);

        // Call the function taking Eigen matrices
        c = eigenFun<casadi::SX>(A, a, B, b);

        // Copy the result from Eigen matrices to casadi matrix
        casadi::SX cs_c = casadi::SX(casadi::Sparsity::dense(c.rows(), 1));
        for (Eigen::Index i = 0; i < c.rows(); ++i)
            cs_c(i) = c[i];

        // Display the resulting casadi matrix
        std::cout << "c = " << cs_c << std::endl;

        // Do some AD
        casadi::SX dc_da = jacobian(cs_c, cs_a);

        // Display the resulting jacobian
        std::cout << "dc/da = " << dc_da << std::endl;

        // Create a function which takes a, b and returns c and dc_da
        casadi::Function fun("fun", casadi::SXVector {cs_a, cs_b}, casadi::SXVector {cs_c, dc_da});
        std::cout << "fun = " << fun << std::endl;

        // Evaluate the function
        casadi::DMVector res = fun(casadi::DMVector {std::vector<double> {1., 2., 3., 4.}, std::vector<double> {-1., -2., -3.}});
        std::cout << "fun(a, b)=" << res << std::endl;
    }

It outputs:

c = [0, ((a_2+(2*a_3))-((-10*b_1)+(-20*b_2))), (((10*a_1)+((11*a_2)+(12*a_3)))-(((-11*b_1)+(-21*b_2))-b_0)), (((20*a_1)+((21*a_2)+(22*a_3)))-((-2*b_0)+((-12*b_1)+(-22*b_2))))]
dc/da = 
[[00, 00, 00, 00], 
 [00, 00, 1, 2], 
 [00, 10, 11, 12], 
 [00, 20, 21, 22]]
fun = fun:(i0[4],i1[3])->(o0[4],o1[4x4,8nz]) SXFunction
fun(a, b)=[[0, -69, 15, 99], 
[[00, 00, 00, 00], 
 [00, 00, 1, 2], 
 [00, 10, 11, 12], 
 [00, 20, 21, 22]]]

This is how I see CasADi can be used in combination with Eigen code parameterized by a scalar type.

[mkatliar]

From the log you provided, I can see that the matrices in the expression are of type Eigen::Matrix<casadi::Matrix<casadi::SXElem>, ...> and scalar is casadi::Matrix<casadi::SXElem>. Does this make sense ?

/usr/local/include/pinocchio/algorithm/aba.hxx:250:17:   required from ‘const typename pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::aba(const pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl>&, pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>&, const Eigen::MatrixBase<Mat>&, const Eigen::MatrixBase<MatRet>&, const Eigen::MatrixBase<TangentVectorType>&)
[with Scalar = casadi::Matrix<casadi::SXElem>; int Options = 0; JointCollectionTpl = pinocchio::JointCollectionDefaultTpl; ConfigVectorType = Eigen::Matrix<casadi::Matrix<casadi::SXElem>, 6, 1, 0, 6, 1>; TangentVectorType1 = Eigen::Matrix<casadi::Matrix<casadi::SXElem>, 6, 1, 0, 6, 1>; TangentVectorType2 = Eigen::Matrix<casadi::Matrix<casadi::SXElem>, 6, 1, 0, 6, 1>; typename pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType = Eigen::Matrix<casadi::Matrix<casadi::SXElem>, -1, 1, 0, -1, 1>]’

It does make sense to me, because casadi::SX is a matrix, and in CasADi a scalar is an equivalent of a 1x1 matrix.

[jmirabel]

Shouldn't you define Eigen::NumTraits<casadi::Matrix<casadi::SXElem> > too then ?

[jcarpent]

Shouldn't you define Eigen::NumTraits<casadi::Matrix<casadi::SXElem> > too then ?

Yes, indeed. I've done it in #813.

[cmastalli]

@cmastalli what do you mean by "solver" here? You mentioned Ipopt -- yes, CasADi does provide interface to NLP solvers, but we are not speaking about optimization here. We are discussing automatic differentiation issues. And yes, CasADi does provide means for automatic differentiation:

CasADi's backbone is a symbolic framework implementing forward and reverse mode of AD on expression graphs

See https://web.casadi.org/ for more details.

@mkatliar I meat NLP solvers such as Ipopt or its own SQP one. So far, I understand the benefits of using automatic differentiation from CasADi is the easy integration to NLP solvers, instead CppAD is more efficient and it can generate the AD code, isn't it?

[mkatliar]

Shouldn't you define Eigen::NumTraits<casadi::Matrix<casadi::SXElem> > too then ?

@jmirabel casadi::SX is a typedef for casadi::Matrix<casadi::SXElem>: https://github.com/casadi/casadi/blob/006f67c/casadi/core/sx_elem.hpp#L251

[jcarpent]

fixed by #813.