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 bd6a5c1..1adb0c1 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 @@ -2878,6 +2878,23 @@ class G1Deploy { motor_command_tmp.q_target.at(10) += static_cast(ankle_offset_rad); // RightAnklePitch } + // Debug: log ankle command vs measured every 1s + static int ankle_dbg_ctr = 0; + if (++ankle_dbg_ctr >= 50) { + ankle_dbg_ctr = 0; + auto ls2 = low_state_buffer_.GetDataWithTime().data; + if (ls2) { + printf("[ANKLE_DBG] cmd_L=%.1f cmd_R=%.1f meas_L=%.1f meas_R=%.1f " + "kp=%.1f offset=%.1f deg\n", + motor_command_tmp.q_target.at(4) * 180.0/M_PI, + motor_command_tmp.q_target.at(10) * 180.0/M_PI, + ls2->motor_state()[4].q() * 180.0/M_PI, + ls2->motor_state()[10].q() * 180.0/M_PI, + motor_command_tmp.kp.at(4), + g_ankle_pitch_offset_deg.load()); + } + } + // Debug: log raw NN actions + measured positions for waist joints every 1s static int waist_dbg_ctr = 0; if (++waist_dbg_ctr >= 50) {