TODO
TODO
TODO
- Contact normal is +x, points from geom1 to geom2:
normals = mjx_data._impl.contact.frame[:, 0, :]
- 4D pyramid approximation, see decodePyramid
efc_to_cframe (3x4) = [[1., 1., 1., 1.], [mu1, -mu1, 0., 0.], [0., 0., mu2, -mu2]]
force_cframe (3x1) = efc_to_cframe @ pyramid (4x1)
isotropic: mu1 == mu2
force_wframe (3x1) = mjx_data._impl.contact.frame.T @ force_cframe
[mu1, mu2] = mjx_data._impl.contact.friction[:, :2]phis = mjx_data._impl.contact.distsliding_vels ([3*n_c]x1) = block_diag(efc_to_cframe) @ mjx_data._impl.efc_J @ mjx_data.qvel