From 90e897ab29c357373d3abf0740b8fe7bf4d8997d Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Mon, 23 Feb 2026 17:40:57 -0600 Subject: [PATCH] Stiffen waist roll/pitch to simulate missing physical clamp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Robot lacks the waist clamp expected by the locked-waist policy (mode_machine=6). Default Kp=28.5 is intentionally low to protect gears against a physical lock, but without the clamp the waist flops under dynamic loads (walking/running), causing backward tilt. Bump waist_roll and waist_pitch Kp to 100, Kd to 5 so the motors hold the waist rigid. Remove the 8-degree offset — the waist default position is correct, it just couldn't hold it. Keep offset keys 7/8 available for tuning if needed. Remove temporary debug logging. Co-Authored-By: Claude Opus 4.6 --- .../input_interface/gamepad_manager.hpp | 5 ++-- .../src/g1_deploy_onnx_ref.cpp | 30 ++++++++----------- 2 files changed, 15 insertions(+), 20 deletions(-) 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);