fix: microduck-walk-flat standing regression — close-feet yaw sign, gait-phase pinning, penalty_scale metric

motrix_envs/src/motrix_envs/locomotion/humanoid/walk_manager_mdp/command.py

@@ -24,17 +24,18 @@
 
 @njit(inline="always")
 def _lane_phase(cmd, phase_out, sin_cos_out, phase_offset, steps, phase_dt) -> None:
-    """Refresh one lane's phase clock, pinning standing commands to ``pi``."""
-    phi = (steps * phase_dt + phase_offset[0] + math.pi) % (2.0 * math.pi) - math.pi
+    """Refresh both lanes' phase clocks, pinning standing commands to ``pi``."""
+    tau = 2.0 * math.pi
+    for i in range(2):
+        phase_out[i] = (steps * phase_dt + phase_offset[i] + math.pi) % tau - math.pi
     speed = math.sqrt(cmd[0] * cmd[0] + cmd[1] * cmd[1])
     if speed < 0.01 and abs(cmd[2]) < 0.01:
-        phi = math.pi
-    phase_out[0] = phi
-    phase_out[1] = (steps * phase_dt + phase_offset[1] + math.pi) % (2.0 * math.pi) - math.pi
-    sin_cos_out[0] = math.sin(phase_out[0])
-    sin_cos_out[1] = math.sin(phase_out[1])
-    sin_cos_out[2] = math.cos(phase_out[0])
-    sin_cos_out[3] = math.cos(phase_out[1])
+        # Standing commands pin both lanes, matching the direct env's
+        # ``phase[stand] = pi``: the gait clock must freeze for both feet.
+        phase_out[:] = math.pi
+    for i in range(2):
+        sin_cos_out[i] = math.sin(phase_out[i])
+        sin_cos_out[2 + i] = math.cos(phase_out[i])
 
 
 @njit(inline="always")
@@ -111,6 +112,7 @@ def reset(self, ctx: ResetContext) -> None:
         truncated), so the EMA sample matches the direct env's
         ``done = terminated | truncated``.
         """
+        ctx.metrics["penalty_scale"] = float(self.penalty_scale[0])
         if not self.curriculum_enabled or ctx.env_ids.size == 0:
             return
         ep_len = self.steps[ctx.env_ids, 0].astype(np.float64) + 1.0

motrix_envs/src/motrix_envs/locomotion/humanoid/walk_manager_mdp/rewards.py

@@ -182,9 +182,14 @@ def penalty_close_feet_xy_reward(
 ) -> float:
     left = foot_pos[0]
     right = foot_pos[1]
-    fx, fy, _ = rotate_inverse_components(base_quat, (1.0, 0.0, 0.0))
-    yaw = math.atan2(fy, fx)
-    distance = abs(math.cos(yaw) * (left[1] - right[1]) - math.sin(yaw) * (left[0] - right[0]))
+    # Lateral foot separation in the base frame: rotate the world-frame foot
+    # delta's XY components (z forced to zero — vertical separation is
+    # intentionally ignored) back into the base frame and take its y
+    # magnitude. Rotating the world x-axis instead (as a yaw proxy) flips the
+    # yaw sign under the inverse rotation and misjudges staggered feet as
+    # crossing.
+    _, lateral, _ = rotate_inverse_components(base_quat, (left[0] - right[0], left[1] - right[1], 0.0))
+    distance = abs(lateral)
     walk: WalkCommand = ctx.commands["walk"]
     if distance < close_feet_threshold:
         return 1.0 * walk.penalty_scale[0]