Usage of the cpin.aba function

[DomenicoDona]

Bug description

I am trying to use pinocchio.casadi for a 2 DOF planar manipulator.

When calling cpin.aba with the provided inputs, an ArgumentError is raised due to a mismatch in the expected argument types.

Code to reproduce

import pinocchio as pin
print(pin.__version__)
from pinocchio import casadi as cpin
import casadi as ca
import numpy as np

model_name = 'double_pendulum_ode'

# Create the model
pin_model = pin.Model()
dof = 2
a = np.array([0.0, 1.0])
d = np.array([0.0, 0.0])
m = np.array([1.0, 1.0])
ell = np.array([[0.5, 0.0, 0.0], [0.5, 0.0, 0.0]])

I = np.zeros((3, 3, 2))
# Set the slices
I[:,:,0] = np.diag([0.01, 0.01, 0.0])
I[:,:,1] = np.diag([0.01, 0.01, 0.0])

I_sptl = []
joint_placement = []
joint_id = []

for i in range(dof):
    I_sptl.append(pin.Inertia(m[i], ell[i], I[:,:,i]))  # Inertia object
    joint_placement.append(pin.SE3(np.eye(3), np.array([a[i], 0.0, d[i]])))

    if i == 0:
        joint_id.append(pin_model.addJoint(
            0, pin.JointModelRZ(), joint_placement[-1], "joint_{}".format(i+1)
        ))
    else:
        joint_id.append(pin_model.addJoint(
            joint_id[-1], pin.JointModelRZ(), joint_placement[-1], "joint_{}".format(i+1)
        ))

    pin_model.appendBodyToJoint(joint_id[-1], I_sptl[-1], pin.SE3.Identity())

# Create data required for computations
data = pin.Data(pin_model)

q = ca.SX.sym('q', pin_model.nq)
v = ca.SX.sym('v', pin_model.nv)
x = ca.vertcat(q, v)

xdot = ca.SX.sym('xdot', pin_model.nq + pin_model.nv)
tau = ca.SX.sym('tau', pin_model.nq)

print("Type of model: ", type(pin_model))
print("Type of data: ", type(data))
print("Type of q: ", type(q), " Shape: ", q.shape)
print("Type of v: ", type(v), " Shape: ", v.shape)
print("Type of tau: ", type(tau), " Shape: ", tau.shape)

a = cpin.aba(pin_model, data, q, v, tau)

Output

Pinocchio version is:  3.0.0
Type of model:  <class 'pinocchio.pinocchio_pywrap_default.Model'>
Type of data:  <class 'pinocchio.pinocchio_pywrap_default.Data'>
Type of q:  <class 'casadi.casadi.SX'>  Shape:  (2, 1)
Type of v:  <class 'casadi.casadi.SX'>  Shape:  (2, 1)
Type of tau:  <class 'casadi.casadi.SX'>  Shape:  (2, 1)
---------------------------------------------------------------------------
ArgumentError                             Traceback (most recent call last)
<ipython-input-6-d2734e54efa3> in <cell line: 58>()
     56 print("Type of tau: ", type(tau), " Shape: ", tau.shape)
     57 
---> 58 a = cpin.aba(pin_model, data, q, v, tau)
     59 
     60 f_expl = ca.vertcat(v, a)

ArgumentError: Python argument types in
    pinocchio.pinocchio_pywrap_casadi.aba(Model, Data, SX, SX, SX)
