From 09fd7e992b2c62aa9753e5bebf1dfd6a2fac76bc Mon Sep 17 00:00:00 2001 From: apirrone Date: Sun, 13 Sep 2026 14:19:54 +0200 Subject: [PATCH 1/2] odometry: anchor sets sampled on the alpha sole mesh MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The contact odometry picked its anchor from four hardcoded corners: the v1.5 sole bbox, ±27.0 x ±20.6 mm on the foot-site Z = 0 plane, a placeholder since the port. The alpha sole is 54 mm long, sits 7 mm forward of the site, and its bottom is canted 4.9° about X in the site frame, so those corners float 2 mm above the real contact on the low side and 3 mm under the mesh on the high side. `anchors.rs` is GENERATED by microduck_rl/scripts/odom_anchor_points.py, which drops a grid of vertical rays onto the sole collision mesh: V15 the legacy corners (unchanged behaviour) ALPHA4 the flat contact patch's four corners, on the mesh ALPHA16 a 4x4 grid over the whole footprint, bevels included `Odometry` takes an `&'static AnchorSet`; `Odometry::alpha()` still uses V15, so robotd is unchanged until the sets have been compared (infer_policy --odom-compare in microduck_rl). `alpha_with(&ALPHA16)` is the switch. Cost, `update_cost_per_anchor_set` on an x86 laptop: 484 ns per update with 4 points per foot, 951 ns with 16. On the Zero 3's A55 expect a handful of µs — nothing next to a 20 ms tick. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_014ZPSQ6nCsfqannMDXwAKZ3 --- odometry/src/anchors.rs | 95 +++++++++++++++++++++++++++++++ odometry/src/lib.rs | 123 ++++++++++++++++++++++++++++++++++------ 2 files changed, 202 insertions(+), 16 deletions(-) create mode 100644 odometry/src/anchors.rs diff --git a/odometry/src/anchors.rs b/odometry/src/anchors.rs new file mode 100644 index 00000000..f834be6b --- /dev/null +++ b/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], + ], +}; diff --git a/odometry/src/lib.rs b/odometry/src/lib.rs index a3742b5c..d3c9d01d 100644 --- a/odometry/src/lib.rs +++ b/odometry/src/lib.rs @@ -21,16 +21,18 @@ //! 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`] still uses the legacy v1.5 bbox +//! ([`V15`]) until the alpha sets have been compared against it. + +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 +49,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; JOINT_NAMES.len()], @@ -69,7 +73,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 +82,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 +97,20 @@ impl Odometry { } } + /// The alpha robot with the legacy [`V15`] corners — today's production + /// estimator. pub fn alpha() -> Self { - Self::new(Model::alpha()) + Self::new(Model::alpha(), &V15) + } + + /// 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 +210,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 +242,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. From dd0f8c221266132be75ac44426dcf7f9e5497b25 Mon Sep 17 00:00:00 2001 From: apirrone Date: Sun, 13 Sep 2026 14:53:40 +0200 Subject: [PATCH 2/2] odometry: ALPHA16 is the default anchor set Measured in the MuJoCo twin (infer_policy --odom-compare, perfect IMU, 3.2 m of keyboard-driven walking, 23.8 s): set final xy err max xy err final z err v15 17.3 mm 24.5 mm +3.7 mm alpha4 15.6 mm 21.6 mm +0.9 mm alpha16 9.1 mm 19.7 mm +0.5 mm Odometry::alpha() now runs the 16-point grid. V15 and ALPHA4 stay as named sets for alpha_with() and the cost/reference tests. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_014ZPSQ6nCsfqannMDXwAKZ3 --- odometry/src/lib.rs | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/odometry/src/lib.rs b/odometry/src/lib.rs index d3c9d01d..2b94eaa9 100644 --- a/odometry/src/lib.rs +++ b/odometry/src/lib.rs @@ -24,8 +24,11 @@ //! //! 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`] still uses the legacy v1.5 bbox -//! ([`V15`]) until the alpha sets have been compared against it. +//! (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; @@ -97,10 +100,9 @@ impl Odometry { } } - /// The alpha robot with the legacy [`V15`] corners — today's production - /// estimator. + /// The alpha robot with the [`ALPHA16`] grid — the production estimator. pub fn alpha() -> Self { - Self::new(Model::alpha(), &V15) + Self::new(Model::alpha(), &ALPHA16) } /// The alpha robot with a chosen anchor set.