Robot manipulation: kinematics, dynamics, and operational-space control

Why this subject matters now

The kinematics and dynamics of open chains were essentially settled by the 1980s: Denavit and Hartenberg gave the parameter convention in 1955, Whitney gave resolved-rate control in 1969, Khatib gave the operational-space formulation in 1987, and the recursive Newton–Euler and articulated-body algorithms of Featherstone made whole-body dynamics cheap. What changed in the last few years is not the theory but the tooling and the scale at which the theory runs. A practitioner today is expected to reach for a differentiable, GPU-parallel physics engine, MuJoCo through its XLA backend, Isaac Gym, or Brax, and to run thousands of simulated arms in lockstep to train a policy, then deploy that policy on hardware whose low-level controller still solves the same rigid-body equations at a kilohertz. The analytical-derivatives work in Pinocchio and the GPU minimum-jerk planning in NVIDIA's cuRobo exist precisely because the classical equations are now inside the inner loop of learning and optimization, differentiated and vectorized rather than merely evaluated.

The consequence is that the old distinction between a “controls person” who knew the Jacobian and a “learning person” who knew gradients has collapsed. A modern manipulation engineer is expected to derive the operational-space inertia matrix and to know why autodiff through the forward kinematics gives the geometric Jacobian for free; to know that a damped-least-squares step is a Levenberg–Marquardt step and why that keeps joint velocities bounded near a singularity; and to know which of these quantities a foundation-model policy is implicitly re-learning from pixels and which it is better to hand it as a known model. This page is the derivation of the model. It assumes comfort with linear algebra and calculus and nothing about robotics.

Core theory

Rigid-body rotation: the group SO(3) and the exponential map

A rotation of three-dimensional space is a linear map that preserves lengths and orientation. Written as a matrix \( R \) it satisfies \( R\T R = I \) and \( \det R = +1 \). The set of such matrices is the special orthogonal group \( SO(3) \). It is a group under matrix multiplication (the product of two rotations is a rotation, the inverse is \( R^{-1} = R\T \)), but it is not a vector space: the sum of two rotation matrices is not a rotation. This single fact, that orientations do not add, is the source of most of the subtlety in the subject.

To parameterize \( SO(3) \) we ask what velocities a rotating body can have. Differentiate \( R(t)\T R(t) = I \) to get \( \dot R\T R + R\T \dot R = 0 \), so \( R\T \dot R \) is skew-symmetric. Every \( 3\times 3 \) skew-symmetric matrix is the “hat” of a vector \( \omega \in \R^3 \),

So \( \dot R = R\,[\omega_b]_\times \) for a body-frame angular velocity \( \omega_b \). Holding \( \omega \) constant, this is a linear matrix ODE whose solution is the matrix exponential \( R(t) = R(0)\exp(t[\omega]_\times) \). The map from the skew-symmetric matrices (the Lie algebra \( \mathfrak{so}(3) \)) to \( SO(3) \) is therefore the exponential. We can sum the series in closed form. Let \( \omega \) be a unit vector and \( W = [\omega]_\times \). A direct computation gives the key identity \( W^3 = -W \), because \( W^2 = \omega\omega\T - I \) and \( W^3 = W(\omega\omega\T - I) = -W \) (using \( W\omega = 0 \)). The powers therefore cycle: \( W, W^2, -W, -W^2, W, \dots \) Grouping the exponential series by these two matrices,

This is Rodrigues' formula. It says a rotation by angle \( \theta \) about a unit axis \( \omega \) is built from the identity, a \( \sin\theta \) piece in the plane orthogonal to the axis, and a \( (1-\cos\theta) \) piece. The benchmark evaluates the right-hand side and, independently, the truncated matrix-exponential series to twenty-four terms for \( \omega = (1,2,2)/3 \) and \( \theta = 0.7 \); the two agree to \( 0.0 \) at printed precision, confirming the closed form is the exact sum. The inverse map (the logarithm) recovers the axis and angle: \( \theta = \arccos\!\big((\tr R - 1)/2\big) \), and the axis is read from the skew part \( (R - R\T)/(2\sin\theta) \).

Because \( SO(3) \) is a nonabelian group, rotations do not commute. Taking \( R_1 \) as a \( 90^\circ \) rotation about \( z \) and \( R_2 \) as a \( 90^\circ \) rotation about \( y \), the benchmark reports \( \lVert R_1 R_2 - R_2 R_1 \rVert_F = 2.449489743 = \sqrt 6 \): the order of application matters, and by a lot. This is exactly why one cannot treat a three-vector of Euler angles as if it lived in a vector space, and why the Lie-group view, composing on the manifold rather than adding coordinates, is the right one.

Quaternions: a double cover of SO(3)

A unit quaternion \( q = (w, x, y, z) \) with \( w^2+x^2+y^2+z^2 = 1 \) encodes the same rotation with four numbers and no trigonometric evaluation in the composition. A rotation by \( \theta \) about unit axis \( \omega \) is \( q = (\cos\tfrac\theta2,\ \omega\sin\tfrac\theta2) \). The corresponding matrix is

and composition of rotations is quaternion multiplication (the Hamilton product). Note that \( q \) and \( -q \) give the same \( R \): the unit quaternions are a double cover of \( SO(3) \), which is what lets them avoid the gimbal-lock singularities of any three-parameter representation. Quaternions are the standard state for orientation in flight controllers, game engines, and IMU fusion because they interpolate cleanly (SLERP) and renormalize with a single division. The benchmark converts \( R_1 \) and \( R_2 \) to quaternions, multiplies them, converts back, and compares against \( R_1 R_2 \): the maximum elementwise error is \( 0.0 \), confirming the homomorphism holds numerically.

Rigid-body motion: SE(3), twists, screws, and the adjoint

A full rigid displacement combines a rotation \( R \) and a translation \( p \), stacked into a homogeneous transform that acts on points \( \tilde x = (x, 1) \):

These compose by matrix multiplication, \( T_{ab} T_{bc} = T_{ac} \), which is why homogeneous coordinates are used: a chain of frames becomes a product. The benchmark composes \( T_1 \) (the \( z \)-rotation with translation \( (0.3, 0, 0.1) \)) and \( T_2 \) (the \( y \)-rotation with translation \( (0, 0.2, 0.4) \)) and maps the point \( (1, 0, 0) \) expressed in frame 2 into frame 0, obtaining \( (0.1,\ 0.0,\ -0.5) \). The rotation part carries the direction and the translation part shifts it; verifying this by hand is a good check that one has the active-versus-passive convention straight.

