2/2 Use odom topic with velocity and add noise model on top

README.md

@@ -128,8 +128,10 @@ no build flag, no shim and no code change — the same code path they use agains
 - **Transport** is UDPv4 + shared memory by default. `./b run` gives the container `--network host --ipc host`
   precisely so both survive the docker boundary; set `K1_DDS_UDP_ONLY=1` (or `udp_only: true`) if only UDP works
   in your setup.
-- **Topics** — sim publishes `rt/low_state` (IMU + serial/parallel motor state, 50 Hz), `rt/odometer_state`,
-  `rt/fall_down` (on change, ≥1 Hz keepalive) and `rt/battery_state` (constant SOC); sim subscribes to
+- **Topics** — sim publishes `rt/low_state` (IMU + serial/parallel motor state, 50 Hz), `rt/odometer_state`
+  and `rt/odom` (the controller's odometry: ground truth, or drifting with the error model in
+  [`mujoco/config/odometry.yaml`](mujoco/config/odometry.yaml) enabled), `rt/head_pose`, `rt/fall_down` (on change,
+  ≥1 Hz keepalive) and `rt/battery_state` (constant SOC); sim subscribes to
   `rt/joint_ctrl` (`LowCmd`, PD-tracked at 1 kHz in CUSTOM mode). The RPC pair `rt/LocoApiTopicReq` /
   `rt/LocoApiTopicResp` serves `CHANGE_MODE`, `MOVE`, `ROTATE_HEAD`, `LIE_DOWN`, `GET_UP`,
   `GET_UP_WITH_MODE`, `VISUAL_KICK` and `GET_MODE`; unimplemented `api_id`s are accepted with a warning

docs/K1_MUJOCO_SETUP.md

