feat: more multibody & MJCF fixes

crates/rapier2d/tests/snapshot_portability.rs

@@ -31,7 +31,7 @@ use rapier2d::prelude::*;
 /// Snapshot size in bytes, and its FNV-1a digest. Both, because the size alone localizes a
 /// failure: a differing size means a container's *encoding* changed, an equal size with a
 /// differing digest means the values did.
-const GOLDEN: (usize, u64) = (88_532, 0x2984_7b60_fb36_4a99);
+const GOLDEN: (usize, u64) = (88_532, 0x6b31_7afb_d1fa_bf95);
 
 const STEPS: usize = 60;
 

crates/rapier3d-mjcf/src/hooks.rs

@@ -1,26 +1,28 @@
 //! Physics-hook implementations for MJCF features that need runtime
 //! filtering: `<contact><exclude>` pair-suppression and `<contact><pair>`
-//! per-pair friction / margin overrides.
+//! per-pair friction overrides.
 
 use std::collections::HashMap;
 
 use rapier3d::geometry::{ColliderHandle, SolverFlags};
 use rapier3d::math::Real;
 use rapier3d::pipeline::{ContactModificationContext, PairFilterContext, PhysicsHooks};
 
-/// Friction / margin override for a specific collider pair, sourced from a
+/// Friction override for a specific collider pair, sourced from a
 /// `<contact><pair>` element.
+///
+/// The element's `margin` is not represented: it is a contact-generation
+/// distance, which rapier's speculative contacts already cover, so applying it
+/// here would only shift the resting separation.
 #[derive(Copy, Clone, Debug, Default)]
 pub struct PairOverride {
     /// Friction coefficient override.
     pub friction: Option<Real>,
-    /// Margin override (added to the contact distance threshold).
-    pub margin: Option<Real>,
 }
 
 /// Hook implementation honouring MJCF's `<contact><exclude>` (suppress
 /// contact between two collider sets) and `<contact><pair>` (override
-/// friction / margin on a specific pair).
+/// friction on a specific pair).
 ///
 /// The user opts in by passing this object to the rapier physics pipeline
 /// (`pipeline.step(&hooks, ...)`). Without it, excludes are ignored and
@@ -30,7 +32,7 @@ pub struct MjcfContactHooks {
     /// Excluded ordered collider pairs. We store both `(a, b)` and `(b, a)`
     /// for O(1) lookup regardless of which side rapier presents first.
     pub(crate) exclude: std::collections::HashSet<(ColliderHandle, ColliderHandle)>,
-    /// Per-pair friction / margin overrides. Both orderings stored.
+    /// Per-pair friction overrides. Both orderings stored.
     pub(crate) overrides: HashMap<(ColliderHandle, ColliderHandle), PairOverride>,
 }
 
@@ -46,7 +48,7 @@ impl MjcfContactHooks {
         self.exclude.insert((b, a));
     }
 
-    /// Register a friction / margin override for a specific pair.
+    /// Register a friction override for a specific pair.
     pub fn add_override(&mut self, a: ColliderHandle, b: ColliderHandle, ov: PairOverride) {
         self.overrides.insert((a, b), ov);
         self.overrides.insert((b, a), ov);
@@ -80,13 +82,6 @@ impl PhysicsHooks for MjcfContactHooks {
                 // Contact materials are per-manifold since the solver-contact slimming.
                 *ctx.friction = f;
             }
-            // Margin: rapier's solver uses `dist` as penetration depth;
-            // shifting it acts like adding to the contact margin.
-            if let Some(m) = ov.margin {
-                for c in ctx.solver_contacts.iter_mut() {
-                    c.dist -= m;
-                }
-            }
         }
     }
 }

crates/rapier3d-mjcf/src/loader/geom.rs

@@ -6,7 +6,7 @@ use mjcf_rs::Pose as MPose;
 use mjcf_rs::body as mb;
 use mjcf_rs::glam::{DQuat, DVec3};
 use mjcf_rs::model::{BodyEntry, BodyId};
-use rapier3d::dynamics::RigidBody;
+use rapier3d::dynamics::{IntegrationParameters, RigidBody};
 #[cfg(feature = "__meshloader_is_enabled")]
 use rapier3d::geometry::MeshConverter;
 use rapier3d::geometry::{Collider, Group, InteractionGroups, SharedShape};
@@ -350,6 +350,34 @@ impl<'a> Conversion<'a> {
         Some((shape, Pose::IDENTITY))
     }
 
