feat(go2):optimize the go2 Walk gait and increase the maximum command velocity.

motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/go2_walk.py

@@ -53,6 +53,10 @@
     "deadman_button": "L1",
 }
 
+# Conservative command slew limits used only by the MuJoCo sim2sim entry.
+# Units are (m/s^2, m/s^2, rad/s^2) for (vx, vy, yaw_rate).
+_SIM_COMMAND_ACCELERATION_LIMITS = np.array([0.8, 0.6, 1.0], dtype=np.float32)
+
 
 class Go2WalkTaskSpec(TaskSpec):
     """Self-contained control, command, gait and termination parameters."""
@@ -113,6 +117,12 @@ def __init__(self, spec: Go2WalkTaskSpec, robot: RobotSpec) -> None:
         self._termination_min_up_z = spec.termination_min_up_z
         self._previous_action: FloatArray = np.zeros(self.robot.joint_count, dtype=np.float32)
         self._phase = np.float32(0.0)
+        self._command_ramp_enabled = False
+        self._ramped_velocity = np.zeros(3, dtype=np.float32)
+
+    def enable_sim_command_ramp(self) -> None:
+        """Apply conservative command acceleration limits for sim2sim input."""
+        self._command_ramp_enabled = True
 
     @property
     def command_lower(self) -> FloatArray:
