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 78f0641..8fb00a7 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 @@ -67,6 +67,10 @@ inline std::atomic g_knee_offset_deg{0.0}; /// Applied post-NN to motor commands only (NN is blind to this adjustment). inline std::atomic g_hip_pitch_offset_deg{3.0}; +/// Runtime-adjustable ankle pitch offset (degrees). Keys q/w to adjust via tmux. +/// Positive = more dorsiflexion correction (toes up). Applied to both ankles equally. +inline std::atomic g_ankle_pitch_offset_deg{0.0}; + /// Neutral hold mode: bypass NN, command default_angles directly. Key 'n' to toggle. /// Used for visual joint inspection (diagnosing hardware offsets). inline std::atomic g_neutral_hold_mode{false}; @@ -202,6 +206,20 @@ class GamepadManager : public InputInterface { is_manager_key = true; break; } + case 'q': { + double val = g_ankle_pitch_offset_deg.load() - 1.0; + g_ankle_pitch_offset_deg.store(val); + std::cout << "[ANKLE] Pitch offset: " << val << " deg (+ = more dorsiflexion)" << std::endl; + is_manager_key = true; + break; + } + case 'w': { + double val = g_ankle_pitch_offset_deg.load() + 1.0; + g_ankle_pitch_offset_deg.store(val); + std::cout << "[ANKLE] Pitch offset: " << val << " deg (+ = more dorsiflexion)" << std::endl; + is_manager_key = true; + break; + } case 'n': case 'N': { bool cur = g_neutral_hold_mode.load(); 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 37027ce..c877daa 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 @@ -2867,6 +2867,13 @@ class G1Deploy { motor_command_tmp.q_target.at(0) -= static_cast(hip_offset_rad); // LeftHipPitch (more negative = more flexion) motor_command_tmp.q_target.at(6) -= static_cast(hip_offset_rad); // RightHipPitch (more negative = more flexion) } + // Ankle pitch offset (keys q/w). Positive = more dorsiflexion (toes up). + // Corrects for ankle zero offset on new pelvis hardware revision. + double ankle_offset_rad = g_ankle_pitch_offset_deg.load() * M_PI / 180.0; + if (ankle_offset_rad != 0.0) { + motor_command_tmp.q_target.at(4) += static_cast(ankle_offset_rad); // LeftAnklePitch + motor_command_tmp.q_target.at(10) += static_cast(ankle_offset_rad); // RightAnklePitch + } // Debug: log raw NN actions + measured positions for waist joints every 1s static int waist_dbg_ctr = 0;