Pinocchio Casadi interface for matrixToRpy()

[waynekyrie]

Bug description

I'm trying to convert a rotation matrix to a rotation vector. The numerical function works fine, but the casadi interface seems not working. A short example is as shown below.

import casadi as ca
import pinocchio
import pinocchio.casadi as cpin
rot_mtx_ca = ca.SX.eye(3)
cpin.rpy.matrixToRpy(rot_mtx_ca)

The error message is attached below.

Boost.Python.ArgumentError: Python argument types in
    pinocchio.pinocchio_pywrap_casadi.rpy.matrixToRpy(SX)
did not match C++ signature:
    matrixToRpy(Eigen::MatrixBase<Eigen::Matrix<double, 3, 3, 0, 3, 3>> R)

I'm using pinocchio 3.1.0 and casadi 3.6.5.

[waynekyrie]

Hi, I just want to follow up on this issue. Is it indeed a bug or am I doing something wrong?

[Aymenzerahmed]

Hi,
I faced the same issue (in cpp) but used atan2 math equations (found here (with no conditions) instead of matrixTorpy (just a workaround). If I recall, the error I had with matrixTorpy is with the many if expressions with its implementation since it expects numerical values. i.e : if (res[1] < -pi / 2).

[jcarpent]

Hi, I just want to follow up on this issue. Is it indeed a bug or am I doing something wrong?

This is not a bug, but currently, Eigen operations are not compliant with autodiff framworks, with a lot of if/then/else conditions.
One major that we have already done in Pinocchio is to make all the geometrical expressions compliant with autodiff frameworks such as CasADi and others (e.g., CppAD).
I will update matrixToRpy.