@@ -133,10 +143,21 @@ def reset(self, state: RobotState, context: ControlContext[PlanarVelocityCommand
         """Reset action and gait history; callers provide validated state and command."""
         self._previous_action.fill(0)
         self._phase = np.float32(0.0)
+        self._ramped_velocity.fill(0)
 
     def build_observation(self, state: RobotState, context: ControlContext[PlanarVelocityCommand]) -> FloatArray:
         """Consume runtime-validated state and PolicyController-validated input commands."""
-        velocity = context.command.values[0, :]
+        requested_velocity = context.command.values[0, :]
+        if self._command_ramp_enabled:
+            max_delta = _SIM_COMMAND_ACCELERATION_LIMITS * np.float32(context.dt_s)
+            self._ramped_velocity += np.clip(
+                requested_velocity - self._ramped_velocity,
+                -max_delta,
+                max_delta,
+            )
+            velocity = self._ramped_velocity.copy()
+        else:
+            velocity = requested_velocity
         phase = np.zeros(4, dtype=np.float32)
         if np.linalg.norm(velocity) < self._standing_threshold:
             self._phase = np.float32(0.0)
@@ -254,6 +275,8 @@ def prepare(
         spawn_pose: str | None = None,
     ) -> CommandBinding[PlanarVelocityCommand]:
         """Derive the crouch preparation from the simulation spawn pose and build the command source."""
+        if simulation:
+            self.policy_io.enable_sim_command_ramp()
         if simulation and spawn_pose == "lie_down":
             self.enable_simulation_preparation()
         command_lower, command_upper = self.command_bounds

motrix_envs/src/motrix_envs/locomotion/quadruped/anymalc.py

@@ -95,6 +95,10 @@ def __post_init__(self) -> None:
             front_right_contact=base.front_right_contact,
             rear_left_contact=base.rear_left_contact,
             rear_right_contact=base.rear_right_contact,
+            front_left_world_pos=base.front_left_world_pos,
+            front_right_world_pos=base.front_right_world_pos,
+            rear_left_world_pos=base.rear_left_world_pos,
+            rear_right_world_pos=base.rear_right_world_pos,
         )
 
 

motrix_envs/src/motrix_envs/locomotion/quadruped/cfg.py

@@ -15,6 +15,9 @@
 from motrix_env_core.config.scene import (
     ContactSensorCfg,
     ContactSensorReduce,
+    FrameObjectKind,
+    FrameSensorCfg,
+    FrameSensorType,
     NoiseTerrainGeneratorCfg,
     ProceduralHFieldAssetCfg,
     SceneSensorsCfg,
@@ -37,6 +40,20 @@ class QuadrupedTaskSensorsCfg(SceneSensorsCfg):
     front_right_contact: ContactSensorCfg = MISSING
     rear_left_contact: ContactSensorCfg = MISSING
     rear_right_contact: ContactSensorCfg = MISSING
+    front_left_world_pos: FrameSensorCfg = MISSING
+    front_right_world_pos: FrameSensorCfg = MISSING
+    rear_left_world_pos: FrameSensorCfg = MISSING
+    rear_right_world_pos: FrameSensorCfg = MISSING
+
+
+def _world_position_sensor(geom_name: str | None) -> FrameSensorCfg:
+    if geom_name is None:
+        raise ValueError("Quadruped task requires a contact geom name for every leg")
+    return FrameSensorCfg(
+        object_type=FrameObjectKind.geom,
+        object_name=geom_name,
+        sensor_type=FrameSensorType.framepos,
+    )
 
 
 @configclass
@@ -55,6 +72,10 @@ def __post_init__(self) -> None:
             front_right_contact=_contact_sensor(front_right),
             rear_left_contact=_contact_sensor(rear_left),
             rear_right_contact=_contact_sensor(rear_right),
+            front_left_world_pos=_world_position_sensor(front_left),
+            front_right_world_pos=_world_position_sensor(front_right),
+            rear_left_world_pos=_world_position_sensor(rear_left),
+            rear_right_world_pos=_world_position_sensor(rear_right),
         )
 
 
@@ -164,6 +185,8 @@ class VelocityCommandCfg:
     resampling_seconds_range: tuple[float, float] | None = None
 
     def validate(self) -> None:
+        if not np.isfinite(self.standing_probability) or not 0.0 <= self.standing_probability <= 1.0:
+            raise ValueError(f"standing_probability must be finite and inside [0, 1], got {self.standing_probability}")
         if self.resampling_seconds_range is not None:
             low, high = self.resampling_seconds_range
             if not np.isfinite(low) or not np.isfinite(high) or low <= 0.0 or low > high:
@@ -199,6 +222,61 @@ class RewardScales:
     contact: float = 0.24
     swing_feet_z: float = 2.0
     swing_contact: float = -1.0
+    flat_orientation: float = 0.0
+    stand_still: float = 0.0
+    feet_slide: float = 0.0
+    action_smoothness: float = 0.0
+
+
+@configclass
+class FootLateralRewardCfg:
+    """A body-frame lateral foot-placement reward for a named set of feet."""
+
+    name: str = ""
+    feet: tuple[str, ...] = ()
+    phase: str = "swing"
+    target_abs_y: float | None = None
+    min_abs_y: float | None = None
+    max_abs_y: float | None = None
+    width_tolerance: float = 0.02
+    width_scale: float = 0.0
+    lateral_speed_tolerance: float = 0.15
+    lateral_speed_scale: float = 0.0
+    speed_only_outside_band: bool = True
+
+    def validate(self) -> None:
+        if not self.name:
+            raise ValueError("foot lateral reward name must not be empty")
+        if not self.feet or len(set(self.feet)) != len(self.feet):
+            raise ValueError(f"foot lateral reward {self.name!r} must name unique feet")
+        if self.phase not in {"swing", "stance"}:
+            raise ValueError(f"foot lateral reward {self.name!r} phase must be 'swing' or 'stance'")
+        if not np.isfinite(self.width_tolerance) or self.width_tolerance <= 0.0:
+            raise ValueError(f"foot lateral reward {self.name!r} width_tolerance must be positive")
+        if not np.isfinite(self.lateral_speed_tolerance) or self.lateral_speed_tolerance <= 0.0:
+            raise ValueError(f"foot lateral reward {self.name!r} lateral_speed_tolerance must be positive")
+        if not np.isfinite(self.width_scale) or not np.isfinite(self.lateral_speed_scale):
+            raise ValueError(f"foot lateral reward {self.name!r} scales must be finite")
+
+        has_target = self.target_abs_y is not None
+        has_band = self.min_abs_y is not None or self.max_abs_y is not None
+        if has_target == has_band:
+            raise ValueError(
+                f"foot lateral reward {self.name!r} must define either target_abs_y or min_abs_y and max_abs_y"
+            )
+        if has_target:
+            if not np.isfinite(self.target_abs_y) or self.target_abs_y < 0.0:
+                raise ValueError(f"foot lateral reward {self.name!r} target_abs_y must be non-negative")
+            return
+        if self.min_abs_y is None or self.max_abs_y is None:
+            raise ValueError(f"foot lateral reward {self.name!r} band requires min_abs_y and max_abs_y")
+        if (
+            not np.isfinite(self.min_abs_y)
+            or not np.isfinite(self.max_abs_y)
+            or self.min_abs_y < 0.0
+            or self.min_abs_y > self.max_abs_y
+        ):
+            raise ValueError(f"foot lateral reward {self.name!r} must define a non-negative ordered band")
 
 
 @configclass
@@ -209,6 +287,16 @@ class RewardConfig:
     target_foot_height: float = 0.1
     swing_feet_height_sigma: float = 0.05
     base_height_target: float = 0.3
+    stand_still_threshold: float = 0.05
+    foot_lateral_terms: tuple[FootLateralRewardCfg, ...] = ()
+
+    def validate(self) -> None:
+        names: set[str] = set()
+        for term in self.foot_lateral_terms:
+            term.validate()
+            if term.name in names:
+                raise ValueError(f"duplicate foot lateral reward name {term.name!r}")
+            names.add(term.name)
 
 
 @configclass
@@ -238,6 +326,7 @@ def validate(self) -> None:
         super().validate()
         self.randomization.validate()
         self.commands.velocity.validate()
+        self.reward_config.validate()
         if not np.isfinite(self.termination_min_up_z) or not -1.0 <= self.termination_min_up_z <= 1.0:
             raise ValueError("termination_min_up_z must be finite and inside [-1, 1]")
         if (
@@ -263,6 +352,7 @@ def for_play(self) -> "QuadrupedWalkEnvCfg":
 __all__ = [
     "Commands",
     "ControlConfig",
+    "FootLateralRewardCfg",
     "NoiseConfig",
     "QuadrupedWalkRandomizationCfg",
     "QuadrupedSceneCfg",

motrix_envs/src/motrix_envs/locomotion/quadruped/go2.py

@@ -14,6 +14,7 @@
 from motrix_envs.config.scene import StandardSceneObjsCfg
 from motrix_envs.locomotion.quadruped.cfg import (
     Commands,
+    FootLateralRewardCfg,
     QuadrupedSceneCfg,
     QuadrupedWalkEnvCfg,
     QuadrupedWalkRandomizationCfg,
@@ -68,11 +69,25 @@ class Go2WalkDirectEnvCfg(QuadrupedWalkEnvCfg):
 
     reward_config: RewardConfig = RewardConfig(
         scales=RewardScales(
-            similar_to_default=-1,
+            tracking_lin_vel=2.0,
+            flat_orientation=-2.5,
+            stand_still=-1,
+            feet_slide=-0.5,
+            action_smoothness=-0.01,
         ),
         tracking_ang_vel_sigma=0.05,
         target_foot_height=0.05,
         base_height_target=0.35,
+        foot_lateral_terms=(
+            FootLateralRewardCfg(
+                name="rear_swing_width",
+                feet=("rear_left", "rear_right"),
+                phase="swing",
+                target_abs_y=0.145,
+                width_tolerance=0.02,
+                width_scale=-0.05,
+            ),
+        ),
     )
 
     def for_play(self) -> "Go2WalkDirectEnvCfg":
@@ -109,9 +124,32 @@ class Go2WalkRoughDirectEnvCfg(Go2WalkDirectEnvCfg):
         ),
     )
     reward_config: RewardConfig = RewardConfig(
+        scales=RewardScales(
+            tracking_lin_vel=2.0,
+            stand_still=-1.0,
+        ),
         target_foot_height=0.1,
+        # A weak rear-foot guardrail: preserve terrain-dependent foothold
+        # selection while discouraging pathological inward/outward swings.
+        foot_lateral_terms=(
+            FootLateralRewardCfg(
+                name="rear_swing_width_rough",
+                feet=("rear_left", "rear_right"),
+                phase="swing",
+                min_abs_y=0.10,
+                max_abs_y=0.20,
+                width_tolerance=0.04,
+                width_scale=-0.005,
+            ),
+        ),
     )
 
+    def for_play(self) -> "Go2WalkRoughDirectEnvCfg":
+        """Return a deterministic rough-terrain evaluation config."""
+
+        cfg = QuadrupedWalkEnvCfg.for_play(self)
+        return replace(cfg, noise_config=replace(cfg.noise_config, level=0.0))
+
 
 registry.env("go2-walk-flat")(QuadrupedWalkTask)
 registry.env("go2-walk-rough")(QuadrupedWalkTask)