compilation error for C++ casadi pinocchio::difference

[nopaddleboat]

Hello,

I'm using casadi and pinocchio for trajectory optimization. From the test codes below, I saw casadi works for pinocchio::integration function.

  ConfigVectorAD q_ad(model.nq), v_int_ad(model.nv), q_int_ad(model.nq);
  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);
  v_int_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_v_int).data(),model.nv,1);  
  pinocchio::integrate(ad_model,q_ad,v_int_ad,q_int_ad);

Now I want to use pinocchio:difference with the following codes, which however fail to compile. Is there any example code for "difference"? Thanks.

  SX cs_q = SX::sym("q", model.nq);
  SX cs_q1 = SX::sym("q", model.nq);
  ConfigVectorAD q_ad(model.nq), v_diff_ad(model.nv), q_ad1(model.nq);
  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);
  q_ad1 = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q1).data(),model.nq,1);
pin::difference(ad_model, q_ad, q_ad1, v_diff_ad);

[jcarpent]

Could you provide a full reproducible example?

[nopaddleboat]

Thanks very much for your reply @jcarpent . Here is a minimal working example. If I commented out the last line " pinocchio::difference(ad_model, q_ad, q_ad1, v_diff_ad);" no complilation errors occur. Otherwise it complains

error: could not convert ‘casadi::operator>(const casadi::Matrix<casadi::SXElem>&, const casadi::Matrix<casadi::SXElem>&)(pinocchio::TaylorSeriesExpansion<casadi::Matrix<T> >::precision() [with int degree = 3; Scalar = casadi::SXElem]())’ from ‘casadi::Matrix<casadi::SXElem>’ to ‘bool’
[build]    61 |         const Scalar t = ((theta > TaylorSeriesExpansion<Scalar>::template precision<3>())

Here are the main.cpp function and corresponding cmakelist.txt. I complied it on Ubuntu 20 with vscode. Let me know if you need any further information.

#include "pinocchio/autodiff/casadi.hpp"

#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/aba-derivatives.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

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

#include <casadi/casadi.hpp>

using namespace casadi;
namespace pin = pinocchio;

int main() {

  typedef double Scalar;
  typedef casadi::SX ADScalar;

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

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

  Model model;
  pinocchio::buildModels::humanoidRandom(model);
  typedef ADModel::ConfigVectorType ConfigVectorAD;
  ADModel ad_model = model.cast<ADScalar>();
  ADData ad_data(ad_model);

  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
  casadi::SX cs_q1 = casadi::SX::sym("q1", model.nq);
  casadi::SX cs_v_int = casadi::SX::sym("v_inc", model.nv);
  typedef Model::ConfigVectorType ConfigVector;
  typedef Model::TangentVectorType TangentVector;
  ConfigVectorAD q_ad(model.nq),q_ad1(model.nq), v_int_ad(model.nv), q_int_ad(model.nq), v_diff_ad(model.nv);
  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);
  v_int_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_v_int).data(),model.nv,1);

  q_ad1 = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q1).data(),model.nq,1);

  pinocchio::integrate(ad_model,q_ad,v_int_ad,q_int_ad);
  pinocchio::difference(ad_model, q_ad, q_ad1, v_diff_ad);

  return 0;
}

Here is the CmakeLists.txt.

cmake_minimum_required(VERSION 3.0 FATAL_ERROR)

project(optimal_control)

LIST(APPEND PATH "/opt/openrobots/bin")
LIST(APPEND PKG_CONFIG_PATH "/opt/openrobots/lib/pkgconfig")
LIST(APPEND LD_LIBRARY_PATH "/opt/openrobots/lib")
LIST(APPEND PYTHONPATH "/opt/openrobots/lib/python3.8/site-packages")
LIST(APPEND CMAKE_PREFIX_PATH "/opt/openrobots")

find_package(pinocchio REQUIRED)
find_package(casadi)

add_executable(
    main
    src/main.cpp
    )

target_link_libraries(
    main
    pinocchio::pinocchio
    casadi
)

[jorisv]

Hello @nopaddleboat,

I can reproduce your issue with Pinocchio 2.

This is fixed in Pinocchio 3 which will be released soon.