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 b0773b5..645fc5a 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 @@ -50,8 +50,9 @@ 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}; +/// Default 0° — waist should stay neutral. Higher Kp/Kd simulate the missing +/// physical waist clamp so the motors hold rigid during dynamic motion. +inline std::atomic g_waist_pitch_offset_deg{0.0}; #if HAS_ROS2 #include "ros2_input_handler.hpp" 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 bda6a71..9cc4083 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,26 +2836,20 @@ 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); - // Stiffen waist pitch to actually track the offset (default Kp=28.5 too - // weak to overcome gravity on upper body). Match hip-level stiffness. + // Stiffen waist roll/pitch to simulate the missing physical waist clamp. + // Policy was trained for locked-waist variant (mode_machine=6) where + // roll/pitch are fixed joints. Default Kp=28.5 is intentionally low to + // avoid stripping gears against a clamp, but without the clamp the waist + // flops under dynamic loads. Higher gains keep it rigid. + // MuJoCo 13 = waist_roll, 14 = waist_pitch. + motor_command_tmp.kp.at(13) = 100.0f; + motor_command_tmp.kd.at(13) = 5.0f; motor_command_tmp.kp.at(14) = 100.0f; motor_command_tmp.kd.at(14) = 5.0f; - - // Temporary: log waist_pitch motor command every ~2s (100 ticks at 50Hz) - static int waist_log_cnt = 0; - if (++waist_log_cnt >= 100) { - waist_log_cnt = 0; - const auto ls_dbg = low_state_buffer_.GetDataWithTime().data; - float hw_q = ls_dbg ? ls_dbg->motor_state()[14].q() : -999.0f; - std::cout << "[WAIST_DBG] cmd_q=" << motor_command_tmp.q_target.at(14) - << " hw_q=" << hw_q - << " kp=" << motor_command_tmp.kp.at(14) - << " offset=" << g_waist_pitch_offset_deg.load() << "deg" - << std::endl; + // Optional pitch offset (keys 7/8). Default 0°. + double waist_offset_rad = g_waist_pitch_offset_deg.load() * M_PI / 180.0; + if (waist_offset_rad != 0.0) { + motor_command_tmp.q_target.at(14) += static_cast(waist_offset_rad); } motor_command_buffer_.SetData(motor_command_tmp);