Odom anchor sets

odometry/src/anchors.rs

@@ -0,0 +1,95 @@
+//! Sole anchor points for the contact odometry, in each foot's site frame
+//! (X front/back, Y left/right, Z up), metres.
+//!
+//! GENERATED by microduck_rl/scripts/odom_anchor_points.py — do not edit:
+//!   odom_anchor_points.py --rust /home/antoine/Pollen/microduck/odometry/src/anchors.rs
+//! from src/mjlab_microduck/robot/microduck/scene.xml.
+
+/// A named set of candidate contact points, one list per foot.
+pub struct AnchorSet {
+    pub name: &'static str,
+    pub left: &'static [[f64; 3]],
+    pub right: &'static [[f64; 3]],
+}
+
+impl AnchorSet {
+    /// Points for foot 0 (left) or 1 (right).
+    pub fn foot(&self, foot: usize) -> &'static [[f64; 3]] {
+        if foot == 0 { self.left } else { self.right }
+    }
+}
+
+/// Legacy: the v1.5 sole bbox on the site's Z = 0 plane, both feet alike.
+pub const V15: AnchorSet = AnchorSet {
+    name: "v15",
+    left: &[
+        [0.027000, 0.020600, 0.000000],
+        [0.027000, -0.020600, 0.000000],
+        [-0.027000, -0.020600, 0.000000],
+        [-0.027000, 0.020600, 0.000000],
+    ],
+    right: &[
+        [0.027000, 0.020600, 0.000000],
+        [0.027000, -0.020600, 0.000000],
+        [-0.027000, -0.020600, 0.000000],
+        [-0.027000, 0.020600, 0.000000],
+    ],
+};
+
+/// The four corners of the alpha sole's flat contact patch, on the mesh.
+pub const ALPHA4: AnchorSet = AnchorSet {
+    name: "alpha4",
+    left: &[
+        [0.024844, 0.012452, 0.001414],
+        [0.024029, -0.012395, -0.000680],
+        [-0.011047, -0.013121, -0.000751],
+        [-0.011862, 0.013177, 0.001515],
+    ],
+    right: &[
+        [0.024029, 0.012395, -0.000680],
+        [0.024844, -0.012452, 0.001415],
+        [-0.011862, -0.013177, 0.001516],
+        [-0.011047, 0.013121, -0.000751],
+    ],
+};
+
+/// A 4x4 grid over the whole alpha footprint (bevels included), on the mesh.
+pub const ALPHA16: AnchorSet = AnchorSet {
+    name: "alpha16",
+    left: &[
+        [-0.018300, -0.019510, 0.005894],
+        [-0.001500, -0.019659, 0.001516],
+        [0.015500, -0.019659, 0.002082],
+        [0.031699, -0.019061, 0.005531],
+        [-0.018500, -0.006960, 0.002891],
+        [-0.001500, -0.006960, -0.000607],
+        [0.015500, -0.006960, -0.000573],
+        [0.032500, -0.006960, 0.002863],
+        [-0.018500, 0.005740, 0.003709],
+        [-0.001500, 0.005740, 0.000500],
+        [0.015500, 0.005740, 0.000474],
+        [0.032500, 0.005740, 0.003683],
+        [-0.018300, 0.018290, 0.007998],
+        [-0.001500, 0.018439, 0.003859],
+        [0.015500, 0.018439, 0.004318],
+        [0.031699, 0.017841, 0.007574],
+    ],
+    right: &[
+        [-0.018300, -0.018290, 0.007998],
+        [-0.001500, -0.018439, 0.003859],
+        [0.015500, -0.018439, 0.004320],
+        [0.031699, -0.017841, 0.007574],
+        [-0.018500, -0.005740, 0.003712],
+        [-0.001500, -0.005740, 0.000500],
+        [0.015500, -0.005740, 0.000474],
+        [0.032500, -0.005740, 0.003681],
+        [-0.018500, 0.006960, 0.002893],
+        [-0.001500, 0.006960, -0.000607],
+        [0.015500, 0.006960, -0.000573],
+        [0.032500, 0.006960, 0.002863],
+        [-0.018300, 0.019510, 0.005894],
+        [-0.001500, 0.019659, 0.001516],
+        [0.015500, 0.019659, 0.002080],
+        [0.031699, 0.019061, 0.005531],
+    ],
+};

odometry/src/lib.rs