The velocity of a rigid body is a twist \( \mathcal V = (\omega, v) \in \R^6 \): an angular part and a linear part. Just as \( \dot R = R[\omega]_\times \), the full kinematics is \( \dot T = T [\mathcal V]_{se(3)} \) with the \( 4\times4 \) bracket \( [\mathcal V] = \left[\begin{smallmatrix} [\omega]_\times & v \\ 0 & 0 \end{smallmatrix}\right] \). A constant twist integrates to the matrix exponential on \( SE(3) \). A unit twist is a screw, a rotation about and translation along a fixed axis in space (Chasles' theorem: every rigid displacement is a screw motion). The exponential of a screw \( S = (\omega, v) \) through angle \( \theta \), for \( \lVert\omega\rVert = 1 \), is

To move a twist between frames one uses the adjoint of a transform, the \( 6\times6 \) map

The adjoint is exactly the change of basis for twists, and it is what makes the product-of-exponentials Jacobian below fall out cleanly. The benchmark checks the defining identity \( [\mathrm{Ad}_T \mathcal V] = T[\mathcal V]T^{-1} \) for a random twist and reports a maximum error of \( 0.0 \).

Forward kinematics I: Denavit–Hartenberg

Forward kinematics is the map from joint variables \( q \) to the pose of the end-effector \( T(q) \). The classical bookkeeping is the Denavit–Hartenberg convention, which attaches a frame to each link so that the transform between consecutive frames needs only four numbers per joint: a link length \( a_i \), a link twist \( \alpha_i \), a link offset \( d_i \), and a joint angle \( \theta_i \). The transform is a fixed product of two screws about \( x \) and two about \( z \),

and the end-effector pose is the product \( T = \prod_i T_{i-1,i} \). DH is compact and universal in industrial documentation, but the frame-assignment rules are fiddly and non-unique, and the parameters have no direct geometric meaning once the arm has offset or intersecting axes. This is the historical default and worth being able to read, but it is not how the derivations below proceed.

Forward kinematics II: the product of exponentials

The product-of-exponentials (PoE) formula, developed in the Lie-group treatment of Brockett and made standard by Murray, Li, and Sastry and by Lynch and Park, needs no intermediate link frames. Fix the arm at a home configuration \( q = 0 \) where the end-effector pose is \( M \in SE(3) \). Each joint \( i \) is a screw axis \( S_i = (\omega_i, v_i) \) expressed in the fixed space frame, with \( v_i = -\omega_i \times r_i \) for a point \( r_i \) on the axis. Actuating joint \( i \) by \( q_i \) applies the screw motion \( e^{[S_i]q_i} \) to everything outboard of it. Because the space-frame axis of an inboard joint does not move when an outboard joint turns, the poses multiply in order:

The derivation is a short induction. With only joint \( n \) active the end-effector moves by \( e^{[S_n]q_n} M \). Adding joint \( n-1 \) applies a further space-frame screw to that whole result, \( e^{[S_{n-1}]q_{n-1}} \) on the left, because \( S_{n-1} \) is fixed in space and the motion it induces acts on all outboard bodies rigidly. Iterating inboard gives the product. The strength of PoE is that each \( S_i \) is a single geometric object, the screw axis in the home pose, that one reads directly off the mechanism with no frame conventions.

The benchmark builds a six-revolute spatial arm from six screw axes and a home pose \( M \) with the hand at \( (1.05, 0, 0.4) \), and evaluates \( T(q) \) at \( q = (0.3, -0.5, 0.8, 0.2, -0.4, 0.6) \). The resulting hand position is \( (0.855314769,\ 0.214115838,\ 0.392508853) \) with the rotation block printed in full; the home pose \( T(0) = M \) is recovered exactly, as it must be since every exponential is the identity at \( q = 0 \).

The manipulator Jacobian

Differentiating the forward kinematics gives the Jacobian, the linear map from joint velocities to end-effector velocity. For a task map \( p = f(q) \) the analytic Jacobian is simply \( J_a = \partial f/\partial q \). For the full pose one usually wants the geometric Jacobian \( J \) that maps \( \dot q \) to the spatial twist \( \mathcal V = (\omega, v) \). Both are central: \( J_a \) for a task specified in some coordinate chart, \( J \) for velocities, forces, and the dynamics.

Derive it for the two-link arm. From \( p(q) = \big(\ell_1 c_1 + \ell_2 c_{12},\ \ell_1 s_1 + \ell_2 s_{12}\big) \) with the shorthand \( c_1 = \cos q_1,\ c_{12} = \cos(q_1+q_2) \), differentiate each component with respect to \( q_1 \) and \( q_2 \):

At \( q = (30^\circ, 60^\circ) \) this is \( \left[\begin{smallmatrix} -1.5 & -1.0 \\ 0.866025 & 0 \end{smallmatrix}\right] \), matching the benchmark. A central-difference of the forward kinematics agrees with the closed form to a maximum error of \( 0.0 \). For the six-revolute spatial arm the geometric (space) Jacobian comes directly from PoE: its \( i \)-th column is the \( i \)-th screw axis carried forward by the product of the inboard exponentials,

This is the adjoint earning its place: each column is the instantaneous screw of a joint, transported into the current configuration. The benchmark computes this analytic space Jacobian and, independently, the Jacobian by forward-mode autodiff of the PoE forward kinematics (differentiate \( T(q) \), right-multiply by \( T^{-1} \), and un-hat the resulting \( se(3) \) element). The two agree to a maximum absolute difference of \( 0.0 \): analytic geometry and automatic differentiation compute the same object. Over 512 random configurations of the three-link arm the worst-case autodiff-versus-analytic disagreement is likewise \( 0.0 \). What the autodiff cannot beat is speed here: the JIT-compiled JAX Jacobian takes \( 144.9\,\mu\text{s} \) per call against \( 209.7\,\mu\text{s} \) for the pure-Python analytic loop, but a hand-written vectorized analytic Jacobian, or a compiled one from Pinocchio, is far faster than either.

Singularities and manipulability

The Jacobian loses rank at a singularity, a configuration where the arm cannot move its end-effector in some direction no matter what joint velocities are applied. For the two-link arm the determinant is

using the angle-difference identity \( s_{12}c_1 - c_{12}s_1 = \sin\big((q_1{+}q_2) - q_1\big) = \sin q_2 \). At \( q = (30^\circ, 60^\circ) \) this gives \( \sin 60^\circ = 0.866025 \), which the benchmark confirms equals the numerically computed determinant. The arm is singular exactly when \( q_2 = 0 \) (elbow straight) or \( q_2 = \pi \) (elbow folded back): with the elbow straight the two links are collinear and the hand can only move perpendicular to the arm, not along it. At the straight configuration \( q = (0.4, 0) \) the singular values of \( J \) are \( (2.236,\ 0) \), the manipulability measure \( w = \sqrt{\det(J J\T)} \) collapses to \( 3.5\times10^{-8} \approx 0 \), and the left-singular vector for the zero singular value is the direction along the arm that has become unreachable.

The right way to quantify how close a configuration is to singular is not the determinant but the singular values. Yoshikawa's manipulability ellipsoid is the image of the unit joint-velocity sphere under \( J \); its semi-axes are the singular values \( \sigma_i \) along the left-singular vectors. The volume-like measure \( w = \sqrt{\det(J J\T)} = \prod_i \sigma_i \) and the condition number \( \kappa = \sigma_{\max}/\sigma_{\min} \) both report proximity to a singularity, but \( \kappa \) is the one that governs numerical behavior. Sweeping the elbow angle, the benchmark shows \( w \) peaking near \( q_2 = 90^\circ \) (the best-conditioned, most dexterous posture) and falling to zero at the workspace boundary. As \( q_2 \to 0 \) the smallest singular value shrinks in proportion to \( q_2 \) while \( \sigma_{\max} \) stays near \( 2 \); at \( q_2 = 0.03^\circ \) the condition number reaches \( \sim 3800 \) and the norm of the pseudo-inverse, hence the joint velocity needed for a unit hand velocity, blows up in step. This is the concrete danger that damped least squares below is designed to defuse.

Redundancy, the null space, and statics

When the arm has more joints than task dimensions the Jacobian is wide and its null space is nonempty: there are joint velocities that produce no end-effector motion, the arm's self-motion. For the three-link planar arm on a two-dimensional position task, \( J \in \R^{2\times3} \) has a one-dimensional null space. The projector onto it is

where \( J^{+} \) is the Moore–Penrose pseudo-inverse. The benchmark confirms \( N \) is a rank-one projector: idempotent (\( N^2 = N \) to \( 10^{-9} \)), symmetric, and annihilated by \( J \) (so \( J N = 0 \)). Moving the joints a small step along the null direction shifts the end-effector by only \( 5.5\times10^{-4} \) for a joint step of \( 0.05 \), a second-order residual from the linearization, confirming this is genuine self-motion. Redundancy resolution exploits exactly this: pick the least-norm joint velocity that achieves the task, \( J^{+}\dot p \), then add any null-space term \( N z \) to satisfy a secondary objective (avoid a joint limit, stay near a comfortable posture) without disturbing the hand.

The transpose of the Jacobian carries forces the other direction. Static equilibrium equates the virtual work done by an end-effector wrench \( f \) against a joint torque \( \tau \): \( \tau\T \delta q = f\T \delta p = f\T J \delta q \) for all \( \delta q \), hence \( \tau = J\T f \). The benchmark applies \( f = (3, -2) \) to the three-link arm and reads off the joint torques, and verifies the two sides of the virtual-work identity agree (\( -1.478\times10^{-4} \) on both). That \( J\T \) maps task forces to joint torques is the entire basis of operational-space control below.

Inverse kinematics: the closed-form two-link solution

Inverse kinematics asks the harder question: given a desired hand pose, find joint angles that reach it. For the two-link arm it has a clean closed form with exactly the geometry one expects. Squaring and adding the position equations, the cross terms give \( 2\ell_1\ell_2\cos q_2 \), so with \( r^2 = x^2 + y^2 \),

The \( \pm \) is the crux: for a reachable interior point there are two solutions, elbow-up and elbow-down, mirror images across the line from the shoulder to the hand. The sign of \( \sin q_2 \) picks the branch; \( q_1 \) then follows by subtracting the elbow's contribution from the direction to the target. A target is reachable iff \( |\ell_1 - \ell_2| \le r \le \ell_1 + \ell_2 \); on the outer boundary the two solutions merge (the arm is straight, a singularity), and outside it \( \cos q_2 \) exceeds one and there is no solution. The benchmark solves for the target \( (1.2, 0.7) \): elbow-down \( (-15.75^\circ,\ 92.01^\circ) \) and elbow-up \( (76.26^\circ,\ -92.01^\circ) \), both with residual \( 0.0 \).

Numerical inverse kinematics and damped least squares

Closed forms exist only for special geometries (and for six-axis arms with a spherical wrist, via Pieper's decomposition). In general one solves IK numerically as a root-find on the residual \( e(q) = x_d - f(q) \), whose first-order model is \( e(q + \delta q) \approx e(q) - J\,\delta q \). Setting the model residual to zero and solving gives a step. Three choices of solve trade robustness against cost and behavior near singularities.

The Jacobian transpose takes \( \delta q = \alpha J\T e \). This is gradient descent on \( \tfrac12 \lVert e\rVert^2 \), since the gradient of that objective is \( -J\T e \). It needs no matrix solve and never blows up, but it converges slowly and its rate depends on the conditioning of \( J\T J \). The optimal line-search step \( \alpha = (e\T J J\T e)/(e\T J J\T J J\T e) \) helps but does not fix the slowness. The pseudo-inverse (Newton / resolved-rate) step \( \delta q = J^{+} e \) is the least-norm exact solution of the linearized system and converges quadratically near the root, but \( J^{+} = J\T(J J\T)^{-1} \) has entries that diverge as \( \sigma_{\min} \to 0 \), so near a singularity it commands enormous joint velocities.

Damped least squares (Nakamura–Hanafusa 1986, Wampler 1986) fixes this by regularizing. Instead of solving \( J\,\delta q = e \) exactly, minimize a penalized residual that also charges for large steps:

Setting the gradient \( 2 J\T(J\,\delta q - e) + 2\lambda^2 \delta q = 0 \) gives the normal equations \( (J\T J + \lambda^2 I)\,\delta q = J\T e \), so \( \delta q = (J\T J + \lambda^2 I)^{-1} J\T e \). This is precisely a Levenberg–Marquardt step for the nonlinear least-squares problem \( \min_q \tfrac12\lVert x_d - f(q)\rVert^2 \): the damping \( \lambda^2 \) interpolates between the Gauss–Newton step (\( \lambda \to 0 \), the pseudo-inverse) and a small gradient-descent step (\( \lambda \) large, the transpose). Using the push-through identity \( (J\T J + \lambda^2 I)^{-1} J\T = J\T(J J\T + \lambda^2 I)^{-1} \), which one proves by left- and right-multiplying to clear the inverses, the step is usually written in the smaller task space,

The regularization changes the singular-value response from \( 1/\sigma_i \) to \( \sigma_i/(\sigma_i^2 + \lambda^2) \), which is bounded by \( 1/(2\lambda) \) and rolls off smoothly to zero as \( \sigma_i \to 0 \). Near a singularity the arm therefore gives up motion in the ill-conditioned direction rather than commanding a huge velocity to force it, at the cost of a small tracking error of order \( \lambda \) in that direction. This is the standard filter (Chiaverini, Siciliano, and Egeland 1994 give the definitive treatment with hardware experiments).

The benchmark runs all three methods to a \( 10^{-6} \) tolerance from twenty random starts across a range of targets, with a step-norm clamp of \( 0.3 \) for stability. In mid-workspace all three converge every time, but at very different cost: the pseudo-inverse takes a median of \( 9 \) iterations and DLS \( 10 \), while the transpose needs \( 25 \). The gap widens near the boundary. At radius \( 1.95 \) the transpose needs a median of \( 627 \) iterations while the pseudo-inverse and DLS need \( 13 \) and \( 17 \); at radius \( 1.99 \) the transpose converges in only \( 2 \) of \( 20 \) trials within the iteration cap while DLS still converges all \( 20 \). For a genuinely unreachable target at radius \( 2.05 \) every method fails to reach tolerance, as it must, and DLS degrades gracefully to the nearest reachable point rather than diverging.

A damping sweep at the near-singular target (radius \( 1.999 \)) makes the trade-off precise. Undamped (\( \lambda = 0 \)) the raw Newton step reaches a norm of \( 129 \) before the clamp catches it, and although it converges in a median of \( 17 \) iterations it does so by relying on the clamp to survive the singularity. As \( \lambda \) grows the raw step norm falls, from \( 129 \) at \( \lambda = 0 \) toward \( 6.4 \) at \( \lambda = 0.3 \), which is exactly the bounded velocity DLS promises, but the iteration count climbs, from \( 17 \) to \( 102 \) at \( \lambda = 0.1 \) and \( 789 \) at \( \lambda = 0.3 \), because heavy damping slows every step. The engineering answer, and what production code does, is to make \( \lambda \) adaptive: near zero when well-conditioned, ramped up only as \( \sigma_{\min} \) drops below a threshold.

Lagrangian dynamics of a two-link arm

Kinematics says where the arm is; dynamics says how torque produces motion. The Euler–Lagrange equations from the Lagrangian \( L = \mathcal{T} - \mathcal{U} \) (kinetic minus potential energy) give, for any open chain, the canonical form

where \( M \) is the symmetric positive-definite mass (inertia) matrix, \( C\dot q \) collects the Coriolis and centrifugal terms, and \( g \) is the gravity torque. Derive each for the two-link arm with link masses \( m_i \), lengths \( \ell_i \), center-of-mass distances \( \ell_{c_i} \), and centroidal inertias \( I_i \). The center of mass of link 1 sits at \( (\ell_{c_1}c_1,\ \ell_{c_1}s_1) \) and moves with speed \( \ell_{c_1}\dot q_1 \); its kinetic energy is \( \tfrac12(m_1 \ell_{c_1}^2 + I_1)\dot q_1^2 \). Link 2's center of mass is

Its squared speed expands, using \( c_1 c_{12} + s_1 s_{12} = \cos q_2 \), to \( \ell_1^2\dot q_1^2 + \ell_{c_2}^2(\dot q_1{+}\dot q_2)^2 + 2\ell_1\ell_{c_2}\cos q_2\,\dot q_1(\dot q_1{+}\dot q_2) \). Adding the rotational term \( \tfrac12 I_2(\dot q_1 + \dot q_2)^2 \) and collecting the total kinetic energy as \( \mathcal{T} = \tfrac12 \dot q\T M(q)\dot q \), the mass matrix reads

With uniform rods, \( m_i = 1,\ \ell_i = 1,\ \ell_{c_i} = 0.5,\ I_i = 1/12 \), the diagonal-plus-coupling structure at \( q_2 = 60^\circ \) (so \( \cos q_2 = 0.5 \)) evaluates to

exactly the benchmark's value. Its eigenvalues \( (0.1635,\ 2.3365) \) are both positive, and sweeping \( q_2 \) over a full turn the smallest eigenvalue never drops below \( 0.0663 \): \( M \) is uniformly positive definite, which is what makes \( \ddot q = M^{-1}(\tau - C\dot q - g) \) always well posed.

The Coriolis matrix comes from the Christoffel symbols of the mass matrix, \( C_{ij} = \sum_k \tfrac12\big(\partial_{q_k}M_{ij} + \partial_{q_j}M_{ik} - \partial_{q_i}M_{jk}\big)\dot q_k \). Only \( q_2 \) enters \( M \), through the single quantity \( h = -m_2\ell_1\ell_{c_2}\sin q_2 \), which collapses the algebra to

At \( q_2 = 60^\circ \), \( h = -0.5\sin 60^\circ = -0.433013 \). With \( \dot q = (0.5, -0.8) \) the benchmark reports \( C = \left[\begin{smallmatrix} 0.346410 & 0.129904 \\ 0.216506 & 0 \end{smallmatrix}\right] \) and \( C\dot q = (0.069282,\ 0.108253) \), which one can check by hand: \( C_{11} = h\dot q_2 = (-0.433013)(-0.8) = 0.346410 \), and so on. The gravity torque is the gradient of the potential \( \mathcal{U} = m_1 g\, y_{c_1} + m_2 g\, y_{c_2} \):

at \( q = (30^\circ, 60^\circ) \), where the second component vanishes because \( \cos 90^\circ = 0 \). Every one of these matches the script.

The skew-symmetry (passivity) property

One structural identity does more for control than any specific formula: with the Christoffel choice of \( C \), the matrix \( \dot M - 2C \) is skew-symmetric,

The reason is energy. The rate of change of kinetic energy is \( \tfrac{d}{dt}\big(\tfrac12\dot q\T M\dot q\big) = \dot q\T M\ddot q + \tfrac12\dot q\T\dot M\dot q \). Substituting the equations of motion for \( M\ddot q \) and requiring that the work done by the joint torques and gravity account exactly for the energy change (no fictitious power from the Coriolis terms) forces the quadratic form \( \dot q\T(\dot M - 2C)\dot q \) to vanish, which for the symmetric part means \( \dot M - 2C \) is skew. The benchmark verifies this over 2000 random \( (q, \dot q) \): the worst \( \lVert S + S\T \rVert \) with \( S = \dot M - 2C \) is \( 5.5\times10^{-10} \), and the worst value of the quadratic form \( \dot q\T S\dot q \) is \( 3.1\times10^{-9} \), both at the level of the finite-difference used to form \( \dot M \). An explicit instance at \( q = (30^\circ, 60^\circ) \) is \( S = \left[\begin{smallmatrix} 0 & 0.0866 \\ -0.0866 & 0 \end{smallmatrix}\right] \), visibly skew.

This matters because it turns Lyapunov arguments into one-liners. Any energy-like candidate \( V = \tfrac12\dot q\T M\dot q + (\text{potential}) \) has a time derivative in which the \( \dot q\T\dot M\dot q \) and \( \dot q\T C\dot q \) terms combine into \( \tfrac12\dot q\T(\dot M - 2C)\dot q = 0 \), leaving only the terms the controller directly shapes. The whole passivity-based control literature (Slotine and Li 1987 and after) rests on this cancellation.

As an independent check that the hand-derived \( M \), \( C \), and \( g \) are correct, the benchmark builds the same planar arm in MuJoCo and compares over 200 random states. The dense mass matrix from mj_fullM agrees with the analytic \( M \) to \( 6.8\times10^{-14} \); the bias force \( C\dot q + g \) from qfrc_bias agrees to \( 1.1\times10^{-14} \); the forward-dynamics acceleration under a random applied torque agrees to \( 2.2\times10^{-11} \); and the site position matches the closed-form forward kinematics to \( 10^{-15} \). Two independent implementations, a hand derivation and a general-purpose engine's articulated-body algorithm, agree to machine precision, which is the strongest correctness signal one gets.

Energy conservation and the cost of the integrator

With no applied torque and gravity as the only force, total mechanical energy is conserved, so simulating the free arm is a clean test of the integrator. The benchmark drops the arm from \( q = (60^\circ, -45^\circ) \) at rest and integrates for ten seconds. A fourth-order Runge–Kutta step at \( \Delta t = 10^{-3} \) conserves energy to a relative drift of \( 4.2\times10^{-9} \), and shrinking the step to \( 10^{-4} \) brings the drift to \( 7.5\times10^{-14} \), the fourth-order \( O(\Delta t^4) \) scaling made visible. Explicit Euler at the same \( 10^{-3} \) step drifts by \( 12\% \): first-order integration injects spurious energy and would send a long simulation unstable. MuJoCo's own RK4 integrator at its \( 5\times10^{-4} \) timestep drifts by \( 2.2\times10^{-8} \) over the same ten seconds while running at \( \sim 116{,}000 \) steps per second. The lesson generalizes: the integrator is part of the model, and a symplectic or high-order scheme is not a luxury when a policy is trained for millions of simulated steps.

Joint-space control I: PD plus gravity compensation

The simplest useful controller for setpoint regulation is proportional-derivative feedback with a gravity feedforward:

It drives the arm to a constant target \( q_d \) with provable global asymptotic stability, and the proof is the skew-symmetry payoff. Take the Lyapunov candidate

the kinetic energy plus a virtual spring pulling toward the target. It is positive definite and zero only at rest on target. Differentiating, \( \dot V = \dot q\T M\ddot q + \tfrac12\dot q\T\dot M\dot q - (q_d - q)\T K_p\dot q \). Substitute \( M\ddot q = \tau - C\dot q - g \) with the control law, use the skew-symmetry identity \( \dot q\T(\tfrac12\dot M - C)\dot q = 0 \) to cancel the Coriolis terms, and the gravity torque cancels against the compensation. What survives is

So \( V \) never increases, and it stops decreasing only when \( \dot q = 0 \); LaSalle's invariance principle then pins the only invariant set at \( q = q_d \). The benchmark integrates this regulator from \( q_0 = (-0.5, 1.0) \) to \( q_d = (0.7, -0.4) \) and confirms the Lyapunov function falls monotonically from \( V_0 = 170 \) to essentially zero, with the only positive increments (up to \( 1.5\times10^{-5} \)) being RK4 truncation artifacts rather than true increases; the final joint error is \( 5.5\times10^{-9} \). Crucially, PD plus gravity needs only the gravity model, not the full \( M \) and \( C \), which is why it is the workhorse for slow, accurate positioning.

Joint-space control II: computed torque

For tracking a fast trajectory, gravity compensation is not enough because the inertial and Coriolis forces are large. Computed-torque (inverse-dynamics) control cancels the entire nonlinear model and imposes linear error dynamics. Choose

Substituting into \( M\ddot q + C\dot q + g = \tau \) and cancelling \( C\dot q + g \), then left-multiplying by \( M^{-1} \) (which exists since \( M \succ 0 \)), the closed loop becomes, with error \( e = q_d - q \),

a decoupled bank of linear second-order systems. Choosing \( K_p = 100 I,\ K_d = 20 I \) sets a natural frequency of \( 10 \) rad/s with damping ratio \( \zeta = K_d/(2\sqrt{K_p}) = 1 \), critically damped, no overshoot. Tracking a quintic trajectory from \( (0,0) \) to \( (1.2, -0.8) \) over two seconds, the benchmark measures the RMS joint error at \( 7.6\times10^{-4} \) rad for computed torque against \( 2.2\times10^{-2} \) rad for PD-plus-gravity and \( 1.6\times10^{-1} \) rad for PD alone: cancelling the model buys nearly two orders of magnitude, and the peak torques are comparable (\( 23.2 \) versus \( 23.9 \) N·m), so the accuracy is essentially free once the model is known.

The catch is that computed torque is exactly as good as the model. Scaling the controller's link masses by an incorrect factor degrades tracking smoothly: at a \( 10\% \) mass error the RMS error rises from \( 7.6\times10^{-4} \) to \( 1.3\times10^{-2} \), at \( 30\% \) to \( 3.2\times10^{-2} \), and at \( 50\% \) to \( 4.5\times10^{-2} \). At a \( 30\% \) error, computed torque (\( 3.2\times10^{-2} \)) is no longer clearly better than PD-plus-gravity with the same wrong masses (\( 4.4\times10^{-2} \)): the value of model cancellation collapses as the model degrades, which is the practical argument for adaptive control, learned residual models, or simply feeding the controller an accurate identified model. Sample rate matters too: holding the computed torque constant over longer intervals raises the RMS error linearly, from \( 7.6\times10^{-4} \) at 1 kHz to \( 1.5\times10^{-2} \) at 50 Hz, which is why torque loops run at a kilohertz.

Operational-space control

Sometimes the task is naturally in the hand's coordinates, follow a line in space, push with a set force, not in joint angles. Khatib's 1987 operational-space formulation writes the dynamics directly in task space. Start from \( \ddot p = J\ddot q + \dot J\dot q \) (differentiate \( \dot p = J\dot q \)) and the joint dynamics \( \ddot q = M^{-1}(\tau - C\dot q - g) \). If the joint torque comes from a task wrench \( F \) as \( \tau = J\T F \) (the statics identity), substitute to get

Define the operational-space inertia matrix \( \Lambda(q) = \big(J M^{-1} J\T\big)^{-1} \), the apparent inertia the hand presents to a task force. Then choosing

makes the task-space error obey \( \ddot{\tilde e} + K_d\dot{\tilde e} + K_p\tilde e = 0 \) with \( \tilde e = p_d - p \), the same critically damped linear dynamics but now in the hand's frame. The benchmark tracks a straight-line hand trajectory with this law and achieves an RMS task error of \( 2.5\times10^{-4} \) m. The operational-space inertia is the crucial new object: at a well-conditioned posture its eigenvalues are moderate (\( 0.32 \) and \( 1.62 \) kg for the unit arm), but as the arm approaches a singularity \( \Lambda \) blows up along the lost direction, its largest eigenvalue climbing from \( 1.6 \) at \( q_2 = 1 \) rad to \( 6668 \) at \( q_2 = 0.01 \) rad. Physically, the hand becomes infinitely “heavy” to push along the direction the arm cannot move, which is the dynamics restating the kinematic singularity.

For a redundant arm the operational-space law leaves torque unused in the null space, which a secondary controller can claim without disturbing the task. The dynamically consistent form uses the inertia-weighted pseudo-inverse \( \bar J = M^{-1}J\T\Lambda \) and the projector \( N = I - J\T\bar J\T \), so the full torque is \( \tau = J\T F + N\tau_0 \). The benchmark demonstrates the kinematic version on the three-link arm, resolving a two-dimensional position task while a null-space term \( N(q_{\text{rest}} - q) \) pulls the posture toward a comfortable rest configuration. With no null-space gain the redundant joints drift wildly, ending \( 65 \) rad from rest with large task error from the accumulated numerical excursion; with gain \( 10 \) the posture error stays at \( 1.2 \) rad and the task RMS error drops to \( 0.16 \) m. The null-space term is doing real work: it regularizes the redundancy that the task alone leaves undetermined.

Trajectory generation

Controllers track a reference; trajectory generation produces one that respects velocity, acceleration, and jerk limits. The workhorse for a point-to-point move with rest at both ends is the quintic polynomial, which has six coefficients to satisfy six boundary conditions: position, velocity, and acceleration at start and end. With zero boundary velocity and acceleration,

Differentiating, the velocity profile \( 30\tau^2 - 60\tau^3 + 30\tau^4 \) peaks at the midpoint \( \tau = \tfrac12 \) with value \( \tfrac{15}{8} \), so the peak speed is \( v_{\max} = \tfrac{15}{8}\,(q_f - q_0)/T \). The acceleration \( 60\tau - 180\tau^2 + 120\tau^3 \) peaks at \( \tau = \tfrac12 \pm \tfrac{1}{2\sqrt3} \) with magnitude \( 10/\sqrt3 \), giving \( a_{\max} = \tfrac{10}{\sqrt3}\,(q_f - q_0)/T^2 \). For a \( 1.5 \)-rad move in \( 2 \) s the benchmark confirms \( v_{\max} = 1.40625 \) rad/s and \( a_{\max} = 2.165 \) rad/s\( ^2 \), matching the formulas exactly.

The quintic is smooth but conservative in time. A trapezoidal velocity profile, constant acceleration up to a cruise speed, constant cruise, constant deceleration, reaches the target faster for the same peak speed and acceleration. For a \( 1.5 \)-rad move with \( v_{\max} = 1 \) and \( a_{\max} = 2 \), the accelerate phase lasts \( t_a = v_{\max}/a_{\max} = 0.5 \) s, covering \( \tfrac12 a_{\max}t_a^2 = 0.25 \) rad; the cruise phase covers the remaining \( 1.0 \) rad in \( t_c = 1.0 \) s; total \( 2.0 \) s. If the move is too short to reach cruise speed the profile degenerates to a triangle: for a \( 0.3 \)-rad move, \( t_a = \sqrt{\text{disp}/a_{\max}} = 0.387 \) s with peak speed \( 0.775 < v_{\max} \), so it never cruises. The bang-bang (constant \( \pm a_{\max} \)) profile is time-optimal under a pure acceleration bound: the benchmark shows it completing the \( 1.5 \)-rad move in \( 1.73 \) s against \( 2.08 \) s for a quintic held to the same peak acceleration, a \( 1.20\times \) speedup that is the price of the quintic's smoothness.

Minimum-jerk trajectories minimize \( \int_0^T \dddot q^2\,dt \), the accepted model of smooth human reaching (Flash and Hogan 1985). The Euler–Lagrange condition for that functional is \( q^{(6)} = 0 \), a fifth-degree polynomial, and with zero boundary velocity and acceleration it is exactly the quintic above. The minimum jerk cost has the closed form \( \int_0^T \dddot q^2\,dt = 720\,(q_f - q_0)^2/T^5 \); for the \( 1.5 \)-rad, \( 2 \)-s move this is \( 50.625 \), and the benchmark's numerical triple-differentiation of the quintic gives \( 50.42 \), agreeing to discretization error. This identity, that minimum-jerk equals the quintic with rest boundaries, is why the quintic is the default: it is optimal for the most common smoothness objective. For paths through intermediate via points one solves a tridiagonal system for a natural cubic spline; the benchmark fits one through four knots and verifies \( C^2 \) continuity (matching velocity across the interior knot to \( 10^{-12} \)).

Implementation

The forward kinematics and Jacobian of the two-link arm, first the closed-form analytic versions in NumPy, then the same map differentiated automatically in JAX (forward mode) and PyTorch. The point of the pairing is that the analytic Jacobian and the autodiff Jacobian are the same matrix, computed two ways; the benchmark confirms they agree to machine precision.

import numpy as np
L1, L2 = 1.0, 1.0

def fk2(q):                       # q: (2,) joint angles -> (2,) hand position
    x = L1*np.cos(q[0]) + L2*np.cos(q[0] + q[1])
    y = L1*np.sin(q[0]) + L2*np.sin(q[0] + q[1])
    return np.array([x, y])

def jac2(q):                      # analytic 2x2 Jacobian dp/dq
    s1, s12 = np.sin(q[0]), np.sin(q[0] + q[1])
    c1, c12 = np.cos(q[0]), np.cos(q[0] + q[1])
    return np.array([[-L1*s1 - L2*s12, -L2*s12],
                     [ L1*c1 + L2*c12,  L2*c12]])

q = np.array([np.pi/6, np.pi/3])          # 30 deg, 60 deg
print(fk2(q))                             # [0.8660254 1.5]
print(np.linalg.det(jac2(q)))             # 0.8660254  == L1*L2*sin(q2)

import jax, jax.numpy as jnp
jax.config.update("jax_enable_x64", True)
L1, L2 = 1.0, 1.0

def fk2(q):                       # (2,) -> (2,)
    a = jnp.array([q[0], q[0] + q[1]])
    l = jnp.array([L1, L2])
    return jnp.array([jnp.sum(l*jnp.cos(a)), jnp.sum(l*jnp.sin(a))])

jac2 = jax.jacfwd(fk2)            # forward-mode autodiff gives the exact Jacobian
q = jnp.array([jnp.pi/6, jnp.pi/3])
print(fk2(q))                     # [0.8660254 1.5]
print(jnp.linalg.det(jac2(q)))    # 0.8660254, identical to the analytic form

import torch
torch.set_default_dtype(torch.float64)
L1, L2 = 1.0, 1.0

def fk2(q):                       # (2,) -> (2,)
    a = torch.stack([q[0], q[0] + q[1]])
    l = torch.tensor([L1, L2])
    return torch.stack([(l*torch.cos(a)).sum(), (l*torch.sin(a)).sum()])

q = torch.tensor([torch.pi/6, torch.pi/3], requires_grad=True)
J = torch.autograd.functional.jacobian(fk2, q)   # reverse-mode Jacobian
print(fk2(q).detach())            # tensor([0.8660, 1.5000])
print(torch.det(J))               # tensor(0.8660)

Numerical inverse kinematics: the closed-form two-solution solver and the damped-least-squares iteration, in NumPy. The DLS step is a single regularized linear solve, which is why it is both robust and cheap.

import numpy as np

def ik2_closed(p, L1=1.0, L2=1.0):
    x, y = p
    c2 = (x*x + y*y - L1*L1 - L2*L2) / (2*L1*L2)
    c2 = np.clip(c2, -1.0, 1.0)            # reject unreachable targets gracefully
    out = []
    for sign in (+1, -1):                  # elbow-down, elbow-up
        s2 = sign*np.sqrt(max(0.0, 1 - c2*c2))
        q2 = np.arctan2(s2, c2)
        q1 = np.arctan2(y, x) - np.arctan2(L2*s2, L1 + L2*c2)
        out.append(np.array([q1, q2]))
    return out

def ik_dls(target, q0, lam=0.05, tol=1e-6, maxit=2000):
    q = q0.copy()
    for k in range(maxit):
        e = target - fk2(q)                # residual in task space
        if np.linalg.norm(e) < tol:
            return q, k
        J = jac2(q)
        # damped least squares == Levenberg-Marquardt step, written in task space
        dq = J.T @ np.linalg.solve(J @ J.T + lam**2*np.eye(2), e)
        n = np.linalg.norm(dq)
        if n > 0.3:                         # step clamp for stability
            dq = dq*(0.3/n)
        q = q + dq
    return q, maxit

print([np.degrees(s) for s in ik2_closed([1.0, 1.0])])
# [array([ 0., 90.]), array([ 90., -90.])]
q_star, iters = ik_dls(np.array([1.2, 0.7]), np.array([0.1, 0.5]))
print(np.degrees(q_star), iters)           # converges to an elbow-up/down branch

The two-link dynamics and computed-torque control, in NumPy with the hand-derived \( M \), \( C \), \( g \) alongside a JAX version that builds the entire dynamics from automatic differentiation of the Lagrangian: the mass matrix as the Hessian of kinetic energy in \( \dot q \), gravity as the gradient of the potential, and the Coriolis vector from the Christoffel symbols computed by differentiating \( M \). Both produce the benchmark's numbers.

import numpy as np
m1 = m2 = 1.0; l1 = l2 = 1.0; lc1 = lc2 = 0.5; I1 = I2 = 1/12; g = 9.81

def M(q):
    c2 = np.cos(q[1])
    a = m1*lc1**2 + I1 + m2*(l1**2 + lc2**2 + 2*l1*lc2*c2) + I2
    b = m2*(lc2**2 + l1*lc2*c2) + I2
    c = m2*lc2**2 + I2
    return np.array([[a, b], [b, c]])

def C(q, dq):
    h = -m2*l1*lc2*np.sin(q[1])
    return np.array([[h*dq[1], h*(dq[0] + dq[1])],
                     [-h*dq[0], 0.0]])

def grav(q):
    g1 = (m1*lc1 + m2*l1)*g*np.cos(q[0]) + m2*lc2*g*np.cos(q[0] + q[1])
    g2 = m2*lc2*g*np.cos(q[0] + q[1])
    return np.array([g1, g2])

def computed_torque(q, dq, qd, dqd, ddqd, Kp, Kd):
    v = ddqd + Kd @ (dqd - dq) + Kp @ (qd - q)   # linearizing acceleration
    return M(q) @ v + C(q, dq) @ dq + grav(q)    # cancel the model exactly

q = np.array([np.pi/6, np.pi/3]); dq = np.array([0.5, -0.8])
print(M(q))                          # [[2.16667 0.58333] [0.58333 0.33333]]
print(C(q, dq) @ dq)                 # [0.06928 0.10825]
print(grav(q))                       # [12.7436 0.]

import jax, jax.numpy as jnp
jax.config.update("jax_enable_x64", True)
m1 = m2 = 1.0; l1 = l2 = 1.0; lc1 = lc2 = 0.5; I1 = I2 = 1/12; g = 9.81

def kinetic(q, dq):                  # (1/2) dq^T M(q) dq, built from COM velocities
    q1, q2 = q; dq1, dq2 = dq
    vx1, vy1 = -lc1*jnp.sin(q1)*dq1, lc1*jnp.cos(q1)*dq1
    vx2 = -l1*jnp.sin(q1)*dq1 - lc2*jnp.sin(q1 + q2)*(dq1 + dq2)
    vy2 =  l1*jnp.cos(q1)*dq1 + lc2*jnp.cos(q1 + q2)*(dq1 + dq2)
    T1 = 0.5*m1*(vx1**2 + vy1**2) + 0.5*I1*dq1**2
    T2 = 0.5*m2*(vx2**2 + vy2**2) + 0.5*I2*(dq1 + dq2)**2
    return T1 + T2

def potential(q):
    q1, q2 = q
    return m1*g*(lc1*jnp.sin(q1)) + m2*g*(l1*jnp.sin(q1) + lc2*jnp.sin(q1 + q2))

M    = jax.hessian(kinetic, argnums=1)   # d^2 T / d(dq)^2 == M(q)
grav = jax.grad(potential)               # dU/dq == g(q)

def coriolis(q, dq):                     # Christoffel symbols from dM/dq
    dMdq = jax.jacfwd(M, argnums=0)(q, dq)          # (2,2,2): dM_ij/dq_k
    Gamma = 0.5*(dMdq + dMdq.transpose(0, 2, 1) - dMdq.transpose(2, 1, 0))
    return jnp.einsum("ijk,j,k->i", Gamma, dq, dq)

q = jnp.array([jnp.pi/6, jnp.pi/3]); dq = jnp.array([0.5, -0.8])
print(M(q, dq))                          # [[2.16667 0.58333] [0.58333 0.33333]]
print(coriolis(q, dq))                   # [0.06928 0.10825]  == C(q,dq) @ dq
print(grav(q))                           # [12.7436 0.]

Operational-space control, computing the operational-space inertia \( \Lambda \) and mapping a task-space acceleration command to joint torque through \( J\T \). This is the inner loop that produced the \( 2.5\times10^{-4} \)-m tracking error above.

import numpy as np

def osc_torque(q, dq, pd, dpd, ddpd, Kp, Kd):
    J    = jac2(q)
    Minv = np.linalg.inv(M(q))
    Lam  = np.linalg.inv(J @ Minv @ J.T)      # operational-space inertia (JAcobian, M)
    dJ   = (jac2(q + 1e-6*dq) - jac2(q - 1e-6*dq)) / 2e-6   # dJ/dt along dq
    p, dp = fk2(q), J @ dq
    a = ddpd + Kd @ (dpd - dp) + Kp @ (pd - p) - dJ @ dq    # task accel command
    F = Lam @ a                               # task force = apparent inertia * accel
    return J.T @ F + C(q, dq) @ dq + grav(q)  # map to joint torque, add compensation

q = np.array([0.5, 1.0]); dq = np.zeros(2)
J = jac2(q); Lam = np.linalg.inv(J @ np.linalg.inv(M(q)) @ J.T)
print(np.linalg.eigvalsh(Lam))                # [0.3244 1.6171] kg apparent inertia

How it is done in practice

The equations above are what a real system runs, but the engineering gap between the derivation and a deployed arm is mostly about scale, timing, and model fidelity. Production dynamics are not built matrix-by-matrix as in the two-link example; they use recursive algorithms. Featherstone's recursive Newton–Euler computes inverse dynamics (given \( q, \dot q, \ddot q \), find \( \tau \)) in \( O(n) \), and the articulated-body algorithm computes forward dynamics (given \( \tau \), find \( \ddot q \)) in \( O(n) \) as well, versus the \( O(n^3) \) of forming and inverting \( M \) explicitly. For a \( 7 \)-degree-of-freedom arm the constant factors matter less than the fact that these are the algorithms inside Pinocchio, MuJoCo, RBDL, and Drake, and that Pinocchio additionally computes the analytical derivatives of these quantities, which is what makes gradient-based trajectory optimization and differentiable simulation tractable.

Timing is the other reality. A torque control loop runs at \( 1 \) kHz on a real-time thread, which the sample-rate study above shows is not optional: at \( 50 \) Hz the same computed-torque law has twenty times the tracking error. Industrial controllers (a Franka Emika Panda through libfranka, a Universal Robots arm through its RTDE interface) expose exactly this: a \( 1 \)-kHz torque or joint-position interface, with the inverse-dynamics compensation running underneath. The measured MuJoCo throughput here, \( \sim 116{,}000 \) steps per second for a two-link arm at \( 2 \)-kHz physics on a CPU, scales to the thousands-of-environments-in-parallel regime on a GPU that reinforcement learning needs, which is the entire reason GPU physics engines exist.

Model fidelity is where the derivation and reality diverge most. The two-link arm has exact rigid-body dynamics; a real arm has joint friction, gear elasticity, cable dynamics, unmodeled payload, and sensor noise. The \( 30\%\)-mass-error study is a stand-in for this: computed torque with a \( 30\% \) model error is no better than PD-plus-gravity, so real controllers either identify an accurate model (least-squares system identification on the linear-in-parameters dynamics), adapt the model online (Slotine–Li adaptive control), or learn a residual with a neural network on top of the rigid-body prior. The rigid-body model is still worth having as the prior; the debate is only over how much to trust it.

The current research frontier

Three threads dominate the last few years. The first is differentiable and GPU-parallel simulation. MuJoCo, acquired and open-sourced by Google DeepMind, now ships MJX, a re-implementation on JAX and XLA that runs thousands of copies on a GPU and is differentiable end to end; Google's Brax and NVIDIA's Isaac Gym and Warp occupy the same space. The research question is how to differentiate through contact, which is nonsmooth, without the gradients becoming useless; approaches range from smoothed contact models to randomized-smoothing gradient estimators, with groups at MIT, Stanford, and NVIDIA all contributing.

The second is whole-body model-predictive and learned control on legged and mobile manipulators. The ANYmal and quadruped work out of ETH Zurich's Robotic Systems Lab, the humanoid control efforts at Berkeley, CMU, and IIT, and NVIDIA's cuRobo, which does GPU-parallel collision-free minimum-jerk motion generation for arms, all share a structure: a classical model (the same \( M, C, g \) derived here, extended to floating-base and contact) inside a fast optimizer, with a learned policy either replacing the optimizer or warm-starting it. The operational-space and null-space machinery of this page is the backbone of the whole-body-control formulations these systems use to juggle balance, contact forces, and a manipulation task simultaneously. NVIDIA's HOVER (2024) pushes the learned end of this thread by distilling several humanoid whole-body control modes, tracking kinematic targets from teleoperation, joystick commands, and task-space goals, into a single policy, so one network serves as the low-level controller that higher-level planners and learned policies command.

The third is learned manipulation policies and foundation models, which is where the field's attention has moved most sharply. Diffusion policies (Columbia, MIT, Toyota Research), and vision-language-action models, RT-2 from Google DeepMind, OpenVLA from a Stanford-led collaboration, Octo from Berkeley, and the pi-zero model from Physical Intelligence, learn to map pixels and language directly to actions, sidestepping explicit kinematics and dynamics. The Tsinghua RDT line marks how fast this thread moves, RDT-1B (2024) pretrained a 1.2B-parameter diffusion policy for bimanual arms, and RDT-2 (2025) adapts a Qwen2.5-VL backbone to emit 24-step relative action chunks through a residual vector-quantized tokenizer, with zero-shot deployment on unseen embodiments as the stated target. Relative end-effector chunks are only executable through the differential kinematics and inverse-dynamics layers derived here, one more sign that the classical maps do not disappear under the learned stack. The open question is exactly how much of the model these policies should be handed versus forced to learn: a policy that outputs joint targets still relies on an inverse-dynamics controller underneath, and the evidence so far is that giving the learner the kinematic and dynamic priors, rather than making it rediscover them from data, is the more sample-efficient path. That debate, model-based structure versus end-to-end learning, is taken up in depth on the embodied foundation models page; the contact and grasping side is on the advanced manipulation page.

Open source to read

• google-deepmind/mujoco — the physics engine used for the cross-validation above. Read engine_core_smooth.c for the mass-matrix and forward-kinematics routines, and the mjx/ tree for the JAX re-implementation. The MJCF format is the fastest way to specify an arm.
• stack-of-tasks/pinocchio — the reference C++ rigid-body library, with the recursive algorithms and their analytical derivatives. Start with src/algorithm/rnea.hpp (recursive Newton–Euler) and aba.hpp (articulated-body). The Python bindings make it a drop-in for the NumPy code here at production speed.
• petercorke/robotics-toolbox-python — the most readable teaching implementation. robot/ETS.py and the DH models make the forward-kinematics and Jacobian derivations of this page concrete and plottable.
• bulletphysics/bullet3 — Bullet and PyBullet, the widely used contact simulator. Read examples/pybullet/ for inverse-kinematics and inverse-dynamics examples that mirror the numerical IK here.
• NVlabs/curobo — NVIDIA's GPU-parallel motion generation. The kinematics and collision modules show how the batched forward kinematics and Jacobian of this page look when vectorized across thousands of configurations for real-time planning.
• frankaemika/libfranka — the control interface for a real \( 7 \)-DoF arm. The torque-control and Cartesian-impedance examples are the operational-space control of this page running on hardware at \( 1 \) kHz.
• RobotLocomotion/drake — the Toyota Research and MIT toolbox for model-based design, with symbolic and autodiff-backed dynamics and a mature trajectory-optimization stack.

Common misconceptions

“Euler angles are a fine way to store an orientation.” Any three-parameter representation of \( SO(3) \) has a singularity (gimbal lock) where two axes align and a degree of freedom is lost, and near it the angular rates diverge. Quaternions or rotation matrices avoid this because they over-parameterize. Euler angles are fine for a human-readable readout, not for state.

“The Jacobian transpose and the pseudo-inverse are basically the same for IK.” They solve different problems. The transpose is gradient descent, first-order and slow; the pseudo-inverse is a Newton step, second-order and fast but ill-behaved near singularities. The benchmark shows the transpose needing \( 627 \) iterations where the pseudo-inverse needs \( 13 \) at the same near-boundary target. Damped least squares is the pseudo-inverse made safe.

“A singularity is where the arm cannot reach a point.” A kinematic singularity is about velocity, not reachability: it is a configuration where the Jacobian drops rank so the hand cannot instantaneously move in some direction, even though it may sit at a perfectly reachable point. The elbow being straight is singular yet reaches every point on the workspace boundary.

“More joints just makes the arm more capable.” Redundancy adds a null space, which must be resolved: without a secondary objective the extra joints are undetermined and can drift, as the benchmark's null-space demo shows the posture wandering \( 65 \) rad from rest when the null-space gain is zero. Redundancy is a resource, but it is one the controller has to manage.

“Computed torque is strictly better than PD with gravity compensation.” Only when the model is accurate. At a \( 30\% \) mass error computed torque (\( 3.2\times10^{-2} \) rad RMS) is no better than PD-plus-gravity with the same error (\( 4.4\times10^{-2} \)). Model cancellation is worth exactly as much as the model.

“The Coriolis matrix \( C \) is uniquely defined.” Only the product \( C\dot q \) is unique; many matrices \( C \) give the same \( C\dot q \). The Christoffel-symbol choice is the one that makes \( \dot M - 2C \) skew-symmetric, and only that choice, so the passivity property is a property of the factorization, not of the dynamics alone.

“Autodiff through forward kinematics gives something other than the geometric Jacobian.” It gives exactly the geometric Jacobian (or the analytic one, depending on the task map), to machine precision. The benchmark's analytic-versus-autodiff difference is \( 0.0 \) for the two-link, three-link, and six-link arms. Autodiff is a computation of the same object, not an approximation of a different one.

Self-check

References

1. Lynch, K. M., and Park, F. C. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017. Free at modernrobotics.org. The product-of-exponentials reference.
2. Murray, R. M., Li, Z., and Sastry, S. S. A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994. Free at cds.caltech.edu/~murray/mlswiki. The Lie-group treatment.
3. Craig, J. J. Introduction to Robotics: Mechanics and Control, 4th ed. Pearson, 2017.
4. Siciliano, B., Sciavicco, L., Villani, L., and Oriolo, G. Robotics: Modelling, Planning and Control. Springer, 2009. doi:10.1007/978-1-84628-642-1.
5. Spong, M. W., Hutchinson, S., and Vidyasagar, M. Robot Modeling and Control. Wiley, 2005.
6. Featherstone, R. Rigid Body Dynamics Algorithms. Springer, 2008. doi:10.1007/978-1-4899-7560-7.
7. Denavit, J., and Hartenberg, R. S. “A kinematic notation for lower-pair mechanisms based on matrices.” ASME J. Applied Mechanics 22, 1955, 215–221.
8. Whitney, D. E. “Resolved motion rate control of manipulators and human prostheses.” IEEE Trans. Man-Machine Systems 10(2), 1969. doi:10.1109/TMMS.1969.299896.
9. Khatib, O. “A unified approach for motion and force control of robot manipulators: The operational space formulation.” IEEE J. Robotics and Automation 3(1), 1987. doi:10.1109/JRA.1987.1087068.
10. Nakamura, Y., and Hanafusa, H. “Inverse kinematic solutions with singularity robustness for robot manipulator control.” ASME J. Dynamic Systems, Measurement, and Control 108(3), 1986. doi:10.1115/1.3143764.
11. Wampler, C. W. “Manipulator inverse kinematic solutions based on vector formulations and damped least-squares methods.” IEEE Trans. Systems, Man, and Cybernetics 16(1), 1986. doi:10.1109/TSMC.1986.289285.
12. Chiaverini, S., Siciliano, B., and Egeland, O. “Review of the damped least-squares inverse kinematics with experiments on an industrial robot manipulator.” IEEE Trans. Control Systems Technology 2(2), 1994. doi:10.1109/87.309373.
13. Yoshikawa, T. “Manipulability of robotic mechanisms.” Int. J. Robotics Research 4(2), 1985. doi:10.1177/027836498500400201.
14. Slotine, J.-J. E., and Li, W. “On the adaptive control of robot manipulators.” Int. J. Robotics Research 6(3), 1987. doi:10.1177/027836498700600303.
15. Park, F. C., Bobrow, J. E., and Ploen, S. R. “A Lie group formulation of robot dynamics.” Int. J. Robotics Research 14(6), 1995. doi:10.1177/027836499501400606.
16. Flash, T., and Hogan, N. “The coordination of arm movements: An experimentally confirmed mathematical model.” J. Neuroscience 5(7), 1985. doi:10.1523/JNEUROSCI.05-07-01688.1985.
17. Nakanishi, J., Cory, R., Mistry, M., Peters, J., and Schaal, S. “Operational space control: A theoretical and empirical comparison.” Int. J. Robotics Research 27(6), 2008. doi:10.1177/0278364908091463.
18. Todorov, E., Erez, T., and Tassa, Y. “MuJoCo: A physics engine for model-based control.” IROS, 2012. doi:10.1109/IROS.2012.6386109.
19. Carpentier, J., et al. “The Pinocchio C++ library: A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives.” IEEE/SICE SII, 2019. doi:10.1109/SII.2019.8700380.
20. Sundaralingam, B., et al. “cuRobo: Parallelized collision-free robot motion generation.” ICRA, 2023. arXiv:2310.17274.
21. Makoviychuk, V., et al. “Isaac Gym: High performance GPU-based physics simulation for robot learning.” NeurIPS Datasets and Benchmarks, 2021. arXiv:2108.10470.
22. Chi, C., et al. “Diffusion Policy: Visuomotor policy learning via action diffusion.” RSS, 2023. arXiv:2303.04137.
23. Black, K., Brown, N., Driess, D., et al. “Pi-0: A vision-language-action flow model for general robot control.” Physical Intelligence, 2024. arXiv:2410.24164.
24. Liu, S., Wu, L., Li, B., et al. “RDT-1B: A diffusion foundation model for bimanual manipulation.” Tsinghua University, 2024. arXiv:2410.07864.
25. RDT Team, Tsinghua University. RDT-2, an autoregressive vision-language-action model with a residual vector-quantized action tokenizer, 2025. github.com/thu-ml/RDT2.
26. He, T., et al. “HOVER: Versatile neural whole-body controller for humanoid robots.” NVIDIA, 2024. github.com/NVlabs/HOVER.