Recursive Algorithms
Recursive Algorithms
All spatial 6-vectors use (angular ; linear) ordering: \(\twist = [\bm{\omega};\, \bm{v}]\), \(\wrench = [\bm{m};\, \bm{f}]\), \(\screw = [\bm{\omega};\, \bm{v}]\). Matches Lynch & Park (Lynch and Park 2017) §8. The Adjoint operator \(\operatorname{Ad}_T\) and the Lie-bracket matrix form \(\operatorname{ad}_\twist\) are defined in Chapter 5 and used here without redefinition.
The Curse of Dimensionality
As systems scale in complexity—from a simple pendulum (1 DOF) to a fully modeled anatomical golf swing (15+ DOF) or a humanoid Atlas robot (30+ DOF)—the monolithic equations of physics become mathematically intractable.
If you attempt to write the analytical Coriolis, Centrifugal, and Inertial force matrices (like the classic \(M(\bm{q})\ddot{\bm{q}} + C(\bm{q}, \dot{\bm{q}})\dot{\bm{q}} + G(\bm{q}) = \bm{\tau}\)) explicitly for a 15-DOF human model on a piece of paper, the resulting trigonometric expansion will fill thousands of pages.
To control complex mechanisms in the real world at thousands of Hertz, modern engines (such as MuJoCo, Pinocchio, and related physics simulation frameworks) rely entirely on Recursive Algorithms. The foundational algorithms for recursive kinematics and dynamics are detailed in Featherstone (2008, Rigid Body Dynamics Algorithms) and are the mathematical backbone of all modern robotics engines.
Recursive Forward Kinematics
Suppose we want to find the position and velocity of the golf clubhead at the very end of a 15-link robotic golfer.
Instead of deriving a massive equation relating the base of the feet directly to the clubhead, we use the Homogeneous Transforms (\(T \in \SE\)) and Twists (\(\twist \in \se\)) from our previous chapters, and we propagate them sequentially outwards.
To find the speed of the clubhead relative to the ground, we don’t jump straight from the ground to the club.
We ask: How fast is the pelvis moving relative to the feet? Then, we add the speed of the torso relative to the pelvis. Then we add the speed of the shoulder relative to the torso… chain by chain, link by link.
Mathematically, if \(i\) is the current link, and \(i-1\) is its parent: \[\begin{equation} \twist_i = \text{Ad}_{T_{i, i-1}}(\twist_{i-1}) + \screw_i \dot{q}_i \end{equation}\]
This equation takes the velocity of the parent link (\(\twist_{i-1}\)), transforms it into the frame of the current link using the Adjoint Matrix (\(\text{Ad}_T\)), and then adds the new velocity generated by the current motor spinning (\(\screw_i \dot{q}_i\)).
By running a simple loop for i=1 to 15, a computer solves the exact physics of the entire kinematic chain blazing fast, with no trigonometric explosions.
Here is pseudocode for the recursive forward kinematics algorithm:
Near-Singular Configurations: When a screw axis approaches alignment with the global frame normal (e.g., when \(\|\omega\| \to 0\) for a prismatic joint), the matrix exponential \(e^{[\screw]\theta}\) can exhibit numerical instability. Always check the rank of the screw axis before computing; use Moore-Penrose pseudoinverse if singularity is detected.
Quaternion Representation: While the pseudocode uses rotation matrices, consider storing rotations as unit quaternions internally for cumulative products. Matrices require periodic re-orthogonalization after many multiplications; quaternions maintain unit norm automatically and are more numerically stable for long kinematic chains.
Numerical Precision: The matrix exponential is typically computed via eigendecomposition (expensive) or series expansion (fast but approximate). For production systems, use optimized libraries (e.g., Eigen, MATLAB’s expm). Always verify orthonormality constraints: \(R^T R = I\) should hold to machine precision after each link update.
def recursive_forward_kinematics(n_links, joint_angles, screw_axes, T_base):
"""
Compute the forward kinematics of a kinematic chain using recursive propagation.
Args:
n_links: Number of joints in the chain
joint_angles: Array of n_links joint angles theta_i
screw_axes: Array of n_links screw axes S_i (expressed in Space Frame)
T_base: Base (home) configuration as 4x4 homogeneous transform
Returns:
T_end: 4x4 transform of end-effector in Space Frame
T_list: List of transforms at each link
"""
# Initialize transforms
T_current = np.eye(4) # Start at identity (base of kinematic chain)
T_list = [T_current.copy()]
# Forward pass: propagate from base (link 0) to end-effector (link n)
for i in range(n_links):
# Get the twist (screw axis) for joint i
S_i = screw_axes[i]
theta_i = joint_angles[i]
# Compute the 4x4 matrix exponential e^{[S_i] * theta_i}
# [S_i] is the skew-symmetric spatial matrix form of screw S_i
S_matrix_i = screw_to_matrix(S_i) # Convert to 4x4 skew form
exp_S_i = scipy.linalg.expm(S_matrix_i * theta_i)
# Update transform: T_current accumulates the effect of this joint
T_current = T_current @ exp_S_i
T_list.append(T_current.copy())
# Apply the base (home) configuration at the end
T_end = T_current @ T_base
return T_end, T_list
def screw_to_matrix(screw):
"""
Convert a 6D screw axis (w, v) to its 4x4 matrix form [S].
Screw format: screw = [w_x, w_y, w_z, v_x, v_y, v_z]^T
"""
w = screw[0:3] # Angular velocity (rotation axis)
v = screw[3:6] # Linear velocity
S_matrix = np.zeros((4, 4))
S_matrix[0:3, 0:3] = skew(w) # Skew-symmetric form of angular part
S_matrix[0:3, 3] = v # Linear part in translation column
# Bottom row remains [0, 0, 0, 0]
return S_matrix
def skew(w):
"""Convert 3D vector w to its 3x3 skew-symmetric matrix."""
return np.array([[0, -w[2], w[1]],
[w[2], 0, -w[0]],
[-w[1], w[0], 0]])The Recursive Newton-Euler Algorithm (Dynamics)
While Forward Kinematics gives us position and velocity, controlling motion requires solving for forces and torques. If we know the trajectory the robot is currently taking (\(\bm{q}, \dot{\bm{q}}, \ddot{\bm{q}}\)), how much torque \(\bm{\tau}\) must the motors output at this exact millisecond to execute it?
This is solved by the crowning jewel of computational mechanics: the Recursive Newton-Euler Formulation. It executes in two sweeps.
The Outward Pass: Velocity and Acceleration
Just like Forward Kinematics, we cast a sweeping algorithm from the heavy base (the ground) outwards to the lightest tip (the clubhead/end-effector). For every link \(i\), we compute its absolute velocity \(\twist_i\) and its absolute acceleration \(\dot{\twist}_i\).
This outward pass perfectly calculates the exact inertial, Coriolis, and centrifugal accelerations dragging on every single link of the mechanism.
The Inward Pass: Forces and Torques
Once the outward pass finishes at the tip, we reverse direction. We perform a second loop starting from the tip of the robot back down to the ground.
At the tip, we compute the inertial force \(F = ma\) created by the acceleration we just calculated. \[\begin{equation} \wrench_i = I_i \dot{\twist}_i - \text{ad}_{\twist_i}^T (I_i \twist_i) \end{equation}\]
Where \(\wrench_i\) is the Spatial Wrench (Force/Torque vector), and \(I_i\) is the Spatial Inertia matrix.
Then, we recursively drag that force down the arm. The shoulder joint has to support the force of the arm, plus the new forces generated by the shoulder itself. The torso has to support the forces from the shoulder. The torque required at any specific motor \(i\) is extracted by projecting the total Wrench back onto the single scalar axis of its mechanical joint.
Here is pseudocode for the complete Recursive Newton-Euler Algorithm:
Near-Singular Configurations: During the inward pass, near-singular wrist configurations (e.g., when the last three axes become coplanar) can amplify numerical errors in wrench transformations. Monitor the condition number of the Adjoint matrix \(\text{Ad}_T\); if it exceeds a threshold, flag the configuration as unstable and apply numerical regularization.
Numerical Precision in Adjoint Maps: The Adjoint matrix transformation of wrenches (line: Ad_T.T @ wrenches) is where most numerical drift accumulates in long chains. Use double-precision arithmetic and periodically verify that the wrench values remain physically plausible (e.g., magnitudes should not grow exponentially).
Quaternion Integration: If rotations are stored as quaternions (recommended), construct the Adjoint matrix from the quaternion-derived rotation \(R\) rather than extracting it from a drift-corrupted rotation matrix. This maintains numerical stability throughout both the outward and inward passes.
def recursive_newton_euler(n_links, joint_angles, joint_velocities, joint_accelerations,
spatial_inertias, link_masses, screw_axes,
gravity_vector=[0, 0, -9.81]):
"""
Compute joint torques using Recursive Newton-Euler Inverse Dynamics.
Args:
n_links: Number of joints
joint_angles, joint_velocities, joint_accelerations: Arrays of size n_links
spatial_inertias: List of 6x6 spatial inertia matrices for each link
link_masses: Array of link masses
screw_axes: Array of 6D screw axes in Space Frame
gravity_vector: 3D gravity acceleration
Returns:
joint_torques: Scalar torques required at each joint
wrenches: Spatial wrenches at each link
"""
# Precompute all transforms using forward kinematics
T_list, _ = recursive_forward_kinematics(n_links, joint_angles, screw_axes, np.eye(4))
# OUTWARD PASS: Compute velocities and accelerations (base to tip)
twists = [np.zeros(6)] # Velocity of base link (ground) is zero
# Gravity as a fictitious base acceleration (Featherstone 2008, Sec. 6.2):
# seed the base spatial acceleration with [-g] in the linear block so that
# the outward recursion propagates gravitational loading into every link
# without an explicit gravity term per link.
gravity = np.asarray(gravity_vector) # e.g. [0, 0, -9.81]
twist_dot_base = np.concatenate([np.zeros(3), -gravity]) # [omega_dot=0 ; v_dot=-g]
twist_derivatives = [twist_dot_base]
coriolis_bias_forces = []
for i in range(n_links):
S_i = screw_axes[i]
# Transform parent twist into current link frame
Ad_T = adjoint_matrix(T_list[i])
twist_parent_in_i = Ad_T @ twists[i]
# Current link velocity = parent velocity + joint contribution
twist_i = twist_parent_in_i + S_i * joint_velocities[i]
twists.append(twist_i)
# Acceleration: time derivative of velocity. Coriolis/centrifugal
# contribution uses the Lie-bracket matrix ad_V (Chapter 5).
twist_derivative_parent = Ad_T @ twist_derivatives[i]
joint_accel_contrib = S_i * joint_accelerations[i]
coriolis_term = ad_matrix(S_i * joint_velocities[i]) @ twist_i
twist_derivative_i = twist_derivative_parent + joint_accel_contrib + coriolis_term
twist_derivatives.append(twist_derivative_i)
# Velocity-product (bias) wrench: ad_V^T (I V).
I_i = spatial_inertias[i]
bias_wrench = ad_matrix(twist_i).T @ (I_i @ twist_i)
coriolis_bias_forces.append(bias_wrench)
# INWARD PASS: Compute wrenches and joint torques (tip to base)
wrenches = [np.zeros(6)] # Wrench at tip (no load beyond the arm)
joint_torques = np.zeros(n_links)
for i in range(n_links - 1, -1, -1):
I_i = spatial_inertias[i]
S_i = screw_axes[i]
# Spatial wrench on link i from its own dynamics
wrench_inertial = I_i @ twist_derivatives[i + 1]
# Transform child wrench back to parent frame
Ad_T_child = adjoint_matrix(T_list[i + 1])
wrench_from_child = Ad_T_child.T @ wrenches[-1] if i < n_links - 1 else np.zeros(6)
# Total wrench on link i
wrench_i = wrench_inertial - coriolis_bias_forces[i] + wrench_from_child
wrenches.append(wrench_i)
# Joint torque: project wrench onto screw axis
# (Screw.T @ I_A @ Screw gives effective 1D inertia)
joint_torques[i] = S_i.T @ wrench_i
return joint_torques[::-1], wrenches[::-1]
def adjoint_matrix(T):
"""
Compute the 6x6 Adjoint matrix Ad_T for a 4x4 homogeneous transform T.
Transports TWISTS between frames: V_A = Ad_{T_AB} V_B.
Wrenches transform by the inverse-transpose: F_A = Ad_{T_AB}^{-T} F_B.
"""
R = T[0:3, 0:3]
p = T[0:3, 3]
p_skew = skew(p)
Ad = np.zeros((6, 6))
Ad[0:3, 0:3] = R
Ad[3:6, 3:6] = R
Ad[3:6, 0:3] = p_skew @ R
return Ad
def ad_matrix(V):
"""
Lie-bracket matrix form ad_V for a spatial 6-vector V = [omega ; v].
Distinct from Ad_T: ad_V appears in Coriolis/centrifugal terms and in
the velocity-product wrench p = ad_V^T (I V) used in RNEA and ABA.
"""
omega = V[0:3]
v = V[3:6]
A = np.zeros((6, 6))
A[0:3, 0:3] = skew(omega)
A[3:6, 0:3] = skew(v)
A[3:6, 3:6] = skew(omega)
return AThe Recursive Newton-Euler algorithm allows the inverse dynamics for a system with \(N\) joints to be solved in \(O(N)\) operations. Time to compute scales linearly, regardless of complexity. This is why robotics works in the real world.
For a chain with \(N\) joints, each recursive pass visits every joint exactly once and does a fixed number of \(6\times 6\) matrix operations per visit:
- RNEA (inverse dynamics): one outward + one inward pass of \(N\) iterations \(\times\) \(O(1)\) \(6\times 6\) ops \(= O(N)\).
- ABA (forward dynamics, Chapter 10): three passes of the same shape \(= O(N)\).
- CRBA (explicit mass matrix): evaluates RNEA once per generalized coordinate to build each column of \(M(\bm{q})\) \(= O(N^2)\).
The \(6\times 6\) block size is fixed by spatial-algebra dimensionality, so the asymptotic rates above are tight; the hidden constants are small enough that 30-DOF humanoids run comfortably at kHz rates.
Summary Structure
Throughout the subsequent volumes of Tangent-Space Methods, physical phenomena like Contraction Metrics, Sums-of-Squares Programming, and LQR-Trees are discussed dynamically. Behind the curtain of those advanced trajectory optimizations, the physics governing the simulated bodies are invariably relying on Quaternions to map their angles and Recursive Algorithms to execute their algebraic realities in \(O(N)\) time.
The mathematical fundamentals are established. Let us begin Volume I.