Hello! I am trying to recalculate the joint space inertia matrix (mjData.qM) on my own, and running into difficulties. 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,