Skip to content

contacts.md

Latest commit

 

History

History
122 lines (94 loc) · 3.91 KB

contacts.md

File metadata and controls

122 lines (94 loc) · 3.91 KB
Raw

Contact Dynamics

featherstone provides three contact solving approaches: LCP (rigid), smooth (differentiable), and Newton (stacking).

Ground Contacts

The simplest case: a robot interacting with a flat ground plane.

use featherstone::prelude::*;
use featherstone::contact::ground_plane_contacts;
use featherstone::contact_jacobian::ContactConstraints;

// Detect contacts with ground at y=0
let manifold = ground_plane_contacts(
    &body,
    0.0,                    // ground height
    Vector3::y(),           // ground normal (up)
    0.5,                    // friction coefficient
    0.0,                    // restitution (0 = inelastic)
);

if !manifold.contacts.is_empty() {
    // Build constraint matrices
    let constraints = ContactConstraints::from_manifold(&body, &manifold);

    // Solve for contact impulses
    let config = LcpSolverConfig::default();
    let result = solve_contact_lcp(&body, &constraints, dt, &config);

    // Apply 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() {
        body.qd += m_inv * &result.tau_contact;
    }
}

LCP Solver (Rigid Contacts)

Projected Gauss-Seidel with warm-starting. Best for rigid contacts where penetration must be zero.

let config = LcpSolverConfig {
    max_iterations: 100,
    tolerance: 1e-6,
    baumgarte: 0.1,           // penetration correction strength
    restitution_threshold: 0.01,
    warm_start: 0.9,          // reuse previous frame's impulses
    ..Default::default()
};

Key properties:

  • Normal impulse >= 0 (contacts push, never pull)
  • Friction cone: ||f_t|| <= mu * f_n
  • Complementarity: lambda * constraint = 0
  • Convergence: monotonic residual decay

Smooth Contacts (Differentiable)

Spring-damper penalty model. Best for gradient-based optimization and RL.

use featherstone::smooth_contact::{smooth_contact_forces, SmoothContactConfig};

let config = SmoothContactConfig::default();  // stiffness=1e5, damping=1e3
// Or: SmoothContactConfig::stiff() for rigid-like behavior
// Or: SmoothContactConfig::soft() for compliant contacts

let (tau_contact, energy) = smooth_contact_forces(&body, &constraints, &config);

Trade-off vs LCP:

  • Smooth: differentiable, no iteration, slight penetration allowed
  • LCP: exact non-penetration, not differentiable, iterative

Simulation Loop with Contacts

The recommended contact simulation pattern:

let dt = 0.001;
let integrator = IntegratorConfig { dt, ..Default::default() };
let lcp_config = LcpSolverConfig::default();

loop {
    // Phase 1: Compute accelerations
    featherstone::integrator::compute_accelerations(&mut body, &integrator);

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

    // Phase 3: Detect and resolve contacts (between velocity and position update)
    let manifold = ground_plane_contacts(&body, 0.0, Vector3::y(), 0.5, 0.0);
    if !manifold.contacts.is_empty() {
        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let result = solve_contact_lcp(&body, &constraints, dt, &lcp_config);
        let m = featherstone::crba::crba_mass_matrix(&body);
        if let Some(m_inv) = m.try_inverse() {
            body.qd += m_inv * &result.tau_contact;
        }
    }

    // Phase 4: Update positions
    featherstone::integrator::update_positions(&mut body, &integrator);
}

Collision Shapes

Contact detection supports:

Shape Detection Notes
Sphere-Sphere Analytical Fastest
Sphere-Box Analytical Clamped closest point
Box-Box SAT + Sutherland-Hodgman 15-axis SAT, polygon clipping
Capsule-Capsule Segment distance Analytical
Capsule-Sphere Segment-point Analytical
Capsule-Box Multi-point sphere-box 3 test points
ConvexHull-* GJK/EPA General convex shapes