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

//! Load a URDF robot and simulate forward dynamics
//!
//! Demonstrates: URDF parsing, articulated body conversion, ABA forward dynamics,
//! time integration of a 2-link revolute arm under gravity.
//!
//! Run with: cargo run --example urdf_robot --features urdf

#[cfg(not(feature = "urdf"))]
fn main() {
    eprintln!("This example requires the `urdf` feature.");
    eprintln!("Run with: cargo run --example urdf_robot --features urdf");
}

#[cfg(feature = "urdf")]
fn main() {
    use featherstone::prelude::*;
    use featherstone::urdf_convert::articulated_body_from_urdf;

    // --- Define a simple 2-link arm as inline URDF ---
    let urdf_xml = r#"
    <robot name="two_link_arm">
      <link name="base_link">
        <inertial>
          <mass value="5.0"/>
          <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
        </inertial>
      </link>
      <link name="upper_arm">
        <inertial>
          <origin xyz="0.15 0 0"/>
          <mass value="1.0"/>
          <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
        </inertial>
      </link>
      <link name="forearm">
        <inertial>
          <origin xyz="0.1 0 0"/>
          <mass value="0.5"/>
          <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.005"/>
        </inertial>
      </link>
      <joint name="shoulder" type="revolute">
        <parent link="base_link"/>
        <child link="upper_arm"/>
        <origin xyz="0 0 0.1"/>
        <axis xyz="0 0 1"/>
        <limit lower="-1.57" upper="1.57" effort="100" velocity="5"/>
      </joint>
      <joint name="elbow" type="revolute">
        <parent link="upper_arm"/>
        <child link="forearm"/>
        <origin xyz="0.3 0 0"/>
        <axis xyz="0 0 1"/>
        <limit lower="-3.14" upper="3.14" effort="50" velocity="10"/>
      </joint>
    </robot>
    "#;

    // --- Parse URDF and convert to ArticulatedBody ---
    let robot: urdf_rs::Robot = urdf_rs::read_from_string(urdf_xml).expect("Failed to parse URDF");
    let conversion = articulated_body_from_urdf(&robot).expect("Failed to convert URDF");
    let mut body = conversion.body;

    println!("Robot: {}", robot.name);
    println!("Bodies: {}, DOF: {}", body.body_count(), body.dof_count());
    for (i, name) in conversion.link_names.iter().enumerate() {
        let mass = body.bodies[i].inertia.mass;
        println!("  [{i}] {name} (mass={mass:.2} kg)");
    }
    println!();

    // --- Set initial configuration: shoulder at 45 deg ---
    body.set_joint_q(1, &[std::f32::consts::FRAC_PI_4]);
    body.set_gravity(nalgebra::Vector3::new(0.0, -9.81, 0.0));

    // --- Configure integrator ---
    let dt = 0.001;
    let config = IntegratorConfig {
        method: IntegrationMethod::SemiImplicitEuler,
        dt,
        ..Default::default()
    };

    // --- Simulate for 1 second, print every 100 ms ---
    let steps_per_print = 100;
    let total_prints = 10;

    println!(
        "{:>8}  {:>12}  {:>12}  {:>10}  {:>10}",
        "time(s)", "shoulder(deg)", "elbow(deg)", "qd1", "qd2"
    );
    println!("{}", "-".repeat(62));

    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);
    }
}

#[cfg(feature = "urdf")]
fn print_state(t: f32, body: &featherstone::body::ArticulatedBody) {
    // Joint indices: body 0 is fixed base, body 1 = shoulder, body 2 = elbow
    let q1_deg = body.q[body.bodies[1].q_index].to_degrees();
    let q2_deg = body.q[body.bodies[2].q_index].to_degrees();
    let qd1 = body.qd[body.bodies[1].v_index];
    let qd2 = body.qd[body.bodies[2].v_index];
    println!(
        "{:>8.3}  {:>12.3}  {:>12.3}  {:>10.4}  {:>10.4}",
        t, q1_deg, q2_deg, qd1, qd2
    );
}