Lower standing height to 0.72m and default IMU offset to -2 deg

Adds knee bend by passing explicit height (0.72m vs 0.789m default) to the
planner for all standing locomotion modes. Runtime-adjustable via keys 5/6
(±1cm). IMU pitch offset default changed from 0 to -2 deg to correct the
forward-tilt bias observed on the physical robot.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp

@@ -47,13 +47,17 @@
 #include "../localmotion_kplanner.hpp"  // For LocomotionMode enum
 
 /// Runtime-adjustable IMU pitch offset (degrees). Keys 9/0 to adjust via tmux.
-inline std::atomic<double> g_imu_pitch_offset_deg{0.0};
+inline std::atomic<double> g_imu_pitch_offset_deg{-2.0};
 
 /// Runtime-adjustable waist pitch offset (degrees). Keys 7/8 to adjust via tmux.
 /// SONIC is trained for 29-DOF free waist — all 3 waist DOFs are actively
 /// controlled by the policy NN. This offset is for experimentation only.
 inline std::atomic<double> g_waist_pitch_offset_deg{0.0};
 
+/// Runtime-adjustable standing height (meters). Keys 5/6 to adjust via tmux.
+/// Lowered from planner default 0.789m to add knee bend.
+inline std::atomic<double> g_standing_height_m{0.72};
+
 #if HAS_ROS2
 #include "ros2_input_handler.hpp"
 #endif
@@ -157,6 +161,20 @@ class GamepadManager : public InputInterface {
             is_manager_key = true;
             break;
           }
+          case '5': {
+            double val = std::max(g_standing_height_m.load() - 0.01, 0.55);
+            g_standing_height_m.store(val);
+            std::cout << "[HEIGHT] Standing height: " << val << " m" << std::endl;
+            is_manager_key = true;
+            break;
+          }
+          case '6': {
+            double val = std::min(g_standing_height_m.load() + 0.01, 0.80);
+            g_standing_height_m.store(val);
+            std::cout << "[HEIGHT] Standing height: " << val << " m" << std::endl;
+            is_manager_key = true;
+            break;
+          }
         }
 
         if (!is_manager_key && current_) {
@@ -767,11 +785,17 @@ class GamepadManager : public InputInterface {
           final_height = 0.4f;
         }
 
+        // Apply standing height override for standing modes (keys 5/6 to adjust)
+        if (final_height < 0.0 && is_standing_motion_mode(static_cast<LocomotionMode>(final_mode))) {
+          final_height = g_standing_height_m.load();
+        }
+
         MovementState mode_state(final_mode, final_movement, final_facing_direction, final_speed, final_height);
         if constexpr (DEBUG_LOGGING) {
           if (final_mode != static_cast<int>(LocomotionMode::IDLE)) {
             std::cout << "[GamepadManager] -> Planner: mode=" << final_mode
-                      << " speed=" << final_speed << " mag=" << planner_stick_magnitude_ << std::endl;
+                      << " speed=" << final_speed << " height=" << final_height
+                      << " mag=" << planner_stick_magnitude_ << std::endl;
           }
         }
         movement_state_buffer.SetData(mode_state);