Add torque, mode, lost flags to waist debug output

Diagnose whether waist motors are actually receiving commands.

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

@@ -2849,21 +2849,26 @@ class G1Deploy {
         double raw_yaw = floatarr[isaaclab_to_mujoco[12]];
         double raw_roll = floatarr[isaaclab_to_mujoco[13]];
         double raw_pitch = floatarr[isaaclab_to_mujoco[14]];
-        // Read measured positions from low state
+        // Read measured positions + torque 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,
-               meas_yaw * 180.0/M_PI,
-               meas_roll * 180.0/M_PI,
-               meas_pitch * 180.0/M_PI);
+        if (ls) {
+          auto& ms = ls->motor_state();
+          printf("[WAIST] raw_nn=%.3f/%.3f/%.3f  "
+                 "cmd=%.1f/%.1f/%.1f  "
+                 "meas=%.1f/%.1f/%.1f  "
+                 "tau=%.2f/%.2f/%.2f Nm  "
+                 "mode=%d/%d/%d  lost=%d/%d/%d\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,
+                 ms[12].q() * 180.0/M_PI,
+                 ms[13].q() * 180.0/M_PI,
+                 ms[14].q() * 180.0/M_PI,
+                 ms[12].tau_est(), ms[13].tau_est(), ms[14].tau_est(),
+                 ms[12].mode(), ms[13].mode(), ms[14].mode(),
+                 ms[12].lost(), ms[13].lost(), ms[14].lost());
+        }
       }
 
       motor_command_buffer_.SetData(motor_command_tmp);