From b1c75936e63807700d79c9fffa07a6e9ea4e9bcf Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Mon, 23 Feb 2026 17:22:26 -0600 Subject: [PATCH] Increase waist pitch PD gains to track offset MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Default Kp=28.5 generates only ~4 Nm at 8° error, insufficient to overcome gravity on the upper body. Bump to Kp=100, Kd=5 (matching hip-level stiffness) so the waist motor actually drives to the commanded position. Motor effort limit is 25 Nm, so 14 Nm command at 8° error is well within range. Co-Authored-By: Claude Opus 4.6 --- .../src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 6659ac4..3b6430f 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 @@ -2840,6 +2840,10 @@ class G1Deploy { // 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. + motor_command_tmp.kp.at(14) = 100.0f; + motor_command_tmp.kd.at(14) = 5.0f; motor_command_buffer_.SetData(motor_command_tmp); return true;