From 6d3f2f01545ca9eb498c15486a3980858fc4f5a5 Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Mon, 23 Feb 2026 13:54:24 -0600 Subject: [PATCH] Restore 0.2 m/s minimum speed (planner trained range), 2s ramp Planner NN was trained with SLOW_WALK range [0.2, 0.8] m/s per NVIDIA docs. Sending 0.0 m/s is out-of-distribution. Restored 0.2 minimum, ramp now 0.2->0.8 over ~2s at 100Hz input loop. Co-Authored-By: Claude Opus 4.6 --- .../include/input_interface/gamepad_manager.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 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 0323d1d..8b5f135 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 @@ -693,7 +693,7 @@ class GamepadManager : public InputInterface { double final_speed = planner_use_movement_speed_; double final_height = planner_use_height_; - // LOCKED TO SLOW_WALK for testing — speed 0.0-0.8 m/s + // LOCKED TO SLOW_WALK for testing — speed 0.2-0.8 m/s (planner trained range) // Proportional to stick magnitude with acceleration rate limiting. if (planner_stick_magnitude_ < dead_zone_) { // Stick in dead zone — IDLE @@ -701,25 +701,25 @@ class GamepadManager : public InputInterface { final_movement = {0.0f, 0.0f, 0.0f}; final_speed = -1.0f; final_height = -1.0f; - smoothed_speed_ = 0.0; // Reset ramp when idle + smoothed_speed_ = 0.2; // Reset to planner minimum so ramp starts there } else if (planner_use_movement_mode_ == static_cast(LocomotionMode::CRAWLING) || planner_use_movement_mode_ == static_cast(LocomotionMode::IDEL_KNEEL_TWO_LEGS)) { // Keep crawl/kneel modes as-is double normalized = std::min((planner_stick_magnitude_ - dead_zone_) / (1.0 - dead_zone_), 1.0); final_speed = 0.3 + normalized * (1.2 - 0.3); } else { - // Everything else → SLOW_WALK, 0.0-0.8 m/s + // Everything else → SLOW_WALK, 0.2-0.8 m/s (planner trained range) double normalized = std::min((planner_stick_magnitude_ - dead_zone_) / (1.0 - dead_zone_), 1.0); - double target_speed = normalized * 0.8; // 0.0 at dead zone edge, 0.8 at full stick + double target_speed = 0.2 + normalized * (0.8 - 0.2); // 0.2 min, 0.8 max // Rate-limit acceleration only (decel is instant) - constexpr double max_accel_per_frame = 0.002; // 4s ramp 0→0.8 (100Hz input loop) + constexpr double max_accel_per_frame = 0.003; // ~2s ramp 0.2→0.8 at 100Hz if (target_speed > smoothed_speed_) { smoothed_speed_ = std::min(target_speed, smoothed_speed_ + max_accel_per_frame); } else { smoothed_speed_ = target_speed; // Instant decel } final_mode = static_cast(LocomotionMode::SLOW_WALK); - final_speed = smoothed_speed_; + final_speed = std::max(smoothed_speed_, 0.2); // Never below planner minimum } // Emergency stop resets to idle