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

//! Double pendulum example
//!
//! Two rigid links connected by revolute joints swinging under gravity.
//! Demonstrates: body construction, ABA forward dynamics, time integration.
//!
//! Run with: cargo run --example pendulum

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

fn main() {
    // ---- Build the double pendulum ----

    let mut body = ArticulatedBody::new();

    // Each link: 1 kg uniform rod, 0.5 m long, 0.1 m wide
    let link_mass = 1.0;
    let half_extents = Vector3::new(0.05, 0.25, 0.05); // half-lengths (x, y, z)
    let link_inertia = SpatialInertia::cuboid(link_mass, half_extents);

    // Joint transform: translate down by the full link length (0.5 m)
    let x_tree = SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0));

    // Link 1: attached to world (parent = -1), revolute around Z axis
    body.add_body(
        "upper_link",
        -1,
        GenJoint::Revolute { axis: Vector3::z() },
        link_inertia.clone(),
        x_tree.clone(),
    );

    // Link 2: attached to link 1 (parent = 0), revolute around Z axis
    body.add_body(
        "lower_link",
        0,
        GenJoint::Revolute { axis: Vector3::z() },
        link_inertia,
        x_tree,
    );

    // ---- Set initial conditions ----

    // Start the upper link at 90 degrees (horizontal)
    body.q[0] = std::f32::consts::FRAC_PI_2;
    // Lower link hangs straight down (0 degrees relative to upper)
    body.q[1] = 0.0;

    // ---- Configure the integrator ----

    let dt = 0.001; // 1 ms timestep
    let config = IntegratorConfig {
        method: IntegrationMethod::SemiImplicitEuler,
        dt,
        ..Default::default()
    };

    // ---- Simulate and print state every 100 ms ----

    let steps_per_print = 100; // 100 steps * 1ms = 100ms
    let total_prints = 20; // 2 seconds total

    println!("Double Pendulum Simulation");
    println!("==========================");
    println!(
        "{:>8}  {:>10}  {:>10}  {:>10}  {:>10}",
        "time(s)", "q1(deg)", "q2(deg)", "qd1", "qd2"
    );
    println!("{}", "-".repeat(58));

    // Print initial state
    print_state(0.0, &body);

    for i in 1..=total_prints {
        for _ in 0..steps_per_print {
            step(&mut body, &config);
        }
        let t = i as f32 * steps_per_print as f32 * dt;
        print_state(t, &body);
    }

    // ---- Final summary ----

    println!("{}", "-".repeat(58));
    println!(
        "Total energy drift is small thanks to the symplectic integrator."
    );
}

fn print_state(t: f32, body: &ArticulatedBody) {
    let q1_deg = body.q[0].to_degrees();
    let q2_deg = body.q[1].to_degrees();
    let qd1 = body.qd[0];
    let qd2 = body.qd[1];
    println!(
        "{:>8.3}  {:>10.3}  {:>10.3}  {:>10.4}  {:>10.4}",
        t, q1_deg, q2_deg, qd1, qd2
    );
}