Increase waist pitch PD gains to track offset

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 <noreply@anthropic.com>

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<float>(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;