Forward kinematics gives a pose for each configuration. Its derivative gives a velocity for each joint velocity: the Jacobian $J(q)$ is the matrix with $v = J(q)\,\dot q$. Each column is the velocity of the point when one joint turns at 1 rad/s and the others are still.
Almost everything in the rest of the course goes through $J$. Inverse kinematics inverts it (Lesson 6.3). Operational-space control turns forces at the tool into joint torques with $J^\top$ (Level 8). Contact constraints are built from the Jacobians of contact points (Level 9). Where $J$ loses rank, at a singularity, all of them run into trouble at once.
[!established] MuJoCo’s Jacobians Every
mj_jac*function returns two $3 \times n_v$ matrices:jacp, the linear velocity of a point, andjacr, the angular velocity of the body it is attached to, both in world coordinates, and both multiplyingqvel(so free and ball joints contribute angular-velocity columns, not quaternion derivatives). The functions differ only in the point:mj_jactakes any world point attached to a given body;mj_jacBodyuses the body frame’s origin,mj_jacBodyComits centre of mass,mj_jacSiteandmj_jacGeoma site’s or geom’s frame origin, andmj_jacPointAxisa point plus an axis.mj_jacSubtreeComgives the velocity of a subtree’s centre of mass.mj_jacDotgives $\dot J$. All of them needmj_kinematicsandmj_comPosto have run for the currentqpos(Lesson 6.1).
The two arrows at the tip are the Jacobian’s columns: the tip’s velocity for a unit speed of each joint. The dashed ellipse is every tip velocity you can get from joint velocities of unit length, ${J\dot q : |\dot q| = 1}$. Its axes are the singular values.
mj_jacSite agree to every printed digit.```lab fk2 {“dock”: true, “title”: “Jacobian columns and the manipulability ellipse”, “q”: [0.4, -1.5], “height”: 300, “layers”: [“jacobian”, “ellipse”], “table”: “jacobian”}
## Mathematics
### The two-link Jacobian
Differentiating $x = L_1\cos q_1 + L_2\cos(q_1+q_2)$ and $z = L_1\sin q_1 + L_2\sin(q_1+q_2)$,
$$J(q) = \begin{bmatrix} -L_1 s_1 - L_2 s_{12} & -L_2 s_{12} \\ L_1 c_1 + L_2 c_{12} & L_2 c_{12}\end{bmatrix}, \qquad \det J = L_1 L_2 \sin q_2 ,$$
with $s_{12} = \sin(q_1 + q_2)$ and so on. The determinant vanishes at $q_2 = 0$ (arm straight) and $q_2 = \pm\pi$ (folded back, outside `arm2`'s range). The second column is the velocity the elbow joint gives the tip: perpendicular to the second link, with length $L_2$.
### Singular values and what they cost
With $J = U\Sigma V^\top$, a joint velocity along $v_i$ of unit length moves the tip along $u_i$ at speed $\sigma_i$. To move the tip at speed $s$ along the weakest direction $u_{\min}$ takes $\|\dot q\| = s / \sigma_{\min}$. Yoshikawa's manipulability $w = \sqrt{\det JJ^\top} = \prod_i \sigma_i$ is the area of the ellipse up to $\pi$; for the planar arm $w = |\det J| = L_1 L_2 |\sin q_2|$. Output (2) prints all three for an elbow straightening from $-1.5$ rad to 0: at $q_2 = -0.01$ rad, a 0.1 m/s tip motion along the weak direction needs 49 rad/s at the joints, far beyond any motor.
### Moving a Jacobian along a body
Two points $a$ and $b$ fixed to the same body share its angular velocity $\omega$, and their linear velocities differ by $\omega \times (b - a)$. In matrix form, with $[\cdot]_\times$ the cross-product matrix,
$$J_p(b) = J_p(a) - [\,b - a\,]_\times\, J_r, \qquad J_r(b) = J_r(a).$$
This is why one function, `mj_jac`, suffices for any point: the others are convenience wrappers for common points. Output (3) checks each wrapper against `mj_jac` and the shift rule between two points of `link7`.
### World and local frames
`jacr` gives the angular velocity in world coordinates. In the body's own frame it is $R^\top J_r \dot q$, with $R$ the body's (or site's) rotation matrix. `mj_objectVelocity` returns either, with its `flg_local` argument; output (3) shows both agree with the Jacobian.
### The derivative of the Jacobian
Differentiating $v = J\dot q$ once more, $a = J\ddot q + \dot J \dot q$. The second term is the acceleration of the point when the joints move at constant speed, a centripetal and Coriolis-like effect of the geometry. Task-space controllers that track accelerations need it (Level 8); `mj_jacDot` computes it, and output (4) checks it against a finite difference of $J$ along the motion.
## Implementation
```io
INPUT: arm2.xml, arm7.xml
PROCESS: compare the two-link Jacobian from hand, mj_jacSite and finite differences; tabulate singular values as the elbow straightens; check every mj_jac* wrapper, the shift rule, frames, mj_jacSubtreeCom and mj_jacDot
OUTPUT: printed tables
```python file=examples/l6_2_jacobians.py “"”Lesson 6.2: Jacobians, velocity kinematics and singularities.
INPUT arm2.xml (planar two-link arm), arm7.xml (7-DOF arm) PROCESS (1) the two-link Jacobian derived by hand, from mj_jacSite and from central finite differences, over random poses; (2) singular values, manipulability and the joint speed needed for a 0.1 m/s tip motion as the elbow straightens; (3) the point each mj_jac* function refers to, the rule that moves a Jacobian from one point of a body to another, and world versus local frames; (4) mj_jacDot against a finite difference of J along the motion OUTPUT printed tables
Run: python examples/l6_2_jacobians.py “””
import os
import mujoco import numpy as np
from mjcourse import kinematics, model_path, spatial
N = 200 if os.environ.get(“MJC_FAST”) == “1” else 1000 L1, L2 = 0.5, 0.4
def planar_jacobian(q: np.ndarray) -> np.ndarray: “"”d(x, z)/d(q1, q2) for the two-link arm.””” s1, c1 = np.sin(q[0]), np.cos(q[0]) s12, c12 = np.sin(q[0] + q[1]), np.cos(q[0] + q[1]) return np.array([[-L1 * s1 - L2 * s12, -L2 * s12], [L1 * c1 + L2 * c12, L2 * c12]])
def three_ways() -> None: model = mujoco.MjModel.from_xml_path(str(model_path(“arm2”))) data = mujoco.MjData(model) rng = np.random.default_rng(0) e_mj = e_fd = 0.0 for _ in range(N): q = rng.uniform(model.jnt_range[:, 0], model.jnt_range[:, 1]) data.qpos[:] = q mujoco.mj_kinematics(model, data) mujoco.mj_comPos(model, data) jacp, _ = kinematics.site_jacobian(model, data, “ee”) fdp, _ = kinematics.fd_site_jacobian(model, data, “ee”) hand = planar_jacobian(q) e_mj = max(e_mj, np.abs(jacp[[0, 2]] - hand).max()) e_fd = max(e_fd, np.abs(fdp[[0, 2]] - hand).max()) print(f” {N} random poses: max |mj_jacSite - hand| = {e_mj:.1e}, “ f”max |finite difference (eps 1e-6) - hand| = {e_fd:.1e} m/rad”)
def singular_values() -> None: print(f” {‘q2 (rad)’:>9}{‘sigma1’:>9}{‘sigma2’:>10}{‘w = s1 s2’:>11}{‘L1 L2 |sin q2|’:>16}{‘cond’:>9}{‘|qdot| for 0.1 m/s’:>21}”) for q2 in (-1.5, -0.5, -0.1, -0.01, 0.0): jac = planar_jacobian(np.array([0.4, q2])) u, s, _ = np.linalg.svd(jac) speed = np.inf if s[1] < 1e-12 else 0.1 / s[1] # along the weakest direction u[:, 1] cond = np.inf if s[1] < 1e-12 else s[0] / s[1] print(f” {q2:>9.2f}{s[0]:>9.4f}{s[1]:>10.5f}{s[0] * s[1]:>11.5f}{L1 * L2 * abs(np.sin(q2)):>16.5f}” f”{cond:>9.1f}{speed:>16.2f} rad/s”)
def reference_points() -> None: model = mujoco.MjModel.from_xml_path(str(model_path(“arm7”))) data = mujoco.MjData(model) data.qpos[:] = [0.3, 0.6, -0.4, 1.4, 0.2, 0.9, 0.1] data.qvel[:] = np.random.default_rng(2).normal(size=model.nv) mujoco.mj_forward(model, data) nv, body = model.nv, model.body(“link7”).id site, geom = model.site(“ee”).id, model.geom(“flange”).id
def jac_at(point):
p, r = np.zeros((3, nv)), np.zeros((3, nv))
mujoco.mj_jac(model, data, p, r, point, body)
return p, r
cases = {
"mj_jacBody -> xpos (body frame origin)": (mujoco.mj_jacBody, body, data.xpos[body]),
"mj_jacBodyCom -> xipos (body centre of mass)": (mujoco.mj_jacBodyCom, body, data.xipos[body]),
"mj_jacSite -> site_xpos": (mujoco.mj_jacSite, site, data.site_xpos[site]),
"mj_jacGeom -> geom_xpos": (mujoco.mj_jacGeom, geom, data.geom_xpos[geom]),
}
for name, (fn, idx, point) in cases.items():
p, r = np.zeros((3, nv)), np.zeros((3, nv))
fn(model, data, p, r, idx)
ref_p, ref_r = jac_at(point)
print(f" {name:<46} equals mj_jac at that point: {np.abs(p - ref_p).max():.0e}, rotation part {np.abs(r - ref_r).max():.0e}")
a, b = data.xpos[body], data.site_xpos[site]
pa, ra = jac_at(a)
pb, _ = jac_at(b)
shifted = pa - spatial.hat(b - a) @ ra # v_b = v_a + w x (b - a)
print(f" shift rule J_p(b) = J_p(a) - [b - a]x J_r, link7 origin -> ee site: max error {np.abs(shifted - pb).max():.0e}")
sub = np.zeros((3, nv))
mujoco.mj_jacSubtreeCom(model, data, sub, model.body("link4").id)
mujoco.mj_subtreeVel(model, data)
print(f" mj_jacSubtreeCom(link4) @ qvel = {np.round(sub @ data.qvel, 5)}, subtree_linvel after mj_subtreeVel = "
f"{np.round(data.subtree_linvel[model.body('link4').id], 5)} m/s")
_, r = np.zeros((3, nv)), np.zeros((3, nv))
mujoco.mj_jacSite(model, data, None, r, site)
w_world = r @ data.qvel
vel = np.zeros(6)
mujoco.mj_objectVelocity(model, data, mujoco.mjtObj.mjOBJ_SITE, site, vel, 0)
vel_local = np.zeros(6)
mujoco.mj_objectVelocity(model, data, mujoco.mjtObj.mjOBJ_SITE, site, vel_local, 1)
rot = data.site_xmat[site].reshape(3, 3)
print(f" angular velocity: J_r qvel (world) vs mj_objectVelocity world: {np.abs(w_world - vel[:3]).max():.0e}; "
f"R^T J_r qvel vs local: {np.abs(rot.T @ w_world - vel_local[:3]).max():.0e} rad/s")
def jac_dot() -> None: model = mujoco.MjModel.from_xml_path(str(model_path(“arm7”))) data = mujoco.MjData(model) data.qpos[:] = [0.3, 0.6, -0.4, 1.4, 0.2, 0.9, 0.1] data.qvel[:] = np.random.default_rng(3).normal(size=model.nv) mujoco.mj_forward(model, data) site, nv = model.site(“ee”).id, model.nv dp, dr = np.zeros((3, nv)), np.zeros((3, nv)) mujoco.mj_jacDot(model, data, dp, dr, data.site_xpos[site], model.site_bodyid[site])
def site_jac(qpos):
d = mujoco.MjData(model)
d.qpos[:] = qpos
mujoco.mj_kinematics(model, d)
mujoco.mj_comPos(model, d)
return kinematics.site_jacobian(model, d, site)
h = 1e-6
q_plus, q_minus = data.qpos.copy(), data.qpos.copy()
mujoco.mj_integratePos(model, q_plus, data.qvel, h)
mujoco.mj_integratePos(model, q_minus, data.qvel, -h)
(pp, rp), (pm, rm) = site_jac(q_plus), site_jac(q_minus)
print(f" max |mj_jacDot - (J(q + h qdot) - J(q - h qdot)) / 2h|: translation {np.abs(dp - (pp - pm) / (2 * h)).max():.1e}, "
f"rotation {np.abs(dr - (rp - rm) / (2 * h)).max():.1e}")
print(f" the velocity-product term Jdot qdot at the ee site: {np.round(dp @ data.qvel, 4)} m/s^2")
if name == “main”: print(“(1) the two-link Jacobian three ways”) three_ways() print(“(2) singular values as the elbow straightens (q1 = 0.4 rad)”) singular_values() print(“(3) reference points and frames of MuJoCo’s Jacobians (arm7, link7)”) reference_points() print(“(4) the time derivative of the Jacobian”) jac_dot()
Output:
```text
(1) the two-link Jacobian three ways
1000 random poses: max |mj_jacSite - hand| = 4.4e-16, max |finite difference (eps 1e-6) - hand| = 2.9e-10 m/rad
(2) singular values as the elbow straightens (q1 = 0.4 rad)
q2 (rad) sigma1 sigma2 w = s1 s2 L1 L2 |sin q2| cond |qdot| for 0.1 m/s
-1.50 0.7225 0.27611 0.19950 0.19950 2.6 0.36 rad/s
-0.50 0.9544 0.10046 0.09589 0.09589 9.5 1.00 rad/s
-0.10 0.9837 0.02030 0.01997 0.01997 48.5 4.93 rad/s
-0.01 0.9849 0.00203 0.00200 0.00200 485.0 49.24 rad/s
0.00 0.9849 0.00000 0.00000 0.00000 inf inf rad/s
(3) reference points and frames of MuJoCo's Jacobians (arm7, link7)
mj_jacBody -> xpos (body frame origin) equals mj_jac at that point: 0e+00, rotation part 0e+00
mj_jacBodyCom -> xipos (body centre of mass) equals mj_jac at that point: 0e+00, rotation part 0e+00
mj_jacSite -> site_xpos equals mj_jac at that point: 0e+00, rotation part 0e+00
mj_jacGeom -> geom_xpos equals mj_jac at that point: 0e+00, rotation part 0e+00
shift rule J_p(b) = J_p(a) - [b - a]x J_r, link7 origin -> ee site: max error 1e-16
mj_jacSubtreeCom(link4) @ qvel = [0.08696 0.07185 0.76403], subtree_linvel after mj_subtreeVel = [0.08696 0.07185 0.76403] m/s
angular velocity: J_r qvel (world) vs mj_objectVelocity world: 9e-16; R^T J_r qvel vs local: 9e-16 rad/s
(4) the time derivative of the Jacobian
max |mj_jacDot - (J(q + h qdot) - J(q - h qdot)) / 2h|: translation 1.9e-10, rotation 7.9e-10
the velocity-product term Jdot qdot at the ee site: [-8.6736 -0.6603 1.5403] m/s^2
Reading the output:
| (2) $\sigma_1 \sigma_2$ equals $L_1 L_2 | \sin q_2 | $ in every row. $\sigma_1$ barely changes while $\sigma_2$ falls in proportion to $q_2$: the singularity removes one direction and leaves the other intact. |
mj_jac at the point named in its row, exactly (zero difference, the same arithmetic); the shift rule holds to $10^{-16}$; mj_jacSubtreeCom times qvel equals subtree_linvel after mj_subtreeVel; and the angular velocity agrees with mj_objectVelocity in both frames.mj_jacDot agrees with the finite difference to $10^{-10}$. For this random joint velocity, $\dot J\dot q$ at the tool is several m/s², the same order as gravity: not a correction to ignore in fast motions.IK or control blows up near one pose. Print the singular values of $J$ there. If $\sigma_{\min}$ is near zero, any method that inverts $J$ demands huge joint speeds; damping (Lesson 6.3) is the standard fix.
The tool’s angular velocity is “wrong by a rotation”. jacr is in world coordinates. If you compare with a gyroscope or a body-frame velocity, rotate it: $R^\top J_r \dot q$.
A Jacobian check against finite differences fails at $10^{-6}$. The step is too large or too small for the problem’s scale, or the perturbation was applied to qpos by addition for a free or ball joint. Perturb with mj_integratePos, as kinematics.fd_site_jacobian does, and use central differences.
A free-floating object’s Jacobian has fewer columns than its qpos has entries. Correctly so: Jacobians multiply qvel ($n_v$ entries), not qpos ($n_q$). A free joint has 7 position and 6 velocity coordinates.
For arm7 at the home pose, compute the $6 \times 7$ Jacobian of the ee site, scale its first three rows by $1/0.5$ m (a characteristic reach), and compare the singular values with the unscaled ones from Lesson 5.2. Which conclusions about “how close to singular” survive the change of units?
Draw the planar arm’s manipulability $w(q_1, q_2)$ over the whole joint range as a heat map, mark the singular set, and overlay the workspace in Cartesian coordinates with each tip position coloured by $w$. Then do the same for arm7’s position Jacobian on a slice of its workspace. Where would you put a task that needs fast motion in every direction?
[!research] Learning in the null space and away from singularities Policies that output end-effector velocities rely on the controller to invert $J$, so their behaviour near singular poses is the controller’s, not the policy’s (Lesson 5.2). Manipulability and its gradient appear in learning as auxiliary rewards and as constraints in motion generation; this course’s position is that such terms are design choices to be tested, not defaults, since $w$ depends on the units of the task and on which rows of $J$ are included (inference from the scaling exercise above).
{"id": "6.2-check", "title": "Knowledge check", "questions": [
{"kind": "numeric", "q": "For the two-link arm (L1 = 0.5 m, L2 = 0.4 m), what is det J at q2 = π/2?",
"answer": 0.2, "tol": 0.0001, "unit": "m^2/rad^2",
"explain": "<p>$\\det J = L_1 L_2 \\sin q_2 = 0.2$: the largest it can be, with the links perpendicular.</p>"},
{"kind": "mcq", "q": "A body rotates with angular velocity ω. Point a on it moves at v_a. What is the velocity of point b on the same body?",
"options": ["v_a", "v_a + ω × (b − a)", "v_a − ω × (b − a)", "ω × b"],
"answer": 1,
"explain": "<p>Rigid-body velocity transfer. In Jacobian form, $J_p(b) = J_p(a) - [b - a]_\\times J_r$, because $\\omega \\times r = -r \\times \\omega = -[r]_\\times \\omega$.</p>"},
{"kind": "numeric", "q": "At a pose where the smallest singular value of the position Jacobian is 0.02 m/rad, what joint speed (rad/s) does a tip motion of 0.1 m/s along the corresponding direction require?",
"answer": 5, "tol": 0.01, "unit": "rad/s",
"explain": "<p>$\\|\\dot q\\| = s / \\sigma_{\\min} = 0.1/0.02$ = 5 rad/s.</p>"},
{"kind": "mcq", "q": "Which MuJoCo Jacobian function would you use for the velocity of a fingertip contact point that is neither a site nor a geom centre?",
"options": ["mj_jacSite", "mj_jacBody", "mj_jac with the point and the fingertip's body", "mj_jacSubtreeCom"],
"answer": 2,
"explain": "<p><code>mj_jac</code> takes any world point attached to a body. The others are fixed choices of point.</p>"}
]}
Lesson 6.3 inverts the map: given a desired tool pose, find joint positions, iterating with the Jacobian and handling singularities, joint limits and redundancy.