@@ -21,16 +21,21 @@
 //! boot", which is all a relative-motion consumer needs.
 //!
 //! Alpha only, like the daemon: v1/v1.5 geometry stayed in the prototype.
+//!
+//! The candidate contact points — the "sole corners" — are an [`AnchorSet`],
+//! sampled on the sole mesh by `microduck_rl/scripts/odom_anchor_points.py`
+//! (see [`anchors`]). [`Odometry::alpha`] uses [`ALPHA16`], a 4x4 grid over
+//! the whole sole on the mesh: in the MuJoCo twin over a 3.2 m walk it drifted
+//! 9 mm where the legacy v1.5 bbox ([`V15`]) drifted 17 mm and the flat
+//! patch's four corners ([`ALPHA4`]) 16 mm, and it stands at the true height
+//! (V15 sat 3.7 mm high). The scan costs about half a microsecond more.
+
+mod anchors;
 
+pub use anchors::{ALPHA4, ALPHA16, AnchorSet, V15};
 use duck_ipc_proto::JOINT_NAMES;
 use kinematics::{Model, Pose, Quat, SiteId};
 
-/// Sole half-extents along the foot-site frame X (front/back) and Y
-/// (left/right). Placeholder carried over from the prototype: the v1.5 sole
-/// bbox values, until alpha's `sole_left.stl` is measured.
-const SOLE_HALF_LEN: f64 = 0.0270;
-const SOLE_HALF_WIDTH: f64 = 0.0206;
-
 /// A candidate corner must sit below world Z = `-SWITCH_MARGIN` to bid for the
 /// anchor. The anchor itself sits at Z = 0, so the margin is slack for FK and
 /// IMU noise, not a physical depth.
