From 7ae273f35b8f2ce19944ae546f66dc9d363e2fdf Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Mon, 23 Feb 2026 17:31:36 -0600 Subject: [PATCH] Add temporary waist pitch debug logging Log cmd_q, hw_q, kp for motor[14] every 2s to diagnose why the waist motor isn't tracking the position command. Co-Authored-By: Claude Opus 4.6 --- .../g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp | 13 +++++++++++++ 1 file changed, 13 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 3b6430f..5ed68e3 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 @@ -2845,6 +2845,19 @@ class G1Deploy { motor_command_tmp.kp.at(14) = 100.0f; motor_command_tmp.kd.at(14) = 5.0f; + // Temporary: log waist_pitch motor command every ~2s (100 ticks at 50Hz) + static int waist_log_cnt = 0; + if (++waist_log_cnt >= 100) { + waist_log_cnt = 0; + auto ls = low_state_buffer_.GetData(); + float hw_q = ls ? ls->motor_state()[14].q() : -999.0f; + std::cout << "[WAIST_DBG] cmd_q=" << motor_command_tmp.q_target.at(14) + << " hw_q=" << hw_q + << " kp=" << motor_command_tmp.kp.at(14) + << " offset=" << g_waist_pitch_offset_deg.load() << "deg" + << std::endl; + } + motor_command_buffer_.SetData(motor_command_tmp); return true; }