Add debug logging for raw NN waist action outputs

Log the decoder's raw action values for waist joints every 5s to
determine whether the policy actively commands waist motion or
outputs near-zero (suggesting locked-waist training).

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp

@@ -50,8 +50,8 @@
 inline std::atomic<double> g_imu_pitch_offset_deg{0.0};
 
 /// Runtime-adjustable waist pitch offset (degrees). Keys 7/8 to adjust via tmux.
-/// Default 0° — waist should stay neutral. Higher Kp/Kd simulate the missing
-/// physical waist clamp so the motors hold rigid during dynamic motion.
+/// SONIC is trained for 29-DOF free waist — all 3 waist DOFs are actively
+/// controlled by the policy NN. This offset is for experimentation only.
 inline std::atomic<double> g_waist_pitch_offset_deg{0.0};
 
 #if HAS_ROS2

gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp

@@ -2836,18 +2836,27 @@ class G1Deploy {
         motor_command_tmp.kd.at(i) = kds[i];
         motor_command_tmp.dq_target.at(i) = 0.0;
       }
-      // 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.
       // Optional waist 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<float>(waist_offset_rad);
       }
 
+      // Debug: log raw NN actions for waist joints every 5 seconds
+      static int waist_dbg_ctr = 0;
+      if (++waist_dbg_ctr >= 250) {
+        waist_dbg_ctr = 0;
+        double raw_yaw = floatarr[isaaclab_to_mujoco[12]];
+        double raw_roll = floatarr[isaaclab_to_mujoco[13]];
+        double raw_pitch = floatarr[isaaclab_to_mujoco[14]];
+        printf("[WAIST NN] raw(y/r/p)=%.4f/%.4f/%.4f  "
+               "cmd_deg=%.2f/%.2f/%.2f\n",
+               raw_yaw, raw_roll, raw_pitch,
+               motor_command_tmp.q_target.at(12) * 180.0/M_PI,
+               motor_command_tmp.q_target.at(13) * 180.0/M_PI,
+               motor_command_tmp.q_target.at(14) * 180.0/M_PI);
+      }
+
       motor_command_buffer_.SetData(motor_command_tmp);
       return true;
     }