featherstone 0.1.0

Robotics dynamics engine — O(n) forward/inverse dynamics for kinematic trees, contact solvers, and time integration
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118

# Integration Methods

featherstone provides 5 time integration schemes. Choose based on your accuracy/stability needs.

## Methods

### Semi-Implicit Euler (default)

```rust
IntegrationMethod::SemiImplicitEuler
```

Updates velocity first, then position with the new velocity. Symplectic — preserves phase-space structure, good energy behavior.

- **Order:** 1st
- **ABA calls per step:** 1
- **Best for:** General simulation, contacts, real-time
- **Energy:** Bounded oscillation (no secular drift)

### Explicit Euler

```rust
IntegrationMethod::ExplicitEuler
```

Updates position first (old velocity), then velocity. Not symplectic.

- **Order:** 1st
- **ABA calls per step:** 1
- **Best for:** Comparison/debugging only
- **Energy:** Drifts upward (gains energy over time)

### RK4 (Runge-Kutta 4th order)

```rust
IntegrationMethod::RK4
```

Classic 4th-order method. Highest accuracy for smooth dynamics.

- **Order:** 4th
- **ABA calls per step:** 4
- **Best for:** Smooth dynamics without contacts, trajectory optimization
- **Energy:** Very small drift (4th order)

### Velocity Verlet

```rust
IntegrationMethod::VelocityVerlet
```

2nd-order symplectic. Good energy conservation with moderate cost.

- **Order:** 2nd
- **ABA calls per step:** 2
- **Best for:** Long-horizon energy conservation
- **Energy:** Bounded oscillation (better than Euler)

### Implicit Euler

```rust
IntegrationMethod::ImplicitEuler
```

Unconditionally stable via Newton iteration. Allows large timesteps.

- **Order:** 1st (high numerical damping)
- **ABA calls per step:** 2-5 (Newton iterations)
- **Best for:** Stiff systems (tendons, muscles, high-stiffness contacts)
- **Energy:** Dissipative (artificial damping)

## Comparison

| Method | Order | ABA/step | Symplectic | Stiff-stable | Use case |
|--------|-------|----------|------------|--------------|----------|
| Semi-implicit Euler | 1 | 1 | Yes | No | General, contacts |
| Explicit Euler | 1 | 1 | No | No | Debugging only |
| RK4 | 4 | 4 | No | No | Smooth dynamics |
| Velocity Verlet | 2 | 2 | Yes | No | Long-horizon |
| Implicit Euler | 1 | 2-5 | No | Yes | Stiff systems |

## Configuration

```rust
let config = IntegratorConfig {
    dt: 0.001,                                    // timestep (seconds)
    method: IntegrationMethod::SemiImplicitEuler,  // integration method
    velocity_damping: 1.0,                         // 1.0 = no damping, 0.99 = slight
    normalize_quaternions: true,                   // keep unit quaternions
    joint_limits: vec![                            // per-joint limits (optional)
        Some(JointLimits::from_urdf_params(-1.57, 1.57, 5.0, 50.0)),
        None,  // no limits on joint 1
    ],
};
```

## Split-Phase API

For contact simulation, use the split-phase API to inject contact corrections between velocity and position updates:

```rust
// Phase A: compute accelerations (ABA)
featherstone::integrator::compute_accelerations(&mut body, &config);

// Phase B: update velocities (qd += qdd * dt)
featherstone::integrator::update_velocities(&mut body, &config);

// --- inject contact velocity corrections here ---

// Phase C: update positions (q += qd * dt)
featherstone::integrator::update_positions(&mut body, &config);
```

## Numerical Precision

All integrators use **Kahan compensated summation** for f32 accumulation. This reduces round-off drift from ~0.15% to ~0.01% over long simulations.

The compensation is maintained automatically. If you directly set `body.qd[i]` or `body.q[i]`, use `set_joint_qd()` / `set_joint_q()` instead — these reset the compensation vector.