@@ -47,6 +52,8 @@ const RIGHT: usize = 1;
 pub struct Odometry {
     model: &'static Model,
     feet: [SiteId; 2],
+    /// The candidate contact points on each sole.
+    anchors: &'static AnchorSet,
     /// `JOINT_NAMES` position → model joint index. `None` for the mouth, which
     /// moves no leg.
     joint_map: [Option<usize>; JOINT_NAMES.len()],
@@ -69,7 +76,7 @@ pub struct Odometry {
 }
 
 impl Odometry {
-    pub fn new(model: &'static Model) -> Self {
+    pub fn new(model: &'static Model, anchors: &'static AnchorSet) -> Self {
         let feet = [
             model.site("left_foot").expect("model has a left_foot site"),
             model
@@ -78,6 +85,7 @@ impl Odometry {
         ];
         Self {
             feet,
+            anchors,
             joint_map: JOINT_NAMES.map(|name| model.joint_index(name)),
             angles: vec![0.0; model.num_joints()],
             model,
@@ -92,8 +100,19 @@ impl Odometry {
         }
     }
 
+    /// The alpha robot with the [`ALPHA16`] grid — the production estimator.
     pub fn alpha() -> Self {
-        Self::new(Model::alpha())
+        Self::new(Model::alpha(), &ALPHA16)
+    }
+
+    /// The alpha robot with a chosen anchor set.
+    pub fn alpha_with(anchors: &'static AnchorSet) -> Self {
+        Self::new(Model::alpha(), anchors)
+    }
+
+    /// The anchor set this estimator picks its contact point from.
+    pub fn anchors(&self) -> &'static AnchorSet {
+        self.anchors
     }
 
     /// Advance the estimate by one sensor sample.
@@ -193,17 +212,10 @@ impl Odometry {
     /// The lowest sole corner below the switch threshold, if any, with its
     /// current world X/Y.
     fn lowest_corner(&self, rot: Quat, feet: &[Pose; 2]) -> Option<(usize, [f64; 3], [f64; 2])> {
-        const CORNERS: [[f64; 3]; 4] = [
-            [SOLE_HALF_LEN, SOLE_HALF_WIDTH, 0.0],
-            [SOLE_HALF_LEN, -SOLE_HALF_WIDTH, 0.0],
-            [-SOLE_HALF_LEN, SOLE_HALF_WIDTH, 0.0],
-            [-SOLE_HALF_LEN, -SOLE_HALF_WIDTH, 0.0],
-        ];
-
         let mut lowest = -SWITCH_MARGIN;
         let mut best = None;
         for foot in [LEFT, RIGHT] {
-            for corner in CORNERS {
+            for &corner in self.anchors.foot(foot) {
                 let in_world = rot.rotate(feet[foot].transform_point(corner));
                 let world = [
                     self.position[0] + in_world[0],
@@ -232,6 +244,87 @@ mod tests {
         [(roll / 2.0).cos(), (roll / 2.0).sin(), 0.0, 0.0]
     }
 
+    /// The generated alpha sets must be sole points: inside the sole's
+    /// footprint in the site frame and within its thickness, with the right
+    /// foot the mirror image of the left.
+    #[test]
+    fn alpha_anchor_sets_lie_on_the_sole() {
+        for set in [&ALPHA4, &ALPHA16] {
+            assert_eq!(set.left.len(), set.right.len(), "{}", set.name);
+            for p in set.left.iter().chain(set.right) {
+                assert!((-0.021..=0.035).contains(&p[0]), "{} x {p:?}", set.name);
+                assert!((-0.022..=0.022).contains(&p[1]), "{} y {p:?}", set.name);
+                assert!((-0.002..=0.012).contains(&p[2]), "{} z {p:?}", set.name);
+            }
+            // Each foot is sampled on its own mesh, so the order differs; the
+            // right sole is the left one mirrored in Y (to the sampling step).
+            for l in set.left {
+                let twin = set.right.iter().any(|r| {
+                    (l[0] - r[0]).abs() < 3e-4
+                        && (l[1] + r[1]).abs() < 3e-4
+                        && (l[2] - r[2]).abs() < 3e-4
+                });
+                assert!(twin, "{}: no mirror of {l:?} on the right sole", set.name);
+            }
+        }
+        assert_eq!(ALPHA4.left.len(), 4);
+        assert_eq!(ALPHA16.left.len(), 16);
+    }
+
+    /// Reference trajectory for the Python replica in
+    /// `microduck_rl/scripts/infer_policy.py` (`PyOdometry`): the same joint
+    /// and IMU sequence fed to both must give the same positions.
+    /// `cargo test -p odometry -- --ignored --nocapture dump_reference_trajectory`
+    #[test]
+    #[ignore = "reference dump for the Python replica, run by hand"]
+    fn dump_reference_trajectory() {
+        for set in [&V15, &ALPHA4, &ALPHA16] {
+            let mut odo = Odometry::alpha_with(set);
+            let mut joints = [0.0; JOINT_NAMES.len()];
+            for i in 0..400 {
+                let t = i as f64;
+                joints[1] = 0.15 * (0.05 * t).sin(); // left_hip_roll
+                joints[2] = 0.30 * (0.07 * t).sin(); // left_hip_pitch
+                joints[3] = 0.20 * (0.07 * t).cos(); // left_knee
+                joints[12] = 0.30 * (0.07 * t).cos(); // right_hip_pitch
+                joints[13] = 0.20 * (0.07 * t).sin(); // right_knee
+                odo.update(&joints, roll_quat(0.10 * (0.05 * t).sin()));
+                let [x, y, z] = odo.position();
+                println!(
+                    "REF {} {i} {x:.9} {y:.9} {z:.9} {}",
+                    set.name,
+                    odo.anchor_foot()
+                );
+            }
+        }
+    }
+
+    /// Cost of one `update` per anchor set, for sizing the set the robot
+    /// runs. `cargo test -p odometry --release -- --ignored --nocapture`.
+    #[test]
+    #[ignore = "timing, run by hand"]
+    fn update_cost_per_anchor_set() {
+        let mut joints = [0.0; JOINT_NAMES.len()];
+        for set in [&V15, &ALPHA4, &ALPHA16] {
+            let mut odo = Odometry::alpha_with(set);
+            let iters = 200_000;
+            let start = std::time::Instant::now();
+            for i in 0..iters {
+                // Wiggle the legs so the FK and corner scan see real data.
+                let t = i as f64 * 0.02;
+                joints[0] = 0.3 * t.sin();
+                joints[5] = 0.3 * t.cos();
+                odo.update(&joints, roll_quat(0.05 * t.sin()));
+            }
+            let per = start.elapsed().as_nanos() as f64 / iters as f64;
+            println!(
+                "{:>8}: {} points/foot, {per:7.0} ns per update",
+                set.name,
+                set.left.len()
+            );
+        }
+    }
+
     /// A robot standing still is at the origin and stays there — the first
     /// update seeds the anchor so the trunk starts at (0, 0), and constant
     /// inputs must not integrate into drift.