Fail to generate code for pinocchio::aba with CppAD

[ghost]

Hi,

Pinocchio version : 2.6.8 installed from Conda CppAD: v20220705, CppADCg: v2.4.3

code:

//
// Copyright (c) 2018-2019 CNRS INRIA
//

#include "pinocchio/codegen/cppadcg.hpp"

#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"

#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/rnea.hpp"

#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

int main() {
    typedef double                Scalar;
    typedef CppAD::cg::CG<Scalar> CGScalar;
    typedef CppAD::AD<CGScalar>   ADScalar;

    typedef Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ADVector;

    typedef pinocchio::ModelTpl<Scalar> Model;
    typedef Model::Data                 Data;

    typedef pinocchio::ModelTpl<ADScalar> ADModel;
    typedef ADModel::Data                 ADData;

    Model                          model;
    pinocchio::JointModelComposite root = pinocchio::JointModelComposite(2);
    root.addJoint(pinocchio::JointModelTranslation());
    root.addJoint(pinocchio::JointModelSphericalZYX());
    pinocchio::urdf::buildModel("/home/harper/Documents/model/urdf/mini.urdf", root, model, true);
    model.lowerPositionLimit.head<3>().fill(-1.);
    model.upperPositionLimit.head<3>().fill(1.);
    Data data(model);

    ADModel ad_model = model.cast<ADScalar>();
    ADData  ad_data(ad_model);

    // Sample random configuration
    typedef Model::ConfigVectorType  ConfigVectorType;
    typedef Model::TangentVectorType TangentVectorType;

    ConfigVectorType  q(model.nq);
    TangentVectorType v(model.nv);
    TangentVectorType a(model.nv);
    TangentVectorType tau(model.nv - 6);

    typedef ADModel::ConfigVectorType  ADConfigVectorType;
    typedef ADModel::TangentVectorType ADTangentVectorType;

    ADTangentVectorType X(q.size() + v.size() + a.size() - 6);
    CppAD::Independent(X);
    ADConfigVectorType  ad_q = X.head(model.nq);
    ADTangentVectorType ad_v = X.segment(model.nq, model.nv);
    // ADTangentVectorType ad_a = (ADTangentVectorType(v.size()) << ADTangentVectorType::Zero(6), X.tail(model.nv - 6)).finished();

    ADTangentVectorType ad_tau = (ADTangentVectorType(v.size()) << ADTangentVectorType::Zero(6), X.tail(tau.size())).finished();
    // pinocchio::rnea(ad_model, ad_data, ad_q, ad_v, ad_a);
    pinocchio::aba(ad_model, ad_data, ad_q, ad_v, ad_tau);
    ADVector Y(model.nv);
    Y = ad_data.ddq;

    CppAD::ADFun<CGScalar> fun(X, Y);

    // generates source code
    CppAD::cg::ModelCSourceGen<Scalar> cgen(fun, "aba");
    cgen.setCreateJacobian(true);
    cgen.setCreateForwardZero(true);
    cgen.setCreateForwardOne(true);
    CppAD::cg::ModelLibraryCSourceGen<Scalar> libcgen(cgen);

    // compile source code
    CppAD::cg::DynamicModelLibraryProcessor<Scalar> p(libcgen);

    CppAD::cg::GccCompiler<Scalar>                 compiler;
    std::unique_ptr<CppAD::cg::DynamicLib<Scalar>> dynamicLib = p.createDynamicLibrary(compiler);

    // save to files (not really required)
    CppAD::cg::SaveFilesModelLibraryProcessor<Scalar> p2(libcgen);
    p2.saveSources();

    return 0;
}

The code is adapted from the unittest cppadcg-algo.cpp. The code generation for CentroidalMomentum and rnea were successful, while for aba the generation failed and it prints:

terminate called after throwing an instance of 'CppAD::cg::CGException'
  what():  GreaterThanZero cannot be called for non-parameters

[jcarpent]

@fabinsh Could you have a look on it?

[ghost]

@fabinsh Could you have a look on it?

Looks like your AT didnt work

[ghost]

@fabinsh Could you have a look on it?

@jcarpent I tried to modify the code according to CodeGenABA, and it turns out fun.optimize("no_compare_op") must be used for the code to pass cppad check. Perhaps aba is not yet ready for CppAD codegen?

[jcarpent]

You then should use it. This is expected by CppADCodeGen if I remember well. I close the issue as it seems to be solving your issue.

[FenglongSong]

Hi @xeonz1, I also encounter this issue some days ago with CppADCideGen. I wasn't able to make it work, so I turned to compute the mass matrix using CRBA and then do the matrix inverse to compute forward dynamics, at a cost of higher computational complexity. Did you manage to solve it? I will be happy to know your update :)

[jcarpent]

Perhaps aba is not yet ready for CppAD codegen?

aba is ready. It is just the need of using the right flags of compilation for CppADCodeGen.

[jcarpent]

Hi @xeonz1, I also encounter this issue some days ago with CppADCideGen. I wasn't able to make it work, so I turned to compute the mass matrix using CRBA and then do the matrix inverse to compute forward dynamics, at a cost of higher computational complexity. Did you manage to solve it? I will be happy to know your update :)

As mentioned by @xeonz1, you have to use fun.optimize("no_compare_op").