Support Casadi MX types

[philippbrusius]

Hey,
I am using pinocchio.casadi in my code to benefit from AD for my code.
It seems like only cas.SX.sym is supported.

For example:

import pinocchio.casadi as cpin
import casadi as cas

forces = cas.MX.sym("forces", 3, 1)
torques = cas.MX.sym("torques", 3, 1)

cForce = cpin.Force(forces, torques)

will lead to

Traceback (most recent call last):
File "", line 1, in
Boost.Python.ArgumentError: Python argument types in
Force.init(Force, MX, MX)
did not match C++ signature:
init(boost::python::api::object, pinocchio::ForceTpl<double, 0> clone)
init(_object* self, pinocchio::ForceTpl<casadi::Matrixcasadi::SXElem, 0> clone)
init(_object* self, Eigen::Matrix<casadi::Matrixcasadi::SXElem, 6, 1, 0, 6, 1> array)
init(_object* self, Eigen::Matrix<casadi::Matrixcasadi::SXElem, 3, 1, 0, 3, 1> linear, Eigen::Matrix<casadi::Matrixcasadi::SXElem, 3, 1, 0, 3, 1> angular)
init(_object* self)

With SX types this is working.

MX type has quite an advantage, especially for optimal control applications
Is there a special reason for that? Might in the future also MX types be supported?

Thanks in advance for the support

[jcarpent]

Good question. Could you use SX in your applications?
@AjSat Could you provide more details about pro and cons?

[philippbrusius]

I could use it, but had to wrap the pinocchio code into a casadi function

[jcarpent]

Could you depict a small example to better understand it?

[philippbrusius]

here is some python code that hopefully will give you more context. And also huge thanks for your response!

import pinocchio.casadi as cpin
import pinocchio as pin
import numpy as np
import casadi as cas


def create_pin_model():
    model = pin.Model()
    id_ = model.addJoint(
        0,
        pin.JointModelPX(),
        pin.SE3(np.eye(3), np.zeros(3)),
        ("PX"),
    )

    inertia_matrix = np.eye(3)
    mass = 1
    center_of_mass = np.zeros(3)

    pin_interia = pin.Inertia(mass, center_of_mass, inertia_matrix)
    model.appendBodyToJoint(id_, pin_interia, pin.SE3(np.eye(3), np.zeros(3)))
    cmodel = cpin.Model(model)
    cdata = cmodel.createData()
    return model, cmodel, cdata


def try_SX():
    model, cmodel, cdata = create_pin_model()

    print(type(cdata.C))  # you can see that data as well as model is SX based

    # example force
    forces = cas.SX.sym("forces", 3, 1)
    torques = cas.SX.sym("torques", 3, 1)
    cForce = cpin.Force(forces, torques)  # works fine

    # lets check ABA for example (example makes no sense, but just to check data types)
    q = cas.SX.sym("q", cmodel.nq, 1)
    v = cas.SX.sym("v", cmodel.nv, 1)
    tau = cdata.tau
    accel = cpin.aba(cmodel, cdata, q, v, tau)
    # cpin can handle SX type -> all good


def try_SX_wrapper():
    model, cmodel, cdata = create_pin_model()

    # lets wrap the aba method into a casadi function
    q = cas.SX.sym("q", cmodel.nq, 1)
    v = cas.SX.sym("v", cmodel.nv, 1)
    tau = cdata.tau
    accel = cpin.aba(cmodel, cdata, q, v, tau)
    ABA = cas.Function("ABA", [q, v], [accel], ["q", "v"], ["accel"])

    # now we can try to call it with MX inputs
    qMX = cas.MX.sym("q", cmodel.nq, 1)
    vMX = cas.MX.sym("v", cmodel.nv, 1)

    accelMX = ABA(qMX, vMX)
    # works fine. BUT cdata won't be updated -> huge limitation in usage of pinocchio


def try_SX_data_wrapper():
    model, cmodel, cdata = create_pin_model()

    # lets wrap the aba method into a casadi function
    q = cas.SX.sym("q", cmodel.nq, 1)
    v = cas.SX.sym("v", cmodel.nv, 1)
    tau = cdata.tau
    accel = cpin.aba(cmodel, cdata, q, v, tau)
    ABA = cas.Function("ABA", [q, v], [accel, cdata], ["q", "v"], ["accel", "cdata"])
    # will fail, we are not able to hand back the data class ...


def try_MX():
    model, cmodel, cdata = create_pin_model()

    # example force in MX domain
    forces = cas.MX.sym("forces", 3, 1)
    torques = cas.MX.sym("torques", 3, 1)
    cForce = cpin.Force(forces, torques)  # fails


if __name__ == "__main__":
    try_SX()
    try_SX_wrapper()
    # try_MX()  # -> will fail
    try_SX_data_wrapper()  # -> will fail as well

[jcarpent]

def try_SX_data_wrapper():
    model, cmodel, cdata = create_pin_model()

    # lets wrap the aba method into a casadi function
    q = cas.SX.sym("q", cmodel.nq, 1)
    v = cas.SX.sym("v", cmodel.nv, 1)
    tau = cdata.tau
    accel = cpin.aba(cmodel, cdata, q, v, tau)
    ABA = cas.Function("ABA", [q, v], [accel, cdata], ["q", "v"], ["accel", "cdata"])
    # will fail, we are not able to hand back the data class ...

The failure of this function is expected, as cdata is not a SX. Which behavior do you want to produce?

[jcarpent]

(sorry for the late reply)