feat: testbed rework to avoid the callback patterns (#956)

Author: sebcrozet

.github/workflows/rapier-ci-build.yml

@@ -117,21 +117,6 @@ jobs:
         run: cd crates/rapier2d && cargo build --verbose --target wasm32-unknown-unknown;
       - name: build rapier3d
         run: cd crates/rapier3d && cargo build --verbose --target wasm32-unknown-unknown;
-  build-wasm-emscripten:
-    runs-on: ubuntu-latest
-    env:
-      RUSTFLAGS: -D warnings
-    steps:
-      - uses: actions/checkout@v4
-      - run: rustup target add wasm32-unknown-emscripten
-      - name: build rapier2d
-        run: cd crates/rapier2d && cargo build --verbose --target wasm32-unknown-emscripten;
-      - name: build rapier3d
-        run: cd crates/rapier3d && cargo build --verbose --target wasm32-unknown-emscripten;
-      - name: build rapier2d --features simd-stable
-        run: cd crates/rapier2d && cargo build --verbose --target wasm32-unknown-emscripten --features simd-stable;
-      - name: build rapier3d --features simd-stable
-        run: cd crates/rapier3d && cargo build --verbose --target wasm32-unknown-emscripten --features simd-stable;
   # If this fails, consider changing your text or adding something to .typos.toml
   # You can find typos here: https://crates.io/crates/typos'
   typos:

Cargo.toml

@@ -73,10 +73,10 @@ num-traits = { version = "0.2", default-features = false }
 approx = { version = "0.5", default-features = false }
 
 # Parry (each crate picks its own variant)
-parry2d = { version = "0.28", default-features = false, features = ["required-features"] }
-parry3d = { version = "0.28", default-features = false, features = ["required-features"] }
-parry2d-f64 = { version = "0.28", default-features = false, features = ["required-features"] }
-parry3d-f64 = { version = "0.28", default-features = false, features = ["required-features"] }
+parry2d = { version = "0.29", default-features = false, features = ["required-features"] }
+parry3d = { version = "0.29", default-features = false, features = ["required-features"] }
+parry2d-f64 = { version = "0.29", default-features = false, features = ["required-features"] }
+parry3d-f64 = { version = "0.29", default-features = false, features = ["required-features"] }
 
 # Utilities
 bitflags = "2"
@@ -98,7 +98,6 @@ serde_json = "1"
 oorandom = { version = "11", default-features = false }
 
 [patch.crates-io]
-#wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" }
 #xurdf = { path = "../xurdf/xurdf" }
 
 #simba = { path = "../simba" }

README.md

@@ -1,5 +1,5 @@
 <p align="center">
-  <img src="https://www.rapier.rs/img/rapier_logo_color_textpath_dark.svg" alt="crates.io">
+  <img src="./assets/rapier-logo.jpg" alt="crates.io">
 </p>
 <p align="center">
     <a href="https://discord.gg/vt9DJSW">

assets/rapier-logo-vertical.png

@@ -9,8 +9,8 @@ use std::path::Path;
 
 use mjcf_rs::Pose as MPose;
 use mjcf_rs::body as mb;
-use mjcf_rs::equality::{Equality as MjcfEquality, EqualityConnect, EqualityWeld};
 use mjcf_rs::equality::EqualityJoint as MjcfEqualityJointDef;
+use mjcf_rs::equality::{Equality as MjcfEquality, EqualityConnect, EqualityWeld};
 use mjcf_rs::extras::Sensor as MjcfSensor;
 use mjcf_rs::glam::DVec3;
 use mjcf_rs::model::{BodyEntry, BodyId, Model};
@@ -663,8 +663,8 @@ impl<'a> Conversion<'a> {
     fn resolve_tendon_joints(&self, t: &mjcf_rs::tendon::FixedTendon) -> Vec<(usize, Real)> {
         t.joints
             .iter()
-            .filter_map(|term| {
-                match self.robot.joint_name_to_idx.get(&term.joint).copied() {
+            .filter_map(
+                |term| match self.robot.joint_name_to_idx.get(&term.joint).copied() {
                     Some(idx) => Some((idx, term.coef as Real)),
                     None => {
                         log::warn!(
@@ -674,8 +674,8 @@ impl<'a> Conversion<'a> {
                         );
                         None
                     }
-                }
-            })
+                },
+            )
             .collect()
     }
 
@@ -712,8 +712,7 @@ impl<'a> Conversion<'a> {
             }
             for &(idxk, ck) in &joints[1..] {
                 let already_coupled = self.robot.joint_couplings.iter().any(|c| {
-                    (c.joint1 == idx0 && c.joint2 == idxk)
-                        || (c.joint1 == idxk && c.joint2 == idx0)
+                    (c.joint1 == idx0 && c.joint2 == idxk) || (c.joint1 == idxk && c.joint2 == idx0)
                 });
                 if already_coupled {
                     continue;

assets/rapier-logo.jpg

@@ -327,12 +327,12 @@ pub(super) fn add_springdamper_to_multibody(
     let mut offset = 0;
     let mut ndofs = 0;
     for (i, link) in multibody.links().enumerate() {
-        let nd = link.joint().ndofs();
+        let link_ndofs = link.joint().ndofs();
         if i == link_id {
-            ndofs = nd;
+            ndofs = link_ndofs;
             break;
         }
-        offset += nd;
+        offset += link_ndofs;
     }
     if ndofs == 0 {
         return;

assets/rapier-logo.png

@@ -264,7 +264,8 @@ impl MjcfRobotHandles<Option<MultibodyJointHandle>> {
                         rb.set_position(free_pose(qp, dof.scale, robot.base_shift), true);
                     }
                     if let Some(qv) = qvel {
-                        let lin = Vector::new(qv[0] as Real, qv[1] as Real, qv[2] as Real) * dof.scale;
+                        let lin =
+                            Vector::new(qv[0] as Real, qv[1] as Real, qv[2] as Real) * dof.scale;
                         let ang = Vector::new(qv[3] as Real, qv[4] as Real, qv[5] as Real);
                         rb.set_linvel(lin, true);
                         rb.set_angvel(ang, true);
@@ -279,7 +280,9 @@ impl MjcfRobotHandles<Option<MultibodyJointHandle>> {
             // Articulated DoF (hinge / slide / ball): write to the multibody's
             // generalized coordinates.
             let Some(jidx) = dof.joint else { continue };
-            let Some(jh) = self.joints.get(jidx) else { continue };
+            let Some(jh) = self.joints.get(jidx) else {
+                continue;
+            };
             // `joint` is `None` for a joint that was dropped as a loop closure.
             let Some(handle) = jh.joint else { continue };
             if let Some(link) = multibody_joints.rigid_body_link(jh.link2) {
@@ -292,7 +295,9 @@ impl MjcfRobotHandles<Option<MultibodyJointHandle>> {
             // Compute the displacement (target − current) under an immutable
             // borrow, then apply it under a mutable one.
             let (assembly_id, ndofs, disp) = {
-                let Some(link) = mb.link(link_id) else { continue };
+                let Some(link) = mb.link(link_id) else {
+                    continue;
+                };
                 let joint = link.joint();
                 let coords = joint.coords();
                 let disp = qpos.map(|qp| match dof.kind {
@@ -375,7 +380,8 @@ impl MjcfRobotHandles<ImpulseJointHandle> {
                         world[dof.body] = free_pose(qp, dof.scale, robot.base_shift);
                     }
                     if let Some(qv) = qvel {
-                        let lin = Vector::new(qv[0] as Real, qv[1] as Real, qv[2] as Real) * dof.scale;
+                        let lin =
+                            Vector::new(qv[0] as Real, qv[1] as Real, qv[2] as Real) * dof.scale;
                         let ang = Vector::new(qv[3] as Real, qv[4] as Real, qv[5] as Real);
                         free_vels.push((dof.body, lin, ang));
                     }
@@ -402,8 +408,10 @@ impl MjcfRobotHandles<ImpulseJointHandle> {
         // joint's parent body is already positioned. This mirrors rapier's
         // multibody `body_to_parent`: world₂ = world₁ · frame1 · q · frame2⁻¹.
         for (jidx, j) in robot.joints.iter().enumerate() {
-            world[j.link2] =
-                world[j.link1] * j.joint.local_frame1 * joint_tf[jidx] * j.joint.local_frame2.inverse();
+            world[j.link2] = world[j.link1]
+                * j.joint.local_frame1
+                * joint_tf[jidx]
+                * j.joint.local_frame2.inverse();
         }
 
         for (i, handle) in self.bodies.iter().enumerate() {
@@ -445,7 +453,7 @@ impl MjcfRobotHandles<ImpulseJointHandle> {
 
     /// Like [`apply_controls`](Self::apply_controls) but uniformly scales every
     /// actuator's strength by `gain_scale` (gains and force limits; see
-    /// [`configure_actuator_motor`]). `gain_scale < 1` softens the actuation —
+    /// `configure_actuator_motor`). `gain_scale < 1` softens the actuation —
     /// e.g. to ease servo-driven moves that would otherwise saturate and snap.
     pub fn apply_controls_scaled(
         &self,
@@ -471,7 +479,7 @@ impl MjcfRobotHandles<Option<MultibodyJointHandle>> {
     /// [`apply_controls`](MjcfRobotHandles::<ImpulseJointHandle>::apply_controls):
     /// it reproduces MuJoCo's "actuation" by writing each actuator's motor
     /// onto the multibody link it drives. The per-actuator interpretation is
-    /// identical (see [`configure_actuator_motor`]); call it once per frame
+    /// identical (see `configure_actuator_motor`); call it once per frame
     /// before `pipeline.step`.
     ///
     /// `ctrl` is a flat array of one scalar per actuator, in the order of
@@ -493,7 +501,7 @@ impl MjcfRobotHandles<Option<MultibodyJointHandle>> {
 
     /// Like [`apply_controls_multibody`](Self::apply_controls_multibody) but
     /// uniformly scales every actuator's strength by `gain_scale` (gains and
-    /// force limits; see [`configure_actuator_motor`]). `gain_scale < 1` softens
+    /// force limits; see `configure_actuator_motor`). `gain_scale < 1` softens
     /// the actuation, so a servo-driven move (e.g. easing between keyframes)
     /// ramps in instead of saturating to its force limit and arriving instantly.
     pub fn apply_controls_multibody_scaled(
@@ -507,7 +515,9 @@ impl MjcfRobotHandles<Option<MultibodyJointHandle>> {
             // `H` is `Option<MultibodyJointHandle>` here, so `ah.joint` is
             // `Option<Option<_>>`: the outer `None` means "no actuator joint",
             // the inner `None` means "joint dropped as a loop closure".
-            let Some(Some(handle)) = ah.joint else { continue };
+            let Some(Some(handle)) = ah.joint else {
+                continue;
+            };
             let Some((mb, link_id)) = multibody_joints.get_mut(handle) else {
                 continue;
             };

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

@@ -0,0 +1,105 @@
+//! `apply_controls_multibody_scaled`'s `gain_scale` softens actuation: a lower
+//! scale makes a position servo approach its target more slowly (no snap).
+
+use rapier3d::prelude::*;
+use rapier3d_mjcf::{MjcfLoaderOptions, MjcfMultibodyOptions, MjcfRobot};
+
+// A strong position servo on one hinge — at full strength it slams to target.
+const SERVO: &str = r#"
+<mujoco>
+  <worldbody>
+    <body name="link" pos="0 0 1">
+      <joint name="j" type="hinge" axis="0 1 0"/>
+      <inertial mass="5" diaginertia="2 2 2"/>
+      <geom type="capsule" fromto="0 0 0 0.5 0 0" size="0.03"/>
+    </body>
+  </worldbody>
+  <actuator>
+    <position name="a" joint="j" kp="2000" kv="200" forcerange="-20 20"/>
+  </actuator>
+</mujoco>
+"#;
+
+fn angle(rb: &RigidBody) -> f32 {
+    let x = rb.rotation() * Vector::X;
+    (-x.z).atan2(x.x)
+}
+
+/// Drive `j` toward 1.0 for `steps` steps at the given strength scale; return
+/// the angle reached.
+fn drive_to(steps: usize, gain_scale: Real) -> f32 {
+    let (robot, _) = MjcfRobot::from_str(SERVO, MjcfLoaderOptions::default(), ".").unwrap();
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut ij = ImpulseJointSet::new();
+    let mut mbj = MultibodyJointSet::new();
+    let handles = robot.clone().insert_using_multibody_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut mbj,
+        &mut ij,
+        MjcfMultibodyOptions::default(),
+    );
+    let link = robot.body_name_to_idx["link"];
+    let mut pipeline = PhysicsPipeline::new();
+    let ip = IntegrationParameters::default();
+    let (mut isl, mut bp, mut np, mut ccd) = (
+        IslandManager::new(),
+        DefaultBroadPhase::new(),
+        NarrowPhase::new(),
+        CCDSolver::new(),
+    );
+    for _ in 0..steps {
+        handles.apply_controls_multibody_scaled(&mut bodies, &mut mbj, &[1.0_f32], gain_scale);
+        pipeline.step(
+            Vector::new(0.0, 0.0, 0.0), // no gravity: isolate the servo dynamics
+            &ip,
+            &mut isl,
+            &mut bp,
+            &mut np,
+            &mut bodies,
+            &mut colliders,
+            &mut ij,
+            &mut mbj,
+            &mut ccd,
+            &(),
+            &(),
+        );
+    }
+    angle(
+        bodies
+            .get(handles.bodies[link].as_ref().unwrap().body)
+            .unwrap(),
+    )
+}
+
+#[test]
+fn lower_gain_scale_approaches_target_more_slowly() {
+    let steps = 15;
+    let full = drive_to(steps, 1.0);
+    let soft = drive_to(steps, 0.1);
+    println!("full={full} soft={soft}");
+
+    // Force-limited, so the approach rate scales with strength: at 0.1 the
+    // servo moves roughly an order of magnitude less far in the same time.
+    assert!(
+        full > 0.2,
+        "full-strength servo should be well underway: {full}"
+    );
+    assert!(
+        soft < full * 0.3,
+        "0.1x servo should lag well behind: soft={soft} full={full}"
+    );
+    assert!(
+        soft > 0.0,
+        "0.1x servo should still be moving toward target: {soft}"
+    );
+}
+
+#[test]
+fn zero_ish_gain_does_not_nan() {
+    // The minimum the example slider allows is 0.02; make sure a tiny scale (and
+    // the INFINITY force cap on a forcerange-less actuator) stays finite.
+    let a = drive_to(20, 0.02);
+    assert!(a.is_finite());
+}