Jacobian computation using Pinocchio & CasADI

Hello,

I was trying the new release of Pinocchio and its support for CasADI. Such a great and helpful tool, congrats and thanks! I'm writing this because I'm facing an issue when computing a Jacobian of a cost function and I'd appreciate a little bit of help.

Here's a little bit of context. I played with CasADI and Pinocchio and tried to solve an OCP using a quadrotor as the robot model. Since IPOPT does not support optimization in SE(3), I set the decision variables to lie in the tangent space of a nominal state, which I set to be the identity. When the optimization finishes, I retract the solution to the manifold.

Thus, a cost for the desired pose would have a form like $l(\mathbf{\Delta x}) = \mathbf{e}(\mathbf{\Delta x})^\top \mathbf{e}(\mathbf{\Delta x})$ , with $\mathbf{e}(\mathbf{\Delta x}) = \mathbf{x}_g \ominus (\mathbf{x}_n \oplus \mathbf{\Delta x}) $ and $\mathbf{x}_g$ the goal state and $\mathbf{x}_n$ the nominal state, which I normally set to identity.

When adding this cost to the OCP it generates an error due to nan values at its gradient. Then I computed this gradient separately and it does have nans. However, when computing the gradient using regular Pinocchio and the chain rule, it outputs the correct solution.

I might be doing something wrong with CasADI, since I'm not very familiar with its syntax. At the end of this issue, I'm attaching the code showing the problem. The CasADI Jacobian does not coincide with Pinocchio's output with the chain rule.

Thanks a lot!

The output of the script:

Cost Jacobian using Pin & Chain rule:  [-0.5  0.  -1.   0.   0.   0. ]
Cost Jacobian using CasADI:  [[-0.5, 0, -1, nan, -nan, nan]]

The script:

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

import casadi

import example_robot_data

def pin_jacobian():
    robot = example_robot_data.load('hector')
    model = robot.model

    x_nom = pin.SE3.Identity()
    dx = np.zeros(6)

    x_goal = pin.SE3(pin.Quaternion.Identity(), np.array([0.5, 0, 1]))

    y = pin.integrate(model, pin.SE3ToXYZQUAT(x_nom), dx)
    z = pin.difference(model, y, pin.SE3ToXYZQUAT(x_goal))

    l = 0.5 * z.T @ z

    J_l_z = z
    J_z_y = pin.dDifference(model, y, pin.SE3ToXYZQUAT(x_goal), pin.ArgumentPosition.ARG0)
    J_y_dx = pin.dIntegrate(model, y, dx, pin.ArgumentPosition.ARG1)

    J_l_dx = J_l_z @ J_z_y @ J_y_dx

    print(J_l_dx)

def casadi_jacobian():
    robot = example_robot_data.load('hector')

    model = robot.model
    cmodel = cpin.Model(model)

    x_nom = casadi.SX([0, 0, 0, 0, 0, 0, 1])
    x_goal = casadi.SX([0.5, 0, 1, 0, 0, 0, 1])

    dx = casadi.SX.sym("dx", model.nv)

    y = cpin.integrate(cmodel, x_nom, dx)
    z = cpin.difference(cmodel, y, x_goal)

    l = 0.5 * z.T @ z

    f_l = casadi.Function('f_l', [dx], [l], ['dx'], ['l'])

    J_l_dx = f_l.jacobian()

    print(J_l_dx([0] * 6, 0.625))

def main():
    pin_jacobian()
    casadi_jacobian()

if __name__ == '__main__':
    main()

stack-of-tasks / pinocchio

Jacobian computation using Pinocchio & CasADI #1655