Hello! I am trying to recalculate the joint space inertia matrix (mjData.qM) on my own, and running into difficulties trying to match it. For a simple two-link arm, I'm calculating it as qM = sum_i (J_i^T * M_i * J_i) where J_i is the Jacobian for the COM of the arm segment i (mj_jacBodyCom) and M_i is the inertia matrix of the arm segment (diag(body_mass, body_inertia])). I'm wondering if I need to be rotating the inertia matrix for the arm segment from it's diagonal form before doing the multiplication. Can you provide some insight into what I'm doing incorrectly? Thanks,
Sorry about the delay. Your formula looks right, but I think you are using incompatible coordinate systems. M should be in Cartesian coordinates. For point masses there is no difference, but anisotropic inertias need to be transformed. Here are some slides I wrote a while ago; look at slide 8 in particular. MuJoCo uses the CRB algorithm to compute the joint-space inertia, but if you get the coordinate systems right you should obtain the same answer numerically.
Thanks! The transformation from Cartesian coordinates to generalized coordinates makes sense, but I'm having trouble with the idea of getting M into Cartesian coordinates starting from the values returned by model.body_mass[body_id] and model.body_inertia[body_id]. I didn't see this particular point addressed in the slides, am hoping to get some more insight on how I can go about this. Cheers,