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; }