# 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`:
```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
```rust
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
| `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:
```toml
[dependencies]
featherstone = { version = "0.1", default-features = false }
```
## Crate Structure
| `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):
| 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):
| 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:
```sh
cargo bench
```
## Examples
```sh
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](docs/)** — Getting started, algorithms, contacts, integration, URDF, API patterns
- **[API Reference](https://docs.rs/featherstone)** — Full rustdoc
## License
Apache-2.0