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 c3d8999..c073003 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 @@ -47,7 +47,7 @@ #include "../localmotion_kplanner.hpp" // For LocomotionMode enum /// Runtime-adjustable IMU pitch offset (degrees). Keys 9/0 to adjust via tmux. -inline std::atomic g_imu_pitch_offset_deg{-2.0}; +inline std::atomic g_imu_pitch_offset_deg{0.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 @@ -60,7 +60,12 @@ inline std::atomic g_standing_height_m{0.72}; /// Runtime-adjustable knee offset (degrees). Keys 3/4 to adjust via tmux. /// Positive = more bent knees = lower stance. Applied to both knees equally. -inline std::atomic g_knee_offset_deg{5.0}; +inline std::atomic g_knee_offset_deg{0.0}; + +/// Runtime-adjustable hip pitch offset (degrees). Keys 1/2 to adjust via tmux. +/// Positive = more hip flexion = legs forward = counteracts backward lean. +/// Applied post-NN to motor commands only (NN is blind to this adjustment). +inline std::atomic g_hip_pitch_offset_deg{3.0}; #if HAS_ROS2 #include "ros2_input_handler.hpp" @@ -193,6 +198,20 @@ class GamepadManager : public InputInterface { is_manager_key = true; break; } + case '1': { + double val = g_hip_pitch_offset_deg.load() - 1.0; + g_hip_pitch_offset_deg.store(val); + std::cout << "[HIP] Pitch offset: " << val << " deg (+ = more flexion)" << std::endl; + is_manager_key = true; + break; + } + case '2': { + double val = g_hip_pitch_offset_deg.load() + 1.0; + g_hip_pitch_offset_deg.store(val); + std::cout << "[HIP] Pitch offset: " << val << " deg (+ = more flexion)" << 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 4740329..54451ff 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 @@ -2841,12 +2841,20 @@ class G1Deploy { if (waist_offset_rad != 0.0) { motor_command_tmp.q_target.at(14) += static_cast(waist_offset_rad); } - // Optional knee offset (keys 3/4). Default +5°. Positive = more bent. + // Optional knee offset (keys 3/4). Positive = more bent. double knee_offset_rad = g_knee_offset_deg.load() * M_PI / 180.0; if (knee_offset_rad != 0.0) { motor_command_tmp.q_target.at(3) += static_cast(knee_offset_rad); // LeftKnee motor_command_tmp.q_target.at(9) += static_cast(knee_offset_rad); // RightKnee } + // Hip pitch offset (keys 1/2). Default +3°. Positive = more flexion. + // Corrects for pelvis hardware revision where hip zero is shifted. + // NN is blind to this — applied post-policy to motor commands only. + double hip_offset_rad = g_hip_pitch_offset_deg.load() * M_PI / 180.0; + if (hip_offset_rad != 0.0) { + motor_command_tmp.q_target.at(0) -= static_cast(hip_offset_rad); // LeftHipPitch (more negative = more flexion) + motor_command_tmp.q_target.at(6) -= static_cast(hip_offset_rad); // RightHipPitch (more negative = more flexion) + } // Debug: log raw NN actions + measured positions for waist joints every 1s static int waist_dbg_ctr = 0;