+    /// A geom's `margin` in rapier length units.
+    ///
+    /// MuJoCo uses `margin` as the distance at which a contact is generated,
+    /// with force only starting at `margin - gap`. Rapier already generates
+    /// contacts within its speculative distance, so a margin below that is
+    /// redundant and is dropped; a larger one is honored as a soft-CCD
+    /// prediction on the parent body (see
+    /// [`Self::body_soft_ccd_prediction`]). Mapping it to `contact_skin`
+    /// instead would thicken the surface and make the geoms rest that far
+    /// apart, which is not what the attribute means.
+    fn geom_margin(&self, g: &mb::Geom) -> Real {
+        (g.margin as Real) * self.options.scale
+    }
+
+    /// The soft-CCD prediction distance a body inherits from its geoms'
+    /// `margin`, or `0.0` if no geom asks for more than rapier's speculative
+    /// contacts already cover.
+    fn body_soft_ccd_prediction(&self, entry: &BodyEntry) -> Real {
+        let speculative = IntegrationParameters::default().prediction_distance();
+        entry
+            .body
+            .geoms
+            .iter()
+            .map(|g| self.geom_margin(g))
+            .filter(|m| *m > speculative)
+            .fold(0.0, Real::max)
+    }
+
     /// Convert one MJCF geom into a rapier collider.
     pub(super) fn build_collider(&self, g: &mb::Geom) -> Option<Collider> {
         // `compiler/discardvisual` drops visual-only geoms during compile;
@@ -369,7 +397,6 @@ impl<'a> Conversion<'a> {
         builder.shape = shape;
         builder = builder.position(body_frame_pose);
         builder = builder.friction(g.friction[0] as Real);
-        builder = builder.contact_skin(g.margin as Real);
         builder = builder.collision_groups(self.interaction_groups(g));
         // Enable contact-pair filtering / contact-modification hooks
         // unconditionally — the hooks themselves only kick in if the user
@@ -407,6 +434,10 @@ impl<'a> Conversion<'a> {
         _world_pose: MPose,
         _body: &mut RigidBody,
     ) {
+        let soft_ccd = self.body_soft_ccd_prediction(entry);
+        if soft_ccd > 0.0 {
+            _body.set_soft_ccd_prediction(soft_ccd);
+        }
         self.staged_colliders.clear();
         self.staged_collider_names.clear();
         self.staged_visual_meshes.clear();

crates/rapier3d-mjcf/src/loader/runtime.rs

@@ -5,7 +5,7 @@
 use mjcf_rs::extras::Keyframe;
 
 use rapier3d::dynamics::{
-    GenericJoint, ImpulseJointHandle, ImpulseJointSet, JointAxis, MultibodyIndex,
+    GenericJoint, ImpulseJointHandle, ImpulseJointSet, JointAxis, MotorModel, MultibodyIndex,
     MultibodyJointHandle, MultibodyJointSet, RigidBody, RigidBodySet, RigidBodyType,
 };
 use rapier3d::math::{Pose, Real, Rotation, Vector};
@@ -97,7 +97,7 @@ impl<H> MjcfRobotHandles<H> {
             }
         }
 
-        // <pair>: per-pair friction / margin overrides between two named geoms.
+        // <pair>: per-pair friction overrides between two named geoms.
         for p in &robot.contact_pairs {
             let Some(&(b1, gi1)) = robot.geom_name_to_collider.get(&p.geom1) else {
                 log::warn!("<contact><pair>: unknown geom1 `{}`", p.geom1);
@@ -125,7 +125,6 @@ impl<H> MjcfRobotHandles<H> {
             };
             let ov = PairOverride {
                 friction: p.friction.map(|f| f[0] as Real),
-                margin: p.margin.map(|m| m as Real),
             };
             hooks.add_override(h1.handle, h2.handle, ov);
         }
@@ -566,6 +565,13 @@ fn configure_actuator_motor(
 
     let ax = JointAxis::AngX;
     let lin_ax = JointAxis::LinX;
+    // MuJoCo actuators produce absolute generalized forces: a `<position>`
+    // servo applies `kp·(ctrl − q) − kv·q̇` in N·m, straight into `M q̈ = τ`.
+    // That is the force-based model; the acceleration-based one would rescale
+    // those gains by the link's inertia and change what the authored `kp`
+    // means from one link to the next.
+    data.set_motor_model(ax, MotorModel::ForceBased);
+    data.set_motor_model(lin_ax, MotorModel::ForceBased);
     // MuJoCo's `<position>` defaults `kp` to 1 when it isn't set (directly or
     // through a `<default>` class), not 0. Defaulting to 0 here would give a
     // zero-gain — i.e. completely limp — servo, which is what made e.g. the

crates/rapier3d/tests/multibody_coupling.rs

@@ -54,9 +54,11 @@ fn run_coupling(coeff: Real, offset: Real, target: Real) -> (Real, Real) {
             coeff,
             offset,
         });
-        // Drive link1's hinge to `target`.
+        // Drive link1's hinge to `target`. Internal motors honor their
+        // compliance (they are real springs, not rigid servos), so use
+        // critically damped gains that settle well within the simulated time.
         let link1_joint = &mut mb.links_mut().nth(link1_id).unwrap().joint.data;
-        link1_joint.set_motor_position(JointAxis::AngX, target, 20.0, 2.0);
+        link1_joint.set_motor_position(JointAxis::AngX, target, 100.0, 20.0);
     }
 
     let gravity = Vector::ZERO;

crates/rapier3d/tests/snapshot_portability.rs

@@ -31,7 +31,7 @@ use rapier3d::prelude::*;
 /// Snapshot size in bytes, and its FNV-1a digest. Both, because the size alone localizes a
 /// failure: a differing size means a container's *encoding* changed, an equal size with a
 /// differing digest means the values did.
-const GOLDEN: (usize, u64) = (481_524, 0x6f27_44ea_f0b9_67c7);
+const GOLDEN: (usize, u64) = (481_520, 0x506f_f44e_db9a_7b9b);
 
 const STEPS: usize = 60;
 

examples3d/debug_long_chain3.rs

@@ -6,12 +6,12 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> {
      * World
      */
     let mut world = PhysicsWorld::new();
-    let use_articulations = false;
+    let use_articulations = true;
 
     /*
      * Create the long chain.
      */
-    let num = 100;
+    let num = 85;
     let rad = 0.2;
     let shift = rad * 2.2;
 

src/dynamics/joint/multibody_joint/multibody.rs

@@ -479,6 +479,19 @@ impl Multibody {
 
         self.accelerations.fill(0.0);
 
+        // If there is significant movement, the semi-implicit coriolis term
+        // occasionally cause instabilities due to the matrix being near singular.
+        // To prevent that, we enable a check that verifies the result and fallbacks
+        // to the explicit term if needed
+        let check_implicit_coriolis_divergence = dt * self.velocities.amax() >= 1.0e-3;
+        // Generalized forces that derive from positions or external inputs
+        // (gravity, user forces, spring position terms).
+        let mut pos_forces = DVector::zeros(if check_implicit_coriolis_divergence {
+            self.ndofs
+        } else {
+            0
+        });
+
         // Eqn 42 to 45
         for i in 0..self.links.len() {
             let link = &self.links[i];
@@ -539,6 +552,16 @@ impl Multibody {
                 external_forces.as_vector(),
                 1.0,
             );
+
+            if check_implicit_coriolis_divergence {
+                let applied_forces = Force::new(rb.forces.force, rb.forces.torque);
+                pos_forces.gemv_tr(
+                    1.0,
+                    &self.body_jacobians[i],
+                    applied_forces.as_vector(),
+                    1.0,
+                );
+            }
         }
 
         self.accelerations
@@ -556,17 +579,102 @@ impl Multibody {
                     if k != 0.0 {
                         let q = self.links[li].joint.coords[a];
                         let rest = self.links[li].joint.spring_ref[a];
-                        self.accelerations[idx] += -k * (q - rest) - k * dt * self.velocities[idx];
+                        let spring_pos_force = -k * (q - rest);
+                        self.accelerations[idx] += spring_pos_force - k * dt * self.velocities[idx];
+                        if check_implicit_coriolis_divergence {
+                            pos_forces[idx] += spring_pos_force;
+                        }
                     }
                     idx += 1;
                 }
             }
         }
 
+        // Snapshot the full generalized forces: the energy guard may need them
+        // for a re-solve with the plain mass matrix.
+        let gen_forces = if check_implicit_coriolis_divergence {
+            self.accelerations.clone()
+        } else {
+            DVector::zeros(0)
+        };
+
         self.augmented_mass_indices
             .with_rearranged_rows_mut(&mut self.accelerations, |accs| {
                 self.acc_inv_augmented_mass.solve_mut(accs);
             });
+
+        if check_implicit_coriolis_divergence {
+            self.free_velocity_energy_guard(dt, &gen_forces, &pos_forces);
+        }
+    }
+
+    // If the semi-implicit coriolis solve introduces more energy than the external
+    // forces’ work, we recalculate without the coriolis term in the mass matrix.
+    //
+    // This operates by calculating the effective kinematic energy added by the
+    // external forces after the solve with the semi-implicit mass matrix, and
+    // compare it with the force’s work. If the energy delta exceeds a threshold
+    // (indicating undesired/unrealistic energy injection), we fall back to re-solving
+    // the forces effect but using only the mass matrix without the implicit coriolis
+    // terms.
+    fn free_velocity_energy_guard(&mut self, dt: Real, gen_forces: &DVector, pos_forces: &DVector) {
+        let eff_dim = self.augmented_mass_indices.dim_after_removal(self.ndofs);
+        if eff_dim == 0 {
+            return;
+        }
+
+        // Move all quantities to the kinematic-reduced ordering of the
+        // factorized matrices (no-op when there is no kinematic dof).
+        let mut v = self.velocities.clone();
+        let mut f = gen_forces.clone();
+        let mut f_pos = pos_forces.clone();
+        self.augmented_mass_indices.rearrange_rows(&mut v, true);
+        self.augmented_mass_indices.rearrange_rows(&mut f, true);
+        self.augmented_mass_indices.rearrange_rows(&mut f_pos, true);
+
+        let m = self.augmented_mass.view((0, 0), (eff_dim, eff_dim));
+        let v = v.rows(0, eff_dim);
+        let f = f.rows(0, eff_dim);
+        let f_pos = f_pos.rows(0, eff_dim);
+
+        let energy_delta_minus_work = |accelerations: &DVector| -> Option<Real> {
+            let mut a = accelerations.clone();
+            self.augmented_mass_indices.rearrange_rows(&mut a, true);
+            let a = a.rows(0, eff_dim).into_owned();
+            if !a.iter().all(|x| x.is_finite()) {
+                return None;
+            }
+            let m_a = m * &a;
+            let energy_delta = dt * v.dot(&m_a) + 0.5 * dt * dt * a.dot(&m_a);
+            let work = dt * v.dot(&f) + 0.5 * dt * dt * a.dot(&f_pos);
+            // Margin: a small fraction of the current kinetic energy to absorb
+            // discretization error on healthy steps.
+            let kinetic_energy = 0.5 * v.dot(&(m * v));
+            let energy_margin = 1.0e-2 * kinetic_energy + 1.0e-8;
+            Some(energy_delta - work - energy_margin)
+        };
+
+        match energy_delta_minus_work(&self.accelerations) {
+            Some(excess) if excess <= 0.0 => (), // Implicit update didn’t introduce too much extra energy.
+            _ => {
+                // The implicit solve injected energy (or produced non-finite
+                // values): fall back to the plain mass matrix.
+                self.accelerations.copy_from(gen_forces);
+                self.augmented_mass_indices.with_rearranged_rows_mut(
+                    &mut self.accelerations,
+                    |accs| {
+                        self.inv_augmented_mass.solve_mut(accs);
+                    },
+                );
+
+                // Last-chance check: if all else fails, just clear the accelerations vector
+                // instead of breaking the simulation. Hopefully it will get back to a
+                // sane result in a close timestep.
+                if !self.accelerations.iter().all(|x| x.is_finite()) {
+                    self.accelerations.fill(0.0);
+                }
+            }
+        }
     }
 
     /// Computes the constant terms of the dynamics.

src/dynamics/joint/multibody_joint/unit_multibody_joint.rs

@@ -45,6 +45,7 @@ pub fn unit_joint_limit_constraint(
         min_enabled as u32 as Real * -Real::MAX,
         max_enabled as u32 as Real * Real::MAX,
     ];
+    let cfm_gain = lhs * cfm_coeff;
 
     let constraint = GenericJointConstraint {
         is_rigid_body1: false,
@@ -59,11 +60,11 @@ pub fn unit_joint_limit_constraint(
         joint_id: usize::MAX, // TODO: we don’t support impulse writeback for internal constraints yet.
         impulse: 0.0,
         impulse_bounds,
-        inv_lhs: crate::utils::inv(lhs),
+        inv_lhs: crate::utils::inv(lhs + cfm_gain),
         rhs: rhs_wo_bias + rhs_bias,
         rhs_wo_bias,
         cfm_coeff,
-        cfm_gain: 0.0,
+        cfm_gain,
         writeback_id: WritebackId::Limit(dof_id),
     };
 
@@ -102,6 +103,7 @@ pub fn unit_joint_motor_constraint(
 
     let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J
     let impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
+    let cfm_gain = lhs * motor_params.cfm_coeff + motor_params.cfm_gain;
 
     let mut rhs_wo_bias = 0.0;
     if motor_params.erp_inv_dt != 0.0 {
@@ -132,8 +134,8 @@ pub fn unit_joint_motor_constraint(
         impulse: 0.0,
         impulse_bounds,
         cfm_coeff: motor_params.cfm_coeff,
-        cfm_gain: motor_params.cfm_gain,
-        inv_lhs: crate::utils::inv(lhs),
+        cfm_gain,
+        inv_lhs: crate::utils::inv(lhs + cfm_gain),
         rhs: rhs_wo_bias,
         rhs_wo_bias,
         writeback_id: WritebackId::Limit(dof_id),