@@ -226,6 +226,7 @@ runs show whether the robot is actually moving.
 | `gains.yaml` | Per-joint PD stiffness/damping + ready pose (seeded from Booster's official K1 deploy config) |
 | `locomotion.yaml` | `module::Locomotion` — initial mode, prepare blend time, fall thresholds |
 | `dds.yaml` | `module::SdkBridge` — DDS domain, UDP-only fallback, battery SOC, unknown-RPC status |
+| `odometry.yaml` | `module::SdkBridge` — odometry error model (scale, white noise, bias drift) on `rt/odometer_state` / `rt/odom`; off = ground truth |
 
 All are read at startup (`--config-dir` or `$K1SIM_CONFIG_DIR` to point elsewhere); `--model`/`--rtf` on the
 command line override the corresponding YAML value for one-off runs (this is what `K1_MODEL`/`K1_RTF` above

mujoco/config/odometry.yaml

@@ -0,0 +1,31 @@
+# Error model of the controller's odometry (module::SdkBridge OdometryModel), as published on
+# rt/odometer_state and rt/odom. The sim's base state is ground truth; the real K1's odometry is
+# the controller's estimate, which drifts. Per planar body axis (forward x, sideways y, yaw z):
+#
+#   v_est = scale * v_true + bias + white noise,
+#
+# and the odometry pose is integrated from v_est. The white noise is a density, so the pose error
+# grows the same whatever the publish rate: a density s gives a position (heading) random walk of
+# s * sqrt(t). The bias is stationary (standard deviation bias_sigma) and wanders over
+# bias_time_constant, so over a window of T seconds it drifts the pose steadily by about
+# bias_sigma * T while T is short of the time constant, and like more random walk beyond it.
+#
+# The values below are untuned placeholders. Fit them from a real recording with motion capture
+# (NUbots_K1 branch tumminello/k1_odom_record: record with the data/odometry_mocap role, then
+# `./b nbs odometry_fit <recording>` prints these values), then enable.
+
+enabled: false # false: publish the ground truth
+
+# Multiplicative error on the true body velocity [vx, vy, wz]; 1 is exact
+scale: [1.0, 1.0, 1.0]
+
+# White noise density on the body velocity [m/sqrt(s), m/sqrt(s), rad/sqrt(s)]
+velocity_noise_density: [0.02, 0.02, 0.02]
+
+# Standard deviation of the velocity bias [m/s, m/s, rad/s]
+bias_sigma: [0.01, 0.01, 0.01]
+
+# Time over which the bias decorrelates [s]
+bias_time_constant: [10.0, 10.0, 10.0]
+
+seed: 0 # random seed; 0 seeds randomly each run

mujoco/idl/builtin_interfaces/msg/Time.idl

@@ -0,0 +1,13 @@
+// builtin_interfaces::msg::dds_::Time_ — see PROTOCOL.md.
+// Field layout verified against include/booster/idl/builtin_interfaces/Time.h.
+
+module builtin_interfaces {
+    module msg {
+        module dds_ {
+            struct Time_ {
+                long sec;
+                unsigned long nanosec;
+            };
+        };
+    };
+};

mujoco/idl/geometry_msgs/msg/PoseWithCovariance.idl

@@ -0,0 +1,15 @@
+// geometry_msgs::msg::dds_::PoseWithCovariance_ — see PROTOCOL.md.
+// Field layout verified against include/booster/idl/geometry_msgs/PoseWithCovariance.h.
+
+#include "Pose.idl"
+
+module geometry_msgs {
+    module msg {
+        module dds_ {
+            struct PoseWithCovariance_ {
+                geometry_msgs::msg::dds_::Pose_ pose;
+                double covariance[36];
+            };
+        };
+    };
+};

mujoco/idl/geometry_msgs/msg/Twist.idl

@@ -0,0 +1,15 @@
+// geometry_msgs::msg::dds_::Twist_ — see PROTOCOL.md.
+// Field layout verified against include/booster/idl/geometry_msgs/Twist.h.
+
+#include "Vector3.idl"
+
+module geometry_msgs {
+    module msg {
+        module dds_ {
+            struct Twist_ {
+                geometry_msgs::msg::dds_::Vector3_ linear;
+                geometry_msgs::msg::dds_::Vector3_ angular;
+            };
+        };
+    };
+};

mujoco/idl/geometry_msgs/msg/TwistWithCovariance.idl

@@ -0,0 +1,15 @@
+// geometry_msgs::msg::dds_::TwistWithCovariance_ — see PROTOCOL.md.
+// Field layout verified against include/booster/idl/geometry_msgs/TwistWithCovariance.h.
+
+#include "Twist.idl"
+
+module geometry_msgs {
+    module msg {
+        module dds_ {
+            struct TwistWithCovariance_ {
+                geometry_msgs::msg::dds_::Twist_ twist;
+                double covariance[36];
+            };
+        };
+    };
+};

mujoco/idl/geometry_msgs/msg/Vector3.idl

@@ -0,0 +1,14 @@
+// geometry_msgs::msg::dds_::Vector3_ — see PROTOCOL.md.
+// Field layout verified against include/booster/idl/geometry_msgs/Vector3.h.
+
+module geometry_msgs {
+    module msg {
+        module dds_ {
+            struct Vector3_ {
+                double x;
+                double y;
+                double z;
+            };
+        };
+    };
+};

mujoco/idl/nav_msgs/msg/Odometry.idl

@@ -0,0 +1,19 @@
+// nav_msgs::msg::dds_::Odometry_ — see PROTOCOL.md.
+// Field layout verified against include/booster/idl/nav_msgs/Odometry.h.
+
+#include "Header.idl"
+#include "PoseWithCovariance.idl"
+#include "TwistWithCovariance.idl"
+
+module nav_msgs {
+    module msg {
+        module dds_ {
+            struct Odometry_ {
+                std_msgs::msg::dds_::Header_ header;
+                string child_frame_id;
+                geometry_msgs::msg::dds_::PoseWithCovariance_ pose;
+                geometry_msgs::msg::dds_::TwistWithCovariance_ twist;
+            };
+        };
+    };
+};

mujoco/idl/std_msgs/msg/Header.idl

@@ -0,0 +1,15 @@
+// std_msgs::msg::dds_::Header_ — see PROTOCOL.md.
+// Field layout verified against include/booster/idl/std_msgs/Header.h.
+
+#include "Time.idl"
+
+module std_msgs {
+    module msg {
+        module dds_ {
+            struct Header_ {
+                builtin_interfaces::msg::dds_::Time_ stamp;
+                string frame_id;
+            };
+        };
+    };
+};