[MuJoCo] Jacobian API

Geometric/Analytical Jacobian

Geometric Jacobian

The relationship between the joint velocities and the corresponding (uniquely-defined) end-effector linear and angular velocities at given point on the end-effector.

Analytical Jacobian

The relationship between the joint velocities and the corresponding (not uniquely-defined) end-effector linear velocities and time derivatives of any representation of the orientation at given point on the end-effector.

Relation

Assume $\phib = (\gamma, \beta, \alpha)$ is the ZYX Euler angle. The velocity relation is given by

where

The jacobian relation is given correspondingly by

or

MuJoCo Jacobian API

The function for computing geometrical jacobian is:

1
void mj_jac(model, data, jacp, jacr, point, body_index)

The geometrical Jacobian has translational (jacp) and rotational (jacr) components. jacp and jacr point to 3-by-nv matrices you allocated that will hold the result. Each row of this matrix is the gradient of the corresponding 3D coordinate of the specified point with respect to the degrees of freedom. If either is NULL, that Jacobian is not computed. 1

The point is in 3D global (world) coordinates. body is the body index. 2

There are also helper functions mj_jacBody, mj_jacSite, etc that compute the Jacobian of the frame of the specified body, site etc.

Note that the position-dependent stages of the computation must have been executed for the current state in order for these functions to return correct results. So to be safe, do mj_forward and then mj_jac. If you do mj_step and then call mj_jac, the Jacobians will correspond to the state before the integration of positions and velocities took place.

1. MuJoCo API Document
2. MuJoCo Forum - Easiest way to compute Jacobian