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 2a47d7d..c3d8999 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 @@ -58,6 +58,10 @@ inline std::atomic g_waist_pitch_offset_deg{0.0}; /// Lowered from planner default 0.789m to add knee bend. inline std::atomic g_standing_height_m{0.72}; +/// Runtime-adjustable knee offset (degrees). Keys 3/4 to adjust via tmux. +/// Positive = more bent knees = lower stance. Applied to both knees equally. +inline std::atomic g_knee_offset_deg{5.0}; + #if HAS_ROS2 #include "ros2_input_handler.hpp" #endif @@ -175,6 +179,20 @@ class GamepadManager : public InputInterface { is_manager_key = true; break; } + case '3': { + double val = g_knee_offset_deg.load() - 1.0; + g_knee_offset_deg.store(val); + std::cout << "[KNEE] Offset: " << val << " deg" << std::endl; + is_manager_key = true; + break; + } + case '4': { + double val = g_knee_offset_deg.load() + 1.0; + g_knee_offset_deg.store(val); + std::cout << "[KNEE] Offset: " << val << " deg" << std::endl; + is_manager_key = true; + break; + } } if (!is_manager_key && current_) { 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 1144577..4740329 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 @@ -2841,6 +2841,12 @@ class G1Deploy { if (waist_offset_rad != 0.0) { motor_command_tmp.q_target.at(14) += static_cast(waist_offset_rad); } + // Optional knee offset (keys 3/4). Default +5°. Positive = more bent. + double knee_offset_rad = g_knee_offset_deg.load() * M_PI / 180.0; + if (knee_offset_rad != 0.0) { + motor_command_tmp.q_target.at(3) += static_cast(knee_offset_rad); // LeftKnee + motor_command_tmp.q_target.at(9) += static_cast(knee_offset_rad); // RightKnee + } // Debug: log raw NN actions + measured positions for waist joints every 1s static int waist_dbg_ctr = 0;