Ways of computing inverse of the mass inertia

[cmastalli]

Hi guys,

While debugging a code based on Pinocchio, I observed that computing the inverse of the mass matrix via pinocchio.computeMinverse and pinocchio.computeAllTerms is different. For more details see the code below

import pinocchio
import numpy as np

def createModel():
    m = pinocchio.Model()
    joint_type = pinocchio.JointModelFreeFlyer()
    joint_name = "sphere_frame"
    m.addJoint(m.getJointId("universe"), joint_type, pinocchio.SE3.Identity(), joint_name)
    p = np.array([2., -0.57240428, 0.4859581, 0.02404804, 0.0073735735, 0., 0.0073735735, -0., 0., 0.0073735735])
    inertia = pinocchio.Inertia.FromDynamicParameters(p)
    m.appendBodyToJoint(m.getJointId(joint_name), inertia, pinocchio.SE3.Identity())
    return m

model = createModel()
data = model.createData()
q = pinocchio.neutral(model)
v = np.zeros(6)

Minv1 = pinocchio.computeMinverse(model, data, q)
pinocchio.computeAllTerms(model, data, q, v)
Minv2 = np.linalg.inv(data.M)

print("pinocchio.computeMinverse(model, data, q):\n", Minv1)
print()
print("np.linalg.inv(data.M):\n", Minv2)

which prints the following results

pinocchio.computeMinverse(model, data, q):
 [[ 1089.038647724906  1279.044572468231    63.294582476347     0.              -221.153421411689  4469.025187820857]
 [ 1279.044572468231  1509.730578952917   -53.735648238862   221.153421411689     0.              5264.011742856972]
 [   63.294582476347   -53.735648238862  2592.950920353578 -4469.025187820857 -5264.011742856972     0.            ]
 [    0.               221.153421411689 -4469.025187820857 18392.63585819789      0.                 0.            ]
 [ -221.153421411689     0.             -5264.011742856972     0.             18392.63585819789      0.            ]
 [ 4469.025187820857  5264.011742856972     0.                 0.                 0.             18392.63585819789 ]]

np.linalg.inv(data.M):
 [[  0.284643988026  -0.253045621154  -0.01252217263   -0.               0.043752896573  -0.884149996761]
 [ -0.253045621154   0.201414479576   0.010631037244  -0.043752896573   0.              -1.041429790568]
 [ -0.01252217263    0.010631037244  -0.012889360991   0.884149996761   1.041429790568  -0.            ]
 [ -0.              -0.043752896573   0.884149996761  77.206523546864 -68.635817125548  -3.396500388959]
 [  0.043752896573   0.               1.041429790568 -68.635817125548  54.631442834674   2.883550898096]
 [ -0.884149996761  -1.041429790568   0.              -3.396500388959   2.883550898096  -3.496096157636]]

However, I would expect that both approaches return the same value. This happens when p[1:3] != 0. Is there something I am doing wrong?

Many thanks for your support.
Carlos

Fyi @Sergim96

[jcarpent]

As far as I can see, your inertia is not positive definite, so not well-defined.
Please use the following script to check on your side.

import pinocchio as pin
import numpy as np

p = np.array([2., -0.57240428, 0.4859581, 0.02404804, 0.0073735735, 0., 0.0073735735, -0., 0., 0.0073735735])
inertia = pin.Inertia.FromDynamicParameters(p)

assert (np.linalg.eig(inertia.inertia)[0] > 0.).all()

In other words, you should be careful to properly setup it.

[cmastalli]

Thanks, @jcarpent!

Now I understand where my misunderstanding was. I thought the FromDynamicParameters function takes the inertia tensor of the body. Instead, it takes the inertia tensor at the barycenter. I apologize for being not careful enough. The documentation is clear, I would say.

Btw, it would be fantastic to bind all the Cholesky functions (i.e., decompose, solve, Mv, UDUtv, etc). Could you confirm for me that it is more efficient to use decompose+solve than just computeMinverse?

If this is the case, we could help you bind this ASAP.

[jcarpent]

It depends. What do you want to do exactly ...

[cmastalli]

I need to run ABADiff first and then compute M^-1 * J where J is a matrix. I was thinking that I could use solve for each column of the J matrix.