Featherstone Algorithm (Articulated Body Algorithm)

The algorithm's most significant advantage is its linear time complexity relative to the number of bodies, n, in the articulated chain. It achieves this by exploiting the chain's tree-like structure, recursively propagating inertia and forces from the leaves to the root and back. This is a dramatic improvement over the naive O(n³) approach of formulating and inverting the full system mass matrix, making real-time simulation of complex robots with dozens of joints feasible.

The algorithm is built on a two-pass recursion over the kinematic tree:

• Outward Pass: Computes the velocity and acceleration of each body, starting from the base and moving outward to the end-effectors.
• Inward Pass: Calculates the articulated body inertia and bias forces, then solves for joint accelerations, propagating from the end-effectors back to the base. This recursive structure mirrors the physical propagation of forces and motion, resulting in an intuitive and numerically stable implementation.

Featherstone's algorithm is formulated using 6D spatial vectors, which combine linear and angular components into a single entity. This compact representation:

• Simplifies equations by treating forces, velocities, accelerations, and inertias in a unified 6D space.
• Reduces computational overhead compared to separate 3D linear and angular calculations.
• Provides an elegant mathematical framework that is central to modern rigid body dynamics libraries like Drake and MuJoCo.

Unlike simpler algorithms designed for single chains, the Featherstone algorithm naturally accommodates branching kinematic structures (e.g., a humanoid robot with two arms and two legs). It treats the system as a connectivity tree, where each body has one parent but can have multiple children. The recursive passes efficiently handle these branches, making it the de facto standard for simulating legged robots and complex robotic manipulators with multiple end-effectors.

A key innovation is bypassing the explicit construction and inversion of the large, dense system mass matrix M(q). Instead, the algorithm works with articulated body inertias—the effective inertia of a subtree as felt at a joint. This avoids the numerical instability and high computational cost (O(n³)) associated with inverting M(q) for systems with many degrees of freedom, which is critical for control applications requiring frequent dynamics calculations.

The algorithm's efficiency makes it indispensable for real-time robotic control paradigms:

• Operational Space Control: Enables fast computation of the dynamically consistent generalized inverse of the Jacobian.
• Model Predictive Control (MPC): Allows rapid forward dynamics predictions over a receding horizon.
• Inverse Dynamics Control: Provides the joint torques needed to track a desired acceleration, essential for high-performance motion. It is the computational engine behind many simulation-for-training (Sim2Real) pipelines in reinforcement learning for robotics.

Forward dynamics is the core computation performed by the Featherstone algorithm. Given the applied forces and torques acting on an articulated rigid body system (like a robot arm), it calculates the resulting accelerations—both linear and angular—of all its links. This solves the equations of motion to predict the system's future state.

• Contrast with Inverse Dynamics: While forward dynamics computes motion from forces, inverse dynamics calculates the forces required to achieve a desired motion.
• Computational Challenge: A naive implementation requires inverting the large, system-wide mass matrix, an O(n³) operation. The Featherstone algorithm's recursive formulation achieves this in O(n) time.

Spatial vector algebra is the 6D mathematical framework that enables the Featherstone algorithm's elegant and compact formulation. It combines three-dimensional linear and angular quantities (like velocity, acceleration, and force) into single six-dimensional vectors and matrices.

• Key Benefit: This unified representation dramatically simplifies the recursive equations of motion for multi-body systems, avoiding separate handling of linear and rotational components.
• Spatial Inertia: A core concept is the spatial inertia matrix, which compactly represents a rigid body's mass, center of mass, and rotational inertia, allowing forces to be related to accelerations in one operation.

Articulated Body Inertia is the central, recursive quantity computed by the Featherstone algorithm. It represents the effective inertia of a subtree of links (from a specific joint outward to the end of the chain) as felt at that joint, taking into account the dynamics of all descendant bodies.

• Recursive Computation: The algorithm calculates this inertia by propagating from the leaf links back toward the base, combining each link's spatial inertia with the articulated body inertia of its outward neighbors.
• Purpose: This pre-computed inertia allows the forward dynamics pass to calculate joint accelerations in a single sweep from the base to the tips, without solving a large coupled system.

A constraint solver is the algorithmic subsystem in a physics engine that calculates forces or impulses to enforce conditions like contact non-penetration and joint limits. While the Featherstone algorithm computes free-body dynamics, real-world simulation requires integrating constraints.

• Interaction: The accelerations computed by forward dynamics serve as input to the constraint solver. The solver then calculates corrective constraint forces, which are fed back into the dynamics equations.
• Common Formulations: Solvers often frame contact and friction as a Linear Complementarity Problem (LCP) or use a penalty method. Efficient solvers use techniques like warm starting from the previous solution to accelerate convergence.

The Newton-Euler equations are the fundamental differential equations of motion for a single rigid body. They combine Newton's second law for linear motion (F = ma) and Euler's equation for rotational motion (τ = Iα + ω × I*ω). The Featherstone algorithm is essentially a recursive, efficient method for solving these equations for an entire connected chain of bodies.

• Base Foundation: The algorithm's recursions are derived from applying the Newton-Euler equations to each individual link while accounting for the forces and torques transmitted through the joints.
• Gyroscopic Forces: The Euler equation includes the gyroscopic term (ω × I*ω), which is crucial for accurately simulating rotating bodies and is inherently handled in the spatial vector formulation.

Operational space control is a robotics control framework where forces are computed directly in the task space (e.g., for a robot's end-effector) rather than in joint space. The Featherstone algorithm is often a critical backend for efficient implementations of this control method.

• Dynamics Relationship: Implementing operational space control requires the dynamically consistent generalized inverse of the Jacobian, which depends on the current mass matrix of the manipulator. The Featherstone algorithm can compute the quantities needed for this inverse very efficiently.
• Application: This enables advanced control strategies where a robot can, for example, make its end-effector appear to have a specific mass or follow a compliant trajectory, directly leveraging the articulated body inertia concepts.