Skip to content

api-patterns.md

Latest commit

 

History

History
158 lines (118 loc) · 4.3 KB

api-patterns.md

File metadata and controls

158 lines (118 loc) · 4.3 KB

API Patterns

Common patterns for using featherstone effectively.

Building Robots Programmatically

Serial chain (robot arm)

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

let link_mass = 1.0;
let link_length = 0.3;

for i in 0..6 {
    let parent = if i == 0 { -1 } else { (i - 1) as i32 };
    arm.add_body(
        format!("link{i}"),
        parent,
        GenJoint::Revolute { axis: Vector3::z() },
        SpatialInertia::from_mass_inertia(
            link_mass,
            Vector3::new(link_length / 2.0, 0.0, 0.0),  // COM at center
            Matrix3::identity() * 0.01,
        ),
        SpatialTransform::from_translation(Vector3::new(link_length, 0.0, 0.0)),
    );
}

Floating base (mobile robot)

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

// Floating base: 6 DOF (translation + rotation)
robot.add_body("base", -1, GenJoint::Floating,
    SpatialInertia::sphere(10.0, 0.3),
    SpatialTransform::identity());

// Arm attached to base
robot.add_body("arm", 0, GenJoint::Revolute { axis: Vector3::z() },
    SpatialInertia::sphere(1.0, 0.1),
    SpatialTransform::from_translation(Vector3::new(0.2, 0.0, 0.0)));

// Set initial height: q = [x, y, z, qw, qx, qy, qz]
robot.set_joint_q(0, &[0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0]);

Branching tree (humanoid)

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

// Torso (floating base)
humanoid.add_body("torso", -1, GenJoint::Floating,
    SpatialInertia::sphere(10.0, 0.2), SpatialTransform::identity());

// Both arms branch from torso (parent = 0)
humanoid.add_body("left_arm", 0, GenJoint::Revolute { axis: Vector3::z() },
    SpatialInertia::sphere(2.0, 0.1),
    SpatialTransform::from_translation(Vector3::new(0.0, 0.3, 0.2)));

humanoid.add_body("right_arm", 0, GenJoint::Revolute { axis: Vector3::z() },
    SpatialInertia::sphere(2.0, 0.1),
    SpatialTransform::from_translation(Vector3::new(0.0, 0.3, -0.2)));

Computed-Torque Control

Track a desired trajectory using inverse dynamics:

// Desired state
let q_des = vec![0.5, -0.3];
let qd_des = vec![0.0, 0.0];
let qdd_des = vec![0.0, 0.0];

// PD gains
let kp = 100.0;
let kd = 20.0;

// Compute feedforward torque via RNEA
body.qdd[0] = qdd_des[0] + kp * (q_des[0] - body.q[0]) + kd * (qd_des[0] - body.qd[0]);
body.qdd[1] = qdd_des[1] + kp * (q_des[1] - body.q[1]) + kd * (qd_des[1] - body.qd[1]);
let (tau, _) = rnea_inverse_dynamics(&body);
body.tau = tau;

// Step simulation with computed torques
aba_forward_dynamics(&mut body);
step(&mut body, &config);

Gravity Compensation

Hold a pose against gravity:

let tau_grav = gravity_compensation(&body);
body.tau = tau_grav;
// Now ABA will produce zero accelerations (balanced)

Forward Kinematics

Get world-frame positions and velocities:

use featherstone::kinematics::*;

let fk = forward_kinematics(&body);

// Position and orientation of each body
for i in 0..body.body_count() {
    let pos = fk.transforms[i].translation;
    let rot = fk.transforms[i].rotation;
    println!("Body {i}: pos={pos}, rot={rot}");
}

// Jacobian: maps joint velocities to body spatial velocity
let jac = body_jacobian(&body, body.body_count() - 1);  // end-effector
let v_ee = &jac * &body.qd;  // end-effector velocity

// Center of mass
let com = com_position(&body);

SpatialInertia Constructors

// From mass, COM offset, and rotational inertia at COM
SpatialInertia::from_mass_inertia(mass, com_offset, inertia_at_com);

// From mass and rotational inertia at COM (COM at body frame origin)
SpatialInertia::from_mass_inertia_at_com(mass, inertia_at_com);

// Preset shapes (uniform density)
SpatialInertia::sphere(mass, radius);
SpatialInertia::cuboid(mass, half_extents);  // Vector3
SpatialInertia::cylinder(mass, radius, half_height);

Thread Safety

ArticulatedBody is fully owned (no Rc, RefCell, or interior mutability). Each instance is independent. Safe to use from multiple threads with separate instances:

let bodies: Vec<ArticulatedBody> = (0..1000).map(|_| make_robot()).collect();

// Process in parallel (e.g., for RL vectorized environments)
bodies.par_iter_mut().for_each(|body| {
    aba_forward_dynamics(body);
    step(body, &config);
});