From d4d5ec5200d7023310ad9ac62e19754140b594af Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Mon, 23 Feb 2026 18:59:30 -0600 Subject: [PATCH] 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 --- .../input_interface/gamepad_manager.hpp | 4 ++-- .../src/g1_deploy_onnx_ref.cpp | 21 +++++++++++++------ 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp index 645fc5a..4d1a231 100644 --- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp +++ b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp @@ -50,8 +50,8 @@ inline std::atomic 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 g_waist_pitch_offset_deg{0.0}; #if HAS_ROS2 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 32e2d15..237ac75 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 @@ -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(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; }