did not match C++ signature:
    aba(pinocchio::ModelTpl<casadi::Matrix<casadi::SXElem>, 0, pinocchio::JointCollectionDefaultTpl> model, pinocchio::DataTpl<casadi::Matrix<casadi::SXElem>, 0, pinocchio::JointCollectionDefaultTpl> {lvalue} data, Eigen::MatrixBase<Eigen::Matrix<casadi::Matrix<casadi::SXElem>, -1, 1, 0, -1, 1> > q, Eigen::MatrixBase<Eigen::Matrix<casadi::Matrix<casadi::SXElem>, -1, 1, 0, -1, 1> > v, Eigen::MatrixBase<Eigen::Matrix<casadi::Matrix<casadi::SXElem>, -1, 1, 0, -1, 1> > tau, std::vector<pinocchio::ForceTpl<casadi::Matrix<casadi::SXElem>, 0>, Eigen::aligned_allocator<pinocchio::ForceTpl<casadi::Matrix<casadi::SXElem>, 0> > > fext, pinocchio::Convention convention=pinocchio.pinocchio_pywrap_default.Convention.LOCAL)
    aba(pinocchio::ModelTpl<casadi::Matrix<casadi::SXElem>, 0, pinocchio::JointCollectionDefaultTpl> model, pinocchio::DataTpl<casadi::Matrix<casadi::SXElem>, 0, pinocchio::JointCollectionDefaultTpl> {lvalue} data, Eigen::MatrixBase<Eigen::Matrix<casadi::Matrix<casadi::SXElem>, -1, 1, 0, -1, 1> > q, Eigen::MatrixBase<Eigen::Matrix<casadi::Matrix<casadi::SXElem>, -1, 1, 0, -1, 1> > v, Eigen::MatrixBase<Eigen::Matrix<casadi::Matrix<casadi::SXElem>, -1, 1, 0, -1, 1> > tau, pinocchio::Convention convention=pinocchio.pinocchio_pywrap_default.Convention.LOCAL)

System

I have tried both on an Ubuntu 22.04 machine and in Google Colab, and the same error arises.

[jorisv]

Hello @DomenicoDona,

To call cpin.aba you have to provide him a casadi model and data.
You can check in this example how to create it from a double model : https://github.com/stack-of-tasks/pinocchio/blob/master/examples/cartpole-casadi.py#L64-L65

[DomenicoDona]

I had a doubt related to some type discrepancy. Sorry for the bother, and thank you!

[jcarpent]

The correct solution is then:

import pinocchio as pin
print(pin.__version__)
from pinocchio import casadi as cpin
import casadi as ca
import numpy as np

model_name = 'double_pendulum_ode'

# Create the model
pin_model = pin.Model()
dof = 2
a = np.array([0.0, 1.0])
d = np.array([0.0, 0.0])
m = np.array([1.0, 1.0])
ell = np.array([[0.5, 0.0, 0.0], [0.5, 0.0, 0.0]])

I = np.zeros((3, 3, 2))
# Set the slices
I[:,:,0] = np.diag([0.01, 0.01, 0.0])
I[:,:,1] = np.diag([0.01, 0.01, 0.0])

I_sptl = []
joint_placement = []
joint_id = []

for i in range(dof):
    I_sptl.append(pin.Inertia(m[i], ell[i], I[:,:,i]))  # Inertia object
    joint_placement.append(pin.SE3(np.eye(3), np.array([a[i], 0.0, d[i]])))

    if i == 0:
        joint_id.append(pin_model.addJoint(
            0, pin.JointModelRZ(), joint_placement[-1], "joint_{}".format(i+1)
        ))
    else:
        joint_id.append(pin_model.addJoint(
            joint_id[-1], pin.JointModelRZ(), joint_placement[-1], "joint_{}".format(i+1)
        ))

    pin_model.appendBodyToJoint(joint_id[-1], I_sptl[-1], pin.SE3.Identity())

# Create data required for computations
cmodel = cpin.Model(pin_model)
cdata = cmodel.createData()

q = ca.SX.sym('q', pin_model.nq)
v = ca.SX.sym('v', pin_model.nv)
x = ca.vertcat(q, v)

xdot = ca.SX.sym('xdot', pin_model.nq + pin_model.nv)
tau = ca.SX.sym('tau', pin_model.nq)

print("Type of model: ", type(pin_model))
print("Type of data: ", type(data))
print("Type of q: ", type(q), " Shape: ", q.shape)
print("Type of v: ", type(v), " Shape: ", v.shape)
print("Type of tau: ", type(tau), " Shape: ", tau.shape)

a = cpin.aba(pin_model, data, q, v, tau)