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 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 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(waist_offset_rad); + motor_command_buffer_.SetData(motor_command_tmp); return true; }