Robot Jacobian and Operational Space Mass Matrix

[gokulmk-12]

Hi, I am using motrixsim to study go2 locomotion using an MPC controller. This requires the use of Site jacobians and Operational Space Inertia matrix for computation. Is there an API to fetch these quantities ? Can you guide me through this .

[wlgys8]

Current Python SDK already provides a site Jacobian API:

site = model.get_site("foot_site")
J = site.get_jacobian(data)
dof_idx = site.dof_vel_indices

site.get_jacobian(data) returns an ndarray with shape (*data.shape, 6, k), where k == len(site.dof_vel_indices). The first 3 rows are angular velocity and the last 3 rows are linear velocity. The columns are compact: they correspond exactly to site.dof_vel_indices, not to all model DoFs.

For the Operational Space Mass/Inertia Matrix, you can compute it from the standard formula once you have the joint-space inertia matrix in the same DoF order:

import numpy as np

site = model.get_site("foot_site")
J = site.get_jacobian(data)          # (..., 6, k)
dof_idx = site.dof_vel_indices       # (k,)

# M_full should be the joint-space inertia matrix in global DoF velocity order.
# Select the submatrix that matches the compact site Jacobian columns.
M_site = M_full[..., dof_idx[:, None], dof_idx]   # (..., k, k)

Minv_JT = np.linalg.solve(M_site, np.swapaxes(J, -1, -2))
lambda_site = np.linalg.pinv(J @ Minv_JT)         # (..., 6, 6)

For position-only control, use the linear rows of the Jacobian:

J_pos = J[..., 3:6, :]
Minv_JT_pos = np.linalg.solve(M_site, np.swapaxes(J_pos, -1, -2))
lambda_pos = np.linalg.pinv(J_pos @ Minv_JT_pos)  # (..., 3, 3)

For orientation-only control, use J[..., 0:3, :] in the same way.

If you are using motrixsim.ik.IkChain instead of a raw site, the SDK also exposes chain.get_end_effector_jac(data), chain.get_end_point_jac(data, end_point), and chain.get_inertia_matrix(data), which are useful for the same OSC-style computation.