From eab8af065f97135364c8c9ca9c600b0792e18b44 Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Sun, 22 Feb 2026 10:11:46 -0600 Subject: [PATCH] Auto-switch locomotion mode based on stick magnitude Walk mode (X): SLOW_WALK gait at 0.1-0.8 m/s (lower half of stick), WALK gait at 0.8-2.0 m/s (upper half). Run mode (Y): WALK gait at 0.8-2.5 m/s (lower half), RUN gait at 2.5-4.5 m/s (upper half). Ensures planner always receives in-distribution speeds matching the neural network's training clip ranges per locomotion mode. Co-Authored-By: Claude Opus 4.6 --- .../input_interface/gamepad_manager.hpp | 40 ++++++++++++++----- 1 file changed, 30 insertions(+), 10 deletions(-) diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp index 62fb01d..c502f35 100644 --- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp +++ b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp @@ -650,7 +650,9 @@ class GamepadManager : public InputInterface { double final_speed = planner_use_movement_speed_; double final_height = planner_use_height_; - // Proportional speed from stick magnitude (replaces binary IDLE gate) + // Proportional speed with auto locomotion mode switching + // X (WALK macro): SLOW_WALK(0.1-0.8) → WALK(0.8-2.0) based on stick magnitude + // Y (RUN macro): WALK(0.8-2.5) → RUN(2.5-4.5) based on stick magnitude if (planner_stick_magnitude_ < dead_zone_) { // Stick in dead zone — IDLE for walk/run, zero-movement for crawl/kneel if (planner_use_movement_mode_ == static_cast(LocomotionMode::WALK) || @@ -660,23 +662,41 @@ class GamepadManager : public InputInterface { final_speed = -1.0f; final_height = -1.0f; } else { - // Keep current mode, but zero out movement final_movement = {0.0f, 0.0f, 0.0f}; final_speed = 0.0f; final_height = 0.4f; } - } else if (planner_use_movement_mode_ == static_cast(LocomotionMode::WALK) || - planner_use_movement_mode_ == static_cast(LocomotionMode::RUN)) { - // Proportional speed from stick magnitude + } else if (planner_use_movement_mode_ == static_cast(LocomotionMode::WALK)) { + // WALK macro: SLOW_WALK (0.1-0.8) → WALK (0.8-2.0) double normalized = std::min((planner_stick_magnitude_ - dead_zone_) / (1.0 - dead_zone_), 1.0); - if (planner_use_movement_mode_ == static_cast(LocomotionMode::WALK)) { - final_speed = 0.3 + normalized * (1.5 - 0.3); // 0.3 to 1.5 m/s - } else { // RUN - final_speed = 2.0 + normalized * (4.5 - 2.0); // 2.0 to 4.5 m/s + if (normalized < 0.5) { + // Lower half: SLOW_WALK gait, speed 0.1 to 0.8 + final_mode = static_cast(LocomotionMode::SLOW_WALK); + double t = normalized / 0.5; + final_speed = 0.1 + t * (0.8 - 0.1); + } else { + // Upper half: WALK gait, speed 0.8 to 2.0 + final_mode = static_cast(LocomotionMode::WALK); + double t = (normalized - 0.5) / 0.5; + final_speed = 0.8 + t * (2.0 - 0.8); + } + } else if (planner_use_movement_mode_ == static_cast(LocomotionMode::RUN)) { + // RUN macro: WALK (0.8-2.5) → RUN (2.5-4.5) + double normalized = std::min((planner_stick_magnitude_ - dead_zone_) / (1.0 - dead_zone_), 1.0); + if (normalized < 0.5) { + // Lower half: WALK gait, speed 0.8 to 2.5 + final_mode = static_cast(LocomotionMode::WALK); + double t = normalized / 0.5; + final_speed = 0.8 + t * (2.5 - 0.8); + } else { + // Upper half: RUN gait, speed 2.5 to 4.5 + final_mode = static_cast(LocomotionMode::RUN); + double t = (normalized - 0.5) / 0.5; + final_speed = 2.5 + t * (4.5 - 2.5); } } else if (planner_use_movement_mode_ == static_cast(LocomotionMode::CRAWLING)) { double normalized = std::min((planner_stick_magnitude_ - dead_zone_) / (1.0 - dead_zone_), 1.0); - final_speed = 0.3 + normalized * (1.2 - 0.3); // 0.3 to 1.2 m/s + final_speed = 0.3 + normalized * (1.2 - 0.3); } // Emergency stop resets to idle