Clarification on C++ Equivalent for "from pinocchio import casadi as cpin" and How to Use RNEA and ABA with CasADi in C++

[robinhhood]

Hello all!

Thank you in advance for your time!

I have Python 3.8 on my Ubuntu 20.04 installed and I built:

• pinocchio 3.1.0 with -DBUILD_WITH_CASADI_SUPPORT=ON
• CasADi 3.6.3 with -DWITH_IPOPT=ON -DWITH_MUMPS=ON

I have successfully tested RNEA and ABA in Python using the following code:

import numpy as np
import casadi as ca
import pinocchio as pin
from pinocchio import casadi as cpin

# Create a Pinocchio model (e.g., a manipulator model)
model = pin.buildSampleModelManipulator()
cmodel = cpin.Model(model) # CasADi type model
cdata = cmodel.createData() # CasADi type data

# Define symbolic variables using CasADi with the correct dimensions
q = ca.SX.sym("q", model.nq)  # Symbolic joint configuration
v = ca.SX.sym("v", model.nv)  # Symbolic joint velocity
a = ca.SX.sym("a", model.nv)  # Symbolic joint acceleration
tau = ca.SX.sym("tau", model.nv)  # Symbolic joint torque

# RNEA and ABA
cpin.computeAllTerms(cmodel, cdata, q, v)
rnea_tau = cpin.rnea(cmodel, cdata, q, v, a)  # RNEA to compute torque
aba_acc = cpin.aba(cmodel, cdata, q, v, tau) # ABA to compute acceleration

# Define CasADi functions for both RNEA and ABA
rnea_func = ca.Function("rnea_func", [q, v, a], [rnea_tau])
aba_func = ca.Function("aba_func", [q, v, tau], [aba_acc])

# Example usage
q_test = np.random.rand(model.nq)
v_test = np.random.rand(model.nv)
a_test = np.random.rand(model.nv)

rnea_result = rnea_func(q_test, v_test, a_test)
print("RNEA Result (torques):", rnea_result)

# Example usage for ABA
tau_test = np.random.rand(model.nv)

aba_result = aba_func(q_test, v_test, tau_test)
print("ABA Result (accelerations):", aba_result)

However, I am normally working with C++ and therefore I want to translate this code to C++ and I am unsure how to proceed. Specifically, I would like to know the following:

1. What is the 1:1 C++ equivalent of the Python import statement from pinocchio import casadi as cpin? Is there a similar namespace or module in C++? I am a little bit confused since this casadi comes from the path: /usr/local/lib/python3.8/site-packages/pinocchio/casadi

2. Which headers should I include to work with CasADi and Pinocchio together in C++? Did I miss something like #include <pinocchio/casadi.hpp>? I’m looking for the equivalent functions like pinocchio::casadi::rnea or pinocchio::casadi::aba. It seems that the following headers are not enough:

   #include <casadi/casadi.hpp>
   #include <pinocchio/autodiff/casadi.hpp>
   #include <pinocchio/autodiff/casadi-algo.hpp>

3. Are there examples available demonstrating how to implement RNEA and ABA with CasADi in C++?

I appreciate any help how to work with CasADi in Pinocchio on the C++ side.

[fabinsch]

Hi @robinhhood,

a good starting point to see how to use CasADi and Pinocchio together in C++ are the unittest, for example this one. Everything is templated on the scalar type (Model, algorithms etc), so instead of using double (which is the default) you use casadi::SX.

[jcarpent]

@robinhhood I've just opened this PR (#2388) that provides two new C++ examples related to the use of RNEA and ABA with CasADi

[robinhhood]

@fabinsch thank you for the hint @jcarpent thank you for the example