featherstone

A robotics dynamics engine in Rust.

Given a robot (a tree of rigid bodies connected by joints), featherstone answers two questions:

1. Forward dynamics — Given joint positions, velocities, and applied torques, what are the accelerations? (used in simulation)
2. Inverse dynamics — Given desired accelerations, what torques are required? (used in control)

It does this in O(n) time using Featherstone's Articulated Body Algorithm — the same class of algorithms used by MuJoCo, Drake, and Pinocchio. Unlike those C++ libraries, featherstone is pure Rust with minimal dependencies, f32 throughout for GPU/SIMD compatibility, and designed for batching thousands of parallel environments (RL training).

What it includes: forward/inverse dynamics, mass matrix computation, forward kinematics, Jacobians, 5 time integrators, GJK/EPA collision detection, LCP and smooth contact solvers, URDF loading, joint limits.

What it doesn't include: rendering, scene management, asset loading, networking. It's the math engine, not the simulator.

Getting Started

Add to your Cargo.toml:

[dependencies]
featherstone = "0.1"
nalgebra = "0.33"

Features

• ABA -- Articulated Body Algorithm: O(n) forward dynamics
• RNEA -- Recursive Newton-Euler: O(n) inverse dynamics, gravity compensation
• CRBA -- Composite Rigid Body Algorithm: mass matrix computation
• Forward Kinematics -- body transforms, velocities, Jacobians, center of mass
• GJK/EPA -- convex collision detection
• LCP Solver -- Projected Gauss-Seidel with warm-starting for rigid contacts
• Newton Solver -- quadratic-convergence contact solver for stacking
• Smooth Contacts -- differentiable contact forces for gradient-based optimization
• 5 Integrators -- semi-implicit Euler, explicit Euler, RK4, velocity Verlet, implicit Euler
• URDF Loading -- parse URDF robot descriptions (feature-gated)
• 6 Joint Types -- Fixed, Revolute, Prismatic, Spherical (quaternion), Floating (6-DOF), Planar

Quick Start

use featherstone::prelude::*;
use nalgebra::Vector3;

fn main() {
    // Create an empty articulated body
    let mut body = ArticulatedBody::new();

    // Add a pendulum link: 1 kg, 0.5 m long, revolute joint around Z axis
    let inertia = SpatialInertia::cuboid(1.0, Vector3::new(0.05, 0.25, 0.05));
    let x_tree = SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0));
    body.add_body("link1", -1, GenJoint::Revolute { axis: Vector3::z() }, inertia.clone(), x_tree.clone());

    // Add a second link hanging from the first
    body.add_body("link2", 0, GenJoint::Revolute { axis: Vector3::z() }, inertia, x_tree);

    // Set initial angle (45 degrees) on the first joint
    body.q[0] = std::f32::consts::FRAC_PI_4;

    // Run forward dynamics -- computes joint accelerations from gravity
    aba_forward_dynamics(&mut body);
    println!("accelerations: {}", body.qdd);

    // Step the simulation forward
    let config = IntegratorConfig { dt: 0.001, ..Default::default() };
    for _ in 0..1000 {
        step(&mut body, &config);
    }
    println!("positions after 1s: {}", body.q);
}

Feature Flags

Flag	Default	Description
urdf	yes	URDF robot description loading via urdf-rs
serde	no	Serialization support for nalgebra types
tracing	no	Diagnostic logging via the tracing crate

Disable default features for a minimal build:

[dependencies]
featherstone = { version = "0.1", default-features = false }

Crate Structure

Module	Description
body	ArticulatedBody tree, BodyDef, state vectors
joint	GenJoint enum (Fixed, Revolute, Prismatic, Spherical, Floating, Planar)
spatial	6D spatial vectors, transforms, inertia
aba	Articulated Body Algorithm (forward dynamics)
rnea	Recursive Newton-Euler (inverse dynamics)
crba	Composite Rigid Body Algorithm (mass matrix)
kinematics	Forward kinematics, Jacobians, CoM
contact	Contact point detection and manifolds
lcp_solver	Projected Gauss-Seidel LCP contact solver
newton_solver	Newton-method contact solver for stacking
smooth_contact	Differentiable smooth contact forces
gjk	GJK/EPA convex collision detection
integrator	Time integration (5 methods)
collider	Collision geometry descriptors
limits	Joint position/velocity/effort limits
error	Error type for fallible operations
urdf_convert	URDF to ArticulatedBody conversion (feature-gated)

Performance

• All core algorithms are O(n) in the number of bodies
• Zero-allocation inner loops (pre-allocated AbaData cache)
• f32 throughout for GPU/SIMD-friendly memory layout
• Kahan compensated summation for long-horizon numerical stability
• Designed for vectorized RL environments (batch thousands of instances)

Benchmarks (6-DOF serial arm, Criterion):

Algorithm	Time	Notes
ABA forward dynamics	1.5 us	O(n) forward dynamics
RNEA inverse dynamics	530 ns	O(n) inverse dynamics
CRBA mass matrix	880 ns	O(n^2) mass matrix
Full step (semi-implicit Euler)	1.7 us	ABA + integration
Contact: sphere-sphere	19 ns	Analytical
Contact: box-box (SAT)	205 ns	Separating axis + clipping

O(n) scaling (ABA forward dynamics):

Bodies	Time	Time/body
2	510 ns	255 ns
4	1.06 us	265 ns
8	2.43 us	304 ns
16	4.85 us	303 ns
32	10.2 us	319 ns

Run benchmarks yourself:

cargo bench

Examples

cargo run --example pendulum      # Double pendulum simulation
cargo run --example urdf_robot    # URDF robot loading and simulation
cargo run --example contacts      # Ground contact with LCP solver

Documentation

• User Guides — Getting started, algorithms, contacts, integration, URDF, API patterns
• API Reference — Full rustdoc

License

Apache-2.0