Forward kinematics answers one question: given the joint positions qpos, where is every frame of the robot? The answer is a product of rigid transforms, taken from the root of the kinematic tree outward. Each body contributes a fixed offset from its parent, written in the model, and each joint contributes a motion that depends on its coordinate. A frame that matters to you, such as a tool tip, is one more fixed offset at the end of the chain.
MuJoCo keeps the fixed offsets in mjModel and computes the poses in mjData. The function that does it, mj_kinematics, is the first stage of every mj_forward and mj_step, and it is cheap enough to call thousands of times inside an inverse-kinematics or planning loop. Knowing exactly what it computes, and what it leaves alone, prevents a class of silent bugs.
[!established] What
mj_kinematicsfills Fromqposand the model’s offsets: body frames (xpos,xquat,xmat), inertial frames (xipos,ximat), joint anchors and axes in world coordinates (xanchor,xaxis), and geom and site frames (geom_xpos,geom_xmat,site_xpos,site_xmat). Centre-of-mass quantities (subtree_com,cinert, and the motion vectorscdofthat Jacobians are built from) come from the next stage,mj_comPos. Velocities, forces, contacts and sensors come later still.
Move the two joints of the planar arm. The table compares the tip position computed by hand, $L_1\cos q_1 + L_2\cos(q_1+q_2)$ and its $z$ counterpart, with MuJoCo’s site_xpos for the same pose, measured from the shoulder: the difference stays at the level of floating-point rounding everywhere. The joint frames show the chain itself: each frame’s $x$ axis (vermilion) runs along its link, and each joint rotates everything after it. Turn on the workspace layer to see every tip position the joint ranges allow.
```lab fk2 {“dock”: true, “title”: “Forward kinematics of the two-link arm”, “q”: [0.6, -1.2], “height”: 300, “layers”: [“frames”], “table”: “fk”}
## Mathematics
### Homogeneous transforms
A rigid transform with rotation $R$ and translation $p$ acts on a point as $x' = Rx + p$. Written as a $4 \times 4$ matrix acting on $(x, 1)$,
$$T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix}, \qquad T_{ac} = T_{ab}\,T_{bc}, \qquad T^{-1} = \begin{bmatrix} R^\top & -R^\top p \\ 0 & 1\end{bmatrix}.$$
$T_{ab}$ maps coordinates in frame $b$ to frame $a$, and the subscripts chain: an offset written in the child's frame is multiplied on the right.
### The two-link chain
`arm2.xml` puts the shoulder 1 m above the world origin, turns both joints about $-y$, and places each child body at the end of its parent link. With $R_{-y}(q)$ the rotation by $q$ about $-y$ and $\mathrm{Tr}(p)$ a pure translation,
$$T_{\text{ee}} = \mathrm{Tr}(0, 0, 1)\; R_{-y}(q_1)\; \mathrm{Tr}(L_1, 0, 0)\; R_{-y}(q_2)\; \mathrm{Tr}(L_2, 0, 0),$$
whose translation is $(L_1\cos q_1 + L_2 \cos(q_1+q_2),\ 0,\ 1 + L_1\sin q_1 + L_2\sin(q_1+q_2))$, the formulas Lesson 5.1 tested. The script multiplies the five matrices for a thousand poses and compares both the position and the rotation with `site_xpos` and `site_xmat`.
### Any tree, from `mjModel` fields
For a body $b$ with parent $p$, MuJoCo's convention is: start from the parent's frame, apply the body's fixed offset `body_pos`, `body_quat` (in the parent's frame), then apply each of the body's joints in order. A hinge with axis $a$ (in the body frame) through anchor $r$ (`jnt_pos`, also in the body frame) rotates about that anchor, not about the body's origin:
$$x \mapsto r + R(a, q)\,(x - r) = R(a, q)\,x + \big(r - R(a, q)\,r\big).$$
> [!derivation] The update the script uses
> Composing the body frame $(p_b, R_b)$ with that rotation gives $R_b \leftarrow R_b\,R(a, q)$ and $p_b \leftarrow p_b + R_b\,(r - R(a,q)\,r)$. With $r = 0$, the common case, the position is unchanged and only the orientation turns. A site adds one more fixed offset: $p_s = p_b + R_b\,\text{site\_pos}$, $R_s = R_b\,R(\text{site\_quat})$.
Slide joints translate along their axis instead, and ball and free joints read a quaternion (and, for free joints, a position) directly from `qpos` (Lesson 1.2). The script implements the hinge case, which covers `arm7`, and compares it with MuJoCo.
## Implementation
```io
INPUT: arm2.xml, arm7.xml
PROCESS: multiply homogeneous transforms for the two-link arm; rebuild arm7's forward kinematics from mjModel fields; inspect which mjData fields mj_kinematics fills; time the stages of the position pipeline
OUTPUT: printed comparisons and timings
```python file=examples/l6_1_forward_kinematics.py “"”Lesson 6.1: forward kinematics as a chain of transforms, and what mj_kinematics does.
INPUT arm2.xml (planar two-link arm), arm7.xml (7-DOF arm) PROCESS (1) the two-link arm’s end-effector pose from a product of 4x4 homogeneous transforms, against MuJoCo’s site_xpos and site_xmat; (2) arm7’s forward kinematics written from mjModel fields alone (body_pos, body_quat, jnt_pos, jnt_axis, site_pos), against MuJoCo; (3) which mjData fields mj_kinematics fills, which it leaves alone, and the wrong Jacobian you get if you stop there; (4) the cost of each stage of the position pipeline OUTPUT printed comparisons and timings
Run: python examples/l6_1_forward_kinematics.py “””
import os import time
import mujoco import numpy as np
from mjcourse import model_path, spatial
N = 200 if os.environ.get(“MJC_FAST”) == “1” else 1000
def homogeneous(rot: np.ndarray, pos: np.ndarray) -> np.ndarray: t = np.eye(4) t[:3, :3], t[:3, 3] = rot, pos return t
def rot_axis(axis: np.ndarray, angle: float) -> np.ndarray: return spatial.quat_to_mat(spatial.axis_angle_to_quat(axis, angle))
def two_link() -> None: model = mujoco.MjModel.from_xml_path(str(model_path(“arm2”))) data = mujoco.MjData(model) minus_y = np.array([0.0, -1.0, 0.0]) rng = np.random.default_rng(0) worst_p = worst_r = 0.0 for _ in range(N): q = rng.uniform(model.jnt_range[:, 0], model.jnt_range[:, 1]) chain = (homogeneous(np.eye(3), [0, 0, 1.0]) # world -> shoulder (link1’s frame) @ homogeneous(rot_axis(minus_y, q[0]), np.zeros(3)) # shoulder joint @ homogeneous(np.eye(3), [0.5, 0, 0]) # along link 1 to the elbow @ homogeneous(rot_axis(minus_y, q[1]), np.zeros(3)) # elbow joint @ homogeneous(np.eye(3), [0.4, 0, 0])) # along link 2 to the ee site data.qpos[:] = q mujoco.mj_kinematics(model, data) site = model.site(“ee”).id worst_p = max(worst_p, np.abs(chain[:3, 3] - data.site_xpos[site]).max()) worst_r = max(worst_r, np.abs(chain[:3, :3] - data.site_xmat[site].reshape(3, 3)).max()) print(f” {N} random poses: max |position - site_xpos| = {worst_p:.1e} m, “ f”max |rotation - site_xmat| = {worst_r:.1e}”)
def fk_from_model(model: mujoco.MjModel, qpos: np.ndarray, site: str) -> tuple[np.ndarray, np.ndarray]: “"”Site pose for a tree whose joints are all hinges, from mjModel fields only.””” xpos = np.zeros((model.nbody, 3)) xmat = np.zeros((model.nbody, 3, 3)) xmat[0] = np.eye(3) for b in range(1, model.nbody): # bodies are stored parents first p = model.body_parentid[b] pos = xpos[p] + xmat[p] @ model.body_pos[b] # fixed offset from the parent rot = xmat[p] @ spatial.quat_to_mat(model.body_quat[b]) for j in range(model.body_jntadr[b], model.body_jntadr[b] + model.body_jntnum[b]): assert model.jnt_type[j] == mujoco.mjtJoint.mjJNT_HINGE anchor = model.jnt_pos[j] # in the body frame r_joint = rot_axis(model.jnt_axis[j], qpos[model.jnt_qposadr[j]]) pos = pos + rot @ (anchor - r_joint @ anchor) # rotate about the anchor, not the origin rot = rot @ r_joint xpos[b], xmat[b] = pos, rot s = model.site(site).id b = model.site_bodyid[s] return xpos[b] + xmat[b] @ model.site_pos[s], xmat[b] @ spatial.quat_to_mat(model.site_quat[s])
def seven_dof() -> None: model = mujoco.MjModel.from_xml_path(str(model_path(“arm7”))) data = mujoco.MjData(model) rng = np.random.default_rng(1) worst_p = worst_r = 0.0 for _ in range(N): q = rng.uniform(model.jnt_range[:, 0], model.jnt_range[:, 1]) pos, rot = fk_from_model(model, q, “ee”) data.qpos[:] = q mujoco.mj_kinematics(model, data) worst_p = max(worst_p, np.abs(pos - data.site(“ee”).xpos).max()) worst_r = max(worst_r, np.abs(rot - data.site(“ee”).xmat.reshape(3, 3)).max()) print(f” {N} random poses: max |position - site_xpos| = {worst_p:.1e} m, “ f”max |rotation - site_xmat| = {worst_r:.1e}”)
def what_kinematics_fills() -> None: model = mujoco.MjModel.from_xml_path(str(model_path(“arm7”))) q = np.array([0.3, 0.6, -0.4, 1.4, 0.2, 0.9, 0.1]) fields = [“xpos”, “xquat”, “xmat”, “xipos”, “ximat”, “xanchor”, “xaxis”, “site_xpos”, “site_xmat”, “geom_xpos”, “geom_xmat”, “subtree_com”, “cdof”, “cinert”, “qfrc_bias”, “sensordata”] data = mujoco.MjData(model) data.qpos[:] = q mujoco.mj_kinematics(model, data) filled = [f for f in fields if np.abs(getattr(data, f)).max() > 0] empty = [f for f in fields if f not in filled] print(f” fresh MjData, mj_kinematics only:\n filled: {filled}\n still zero: {empty}”) mujoco.mj_comPos(model, data) print(f” after mj_comPos as well: subtree_com, cdof, cinert filled: “ f”{all(np.abs(getattr(data, f)).max() > 0 for f in (‘subtree_com’, ‘cdof’, ‘cinert’))}”)
site = model.site("ee").id
good, stale = np.zeros((3, model.nv)), np.zeros((3, model.nv))
data = mujoco.MjData(model)
data.qpos[:] = model.key_qpos[model.key("home").id]
mujoco.mj_forward(model, data) # everything valid for the home pose
data.qpos[:] = q
mujoco.mj_kinematics(model, data) # new pose: frames updated, cdof is not
mujoco.mj_jacSite(model, data, stale, None, site)
mujoco.mj_comPos(model, data)
mujoco.mj_jacSite(model, data, good, None, site)
print(f" Jacobian after mj_kinematics only (cdof from the previous pose) vs after mj_comPos: "
f"max difference {np.abs(stale - good).max():.3f} m/rad")
def timing() -> None: model = mujoco.MjModel.from_xml_path(str(model_path(“arm7”))) data = mujoco.MjData(model) data.qpos[:] = model.key_qpos[model.key(“home”).id] stages = { “mj_kinematics”: lambda: mujoco.mj_kinematics(model, data), “mj_kinematics + mj_comPos”: lambda: (mujoco.mj_kinematics(model, data), mujoco.mj_comPos(model, data)), “mj_fwdPosition”: lambda: mujoco.mj_fwdPosition(model, data), “mj_forward”: lambda: mujoco.mj_forward(model, data), } n = 5000 if os.environ.get(“MJC_FAST”) == “1” else 50000 for name, fn in stages.items(): fn() t0 = time.perf_counter() for _ in range(n): fn() print(f” {name:<27} {1e6 * (time.perf_counter() - t0) / n:6.2f} us per call (arm7, this machine)”)
if name == “main”: print(“(1) two-link arm: a product of five homogeneous transforms”) two_link() print(“(2) arm7: forward kinematics written from mjModel fields”) seven_dof() print(“(3) what mj_kinematics computes, and what it does not”) what_kinematics_fills() print(“(4) cost of the position pipeline, one call”) timing()
Output:
```text
(1) two-link arm: a product of five homogeneous transforms
1000 random poses: max |position - site_xpos| = 4.4e-16 m, max |rotation - site_xmat| = 7.8e-16
(2) arm7: forward kinematics written from mjModel fields
1000 random poses: max |position - site_xpos| = 8.9e-16 m, max |rotation - site_xmat| = 1.6e-15
(3) what mj_kinematics computes, and what it does not
fresh MjData, mj_kinematics only:
filled: ['xpos', 'xquat', 'xmat', 'xipos', 'ximat', 'xanchor', 'xaxis', 'site_xpos', 'site_xmat', 'geom_xpos', 'geom_xmat']
still zero: ['subtree_com', 'cdof', 'cinert', 'qfrc_bias', 'sensordata']
after mj_comPos as well: subtree_com, cdof, cinert filled: True
Jacobian after mj_kinematics only (cdof from the previous pose) vs after mj_comPos: max difference 0.110 m/rad
(4) cost of the position pipeline, one call
mj_kinematics 0.82 us per call (arm7, this machine)
mj_kinematics + mj_comPos 1.19 us per call (arm7, this machine)
mj_fwdPosition 3.59 us per call (arm7, this machine)
mj_forward 4.78 us per call (arm7, this machine)
Reading the output:
mj_kinematics alone, the frames are filled and the centre-of-mass quantities are still zero. On an MjData that was valid for another pose, they are not zero but stale, which is worse: mj_jacSite then mixes the new frames with the old motion vectors and returns a Jacobian that is off by 0.11 m/rad, with no error raised.mj_kinematics costs under a microsecond for the 7-DOF arm; adding mj_comPos makes a valid Jacobian possible at about 1.2 µs; mj_fwdPosition (which adds collision detection, the mass matrix and its factorization) and mj_forward cost three to four times more. An inverse-kinematics loop needs only the first two.[!recommendation] Call the smallest stage that is valid for what you read Positions and orientations of bodies, geoms and sites:
mj_kinematics. Jacobians (anymj_jac*function):mj_kinematicsthenmj_comPos. Contacts, the mass matrix, or anything else position-dependent:mj_fwdPosition. Accelerations, forces and all sensors:mj_forward. When in doubt,mj_forwardis always correct, at a few microseconds per call for a model this size.
site_xpos did not change after writing qpos. mjData holds the outputs of the last computation; writing qpos computes nothing. Call mj_kinematics (or mj_forward) after every write.
A Jacobian is slightly wrong only after the robot moved. It was computed after mj_kinematics without mj_comPos (output 3). The first call often looks right because the previous pose was close.
Hand-written forward kinematics is off by a constant offset. A body with no joint still has a body_pos, or a joint has a nonzero jnt_pos and rotates about it. Walk the chain body by body and compare xpos at each step.
The orientation is right for one joint and wrong for two. Rotations were composed in the wrong order. A child’s rotation multiplies on the right: $R_\text{child} = R_\text{parent}\,R_\text{local}$.
Extend fk_from_model to the tool-centre point of arm7_gripper.xml (gripper/tcp). The gripper was attached at the flange site, so its base body has a body_pos and body_quat equal to that site’s offset: find them in the model and check your result against site_xpos.
Write a vectorised forward kinematics for arm7 in NumPy that computes the ee pose for 100 000 configurations at once, check it against MuJoCo on a sample, and compare its speed with a loop calling mj_kinematics. Explain the result in terms of what each approach computes per call.
[!research] Kinematics is exact; its inputs are not On a real robot, forward kinematics is only as good as the model’s link lengths and joint offsets; calibration estimates them from measured tool poses, and the residuals are typically far larger than the $10^{-15}$ seen here (inference: manufacturing tolerances and joint-encoder offsets are not modelled at all in a nominal description). For learned policies, the same chain decides what “end-effector observation” means: an observation built from
site_xposafter a stale or skippedmj_kinematicscall is the reset bug of the Debugging clinic (case 11) in another form.
{"id": "6.1-check", "title": "Knowledge check", "questions": [
{"kind": "mcq", "q": "You write a new <code>qpos</code> and call only <code>mj_kinematics</code>. Which of these is valid for the new pose?",
"options": ["<code>subtree_com</code>", "<code>site_xpos</code>", "The result of <code>mj_jacSite</code>", "<code>qfrc_bias</code>"],
"answer": 1,
"explain": "<p>Frames of bodies, geoms and sites are computed by <code>mj_kinematics</code>. Centre-of-mass quantities and the motion vectors used by Jacobians come from <code>mj_comPos</code>; bias forces need the rest of <code>mj_forward</code>.</p>"},
{"kind": "numeric", "q": "For the two-link arm (L1 = 0.5 m, L2 = 0.4 m) at q1 = 0, q2 = π/2, how high is the tip above the shoulder, in metres?",
"answer": 0.4, "tol": 0.0001, "unit": "m",
"explain": "<p>$z = L_1 \\sin 0 + L_2 \\sin(\\pi/2) = 0.4$ m; the tip is at $x = 0.5$ m, directly above the elbow.</p>"},
{"kind": "mcq", "q": "A hinge's anchor <code>jnt_pos</code> is r = (0, 0, 0.1) in its body's frame. Where does a body-frame point x go when the joint turns by R?",
"options": ["R x", "R x + r", "r + R (x − r)", "R (x − r)"],
"answer": 2,
"explain": "<p>Rotation about a point: translate the anchor to the origin, rotate, translate back. Equivalently $Rx + (r - Rr)$, the update in the script.</p>"}
]}
Lesson 6.2 differentiates this chain: the Jacobian maps joint velocities to the tip’s velocity, and its singular values say in which directions the arm can and cannot move.