From 5fd56591225e2ea4eeb39500a672038ad9047eea Mon Sep 17 00:00:00 2001
From: Joe DiPrima <epilectrik@gmail.com>
Date: Mon, 23 Feb 2026 17:07:35 -0600
Subject: [PATCH] Add waist pitch offset to compensate for locked-waist
 training
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

The SONIC policy was likely trained on mode_machine=6 (locked waist variant)
where waist_roll and waist_pitch are fixed joints. On our mode_machine=5
robot the waist is free but the NN outputs near-zero for it. This adds a
configurable waist_pitch offset (default +8°) applied directly to the motor
command. Keys 7/8 adjust ±1° at runtime via tmux.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
---
 .../input_interface/gamepad_manager.hpp        | 18 ++++++++++++++++++
 .../src/g1_deploy_onnx_ref.cpp                 |  5 +++++
 2 files changed, 23 insertions(+)

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 8b5f135..b0773b5 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
@@ -49,6 +49,10 @@
 /// Runtime-adjustable IMU pitch offset (degrees). Keys 9/0 to adjust via tmux.
 inline std::atomic<double> g_imu_pitch_offset_deg{0.0};
 
+/// Runtime-adjustable waist pitch offset (degrees). Keys 7/8 to adjust via tmux.
+/// Positive = forward lean. Default 8° to compensate for locked-waist training.
+inline std::atomic<double> g_waist_pitch_offset_deg{8.0};
+
 #if HAS_ROS2
 #include "ros2_input_handler.hpp"
 #endif
@@ -138,6 +142,20 @@ class GamepadManager : public InputInterface {
             is_manager_key = true;
             break;
           }
+          case '7': {
+            double val = g_waist_pitch_offset_deg.load() - 1.0;
+            g_waist_pitch_offset_deg.store(val);
+            std::cout << "[WAIST] Pitch offset: " << val << " deg" << std::endl;
+            is_manager_key = true;
+            break;
+          }
+          case '8': {
+            double val = g_waist_pitch_offset_deg.load() + 1.0;
+            g_waist_pitch_offset_deg.store(val);
+            std::cout << "[WAIST] Pitch offset: " << val << " deg" << std::endl;
+            is_manager_key = true;
+            break;
+          }
         }
 
         if (!is_manager_key && current_) {
diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp
index f20aeb7..6659ac4 100644
--- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp
+++ b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp
@@ -2836,6 +2836,11 @@ class G1Deploy {
         motor_command_tmp.kd.at(i) = kds[i];
         motor_command_tmp.dq_target.at(i) = 0.0;
       }
+      // Waist pitch offset: compensate for policy trained with locked waist.
+      // MuJoCo index 14 = waist_pitch_joint. Positive = forward lean.
+      double waist_offset_rad = g_waist_pitch_offset_deg.load() * M_PI / 180.0;
+      motor_command_tmp.q_target.at(14) += static_cast<float>(waist_offset_rad);
+
       motor_command_buffer_.SetData(motor_command_tmp);
       return true;
     }