Add measured waist positions to debug output

Now logs: raw NN output, motor command (deg), and measured position (deg)
for all 3 waist joints every second.

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

@@ -2842,19 +2842,28 @@ class G1Deploy {
         motor_command_tmp.q_target.at(14) += static_cast<float>(waist_offset_rad);
       }
 
-      // Debug: log raw NN actions for waist joints every 5 seconds
+      // Debug: log raw NN actions + measured positions for waist joints every 1s
       static int waist_dbg_ctr = 0;
       if (++waist_dbg_ctr >= 50) {
         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",
+        // Read measured positions from low state
+        auto ls = low_state_buffer_.GetDataWithTime().data;
+        double meas_yaw = ls ? ls->motor_state()[12].q() : 0.0;
+        double meas_roll = ls ? ls->motor_state()[13].q() : 0.0;
+        double meas_pitch = ls ? ls->motor_state()[14].q() : 0.0;
+        printf("[WAIST] raw_nn=%.3f/%.3f/%.3f  "
+               "cmd=%.1f/%.1f/%.1f  "
+               "meas=%.1f/%.1f/%.1f deg\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_tmp.q_target.at(14) * 180.0/M_PI,
+               meas_yaw * 180.0/M_PI,
+               meas_roll * 180.0/M_PI,
+               meas_pitch * 180.0/M_PI);
       }
 
       motor_command_buffer_.SetData(motor_command_tmp);