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

//! Ground contact example with LCP solver
//!
//! A box dropped from height onto a ground plane. Demonstrates: contact detection,
//! LCP impulse solving, and the full simulation loop with contacts.
//!
//! Run with: cargo run --example contacts

use nalgebra::{Matrix3, Vector3};
use featherstone::prelude::*;
use featherstone::contact::ground_plane_contacts;
use featherstone::contact_jacobian::ContactConstraints;

fn main() {
    // ---- Build a floating box ----

    let mut body = ArticulatedBody::new();
    body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

    // 1 kg box, 0.2 m half-extents, floating base (6-DOF)
    let mass = 1.0;
    let inertia = SpatialInertia::from_mass_inertia_at_com(
        mass,
        Matrix3::from_diagonal(&Vector3::new(0.067, 0.067, 0.067)),
    );
    body.add_body("box", -1, GenJoint::Floating, inertia, SpatialTransform::identity());

    // Start at height 2.0 m (position: x, y, z, qw, qx, qy, qz)
    body.set_joint_q(0, &[0.0, 2.0, 0.0, 1.0, 0.0, 0.0, 0.0]);

    // ---- Simulation loop ----

    let dt = 0.001;
    let integrator_config = IntegratorConfig {
        dt,
        method: IntegrationMethod::SemiImplicitEuler,
        normalize_quaternions: true,
        ..Default::default()
    };

    let lcp_config = LcpSolverConfig {
        max_iterations: 20,
        tolerance: 1e-6,
        baumgarte: 0.2,
        ..Default::default()
    };

    let total_steps = 3000; // 3 seconds
    let print_every = 500;  // every 0.5s

    println!("Dropping box from 2.0 m onto ground plane...\n");
    println!("{:>6}  {:>8}  {:>8}  {:>10}", "time", "height", "vel_y", "contacts");
    println!("{:-<6}  {:-<8}  {:-<8}  {:-<10}", "", "", "", "");

    for i in 0..total_steps {
        // Phase 1: Compute accelerations (ABA)
        featherstone::integrator::compute_accelerations(&mut body, &integrator_config);

        // Phase 2: Update velocities
        featherstone::integrator::update_velocities(&mut body, &integrator_config);

        // Phase 3: Detect ground contacts
        let manifold = ground_plane_contacts(&body, 0.0, Vector3::y(), 0.5, 0.0);
        let nc = manifold.contacts.len();

        // Phase 4: Solve contact impulses and apply velocity correction
        if nc > 0 {
            let constraints = ContactConstraints::from_manifold(&body, &manifold);
            let result = solve_contact_lcp(&body, &constraints, dt, &lcp_config);

            // Apply contact impulse as velocity correction: dqd = M^{-1} * tau_contact
            let m = featherstone::crba::crba_mass_matrix(&body);
            if let Some(m_inv) = m.try_inverse() {
                let dqd = m_inv * &result.tau_contact;
                body.qd += dqd;
            }
        }

        // Phase 5: Update positions
        featherstone::integrator::update_positions(&mut body, &integrator_config);

        // Print status
        if i % print_every == 0 || i == total_steps - 1 {
            let height = body.q[1]; // y position
            let vel_y = body.qd[1]; // y velocity
            let time = (i as f32 + 1.0) * dt;
            println!("{time:6.3}  {height:8.4}  {vel_y:8.4}  {nc:>10}");
        }
    }

    println!("\nFinal position: ({:.4}, {:.4}, {:.4})", body.q[0], body.q[1], body.q[2]);
}