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 c073003..78f0641 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}; +/// 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}; + #if HAS_ROS2 #include "ros2_input_handler.hpp" #endif @@ -198,6 +202,14 @@ class GamepadManager : public InputInterface { is_manager_key = true; break; } + case 'n': + case 'N': { + bool cur = g_neutral_hold_mode.load(); + g_neutral_hold_mode.store(!cur); + std::cout << "[NEUTRAL] Hold mode: " << (!cur ? "ON (default angles)" : "OFF (NN active)") << std::endl; + is_manager_key = true; + break; + } case '1': { double val = g_hip_pitch_offset_deg.load() - 1.0; g_hip_pitch_offset_deg.store(val); 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 54451ff..37027ce 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 @@ -2827,14 +2827,26 @@ class G1Deploy { float* floatarr = action_buffer.data(); MotorCommand motor_command_tmp; - for (int i = 0; i < G1_NUM_MOTOR; i++) { - const double action_value = static_cast(floatarr[isaaclab_to_mujoco[i]]) * g1_action_scale[i]; - last_action[i] = static_cast(floatarr[i]); - motor_command_tmp.q_target.at(i) = static_cast(default_angles[i] + action_value); - motor_command_tmp.tau_ff.at(i) = 0.0; - motor_command_tmp.kp.at(i) = kps[i]; - motor_command_tmp.kd.at(i) = kds[i]; - motor_command_tmp.dq_target.at(i) = 0.0; + + // Neutral hold mode: bypass NN, command default angles directly (key 'n') + if (g_neutral_hold_mode.load()) { + for (int i = 0; i < G1_NUM_MOTOR; i++) { + motor_command_tmp.q_target.at(i) = static_cast(default_angles[i]); + motor_command_tmp.tau_ff.at(i) = 0.0; + motor_command_tmp.kp.at(i) = kps[i]; + motor_command_tmp.kd.at(i) = kds[i]; + motor_command_tmp.dq_target.at(i) = 0.0; + } + } else { + for (int i = 0; i < G1_NUM_MOTOR; i++) { + const double action_value = static_cast(floatarr[isaaclab_to_mujoco[i]]) * g1_action_scale[i]; + last_action[i] = static_cast(floatarr[i]); + motor_command_tmp.q_target.at(i) = static_cast(default_angles[i] + action_value); + motor_command_tmp.tau_ff.at(i) = 0.0; + motor_command_tmp.kp.at(i) = kps[i]; + motor_command_tmp.kd.at(i) = kds[i]; + motor_command_tmp.dq_target.at(i) = 0.0; + } } // Optional waist pitch offset (keys 7/8). Default 0°. double waist_offset_rad = g_waist_pitch_offset_deg.load() * M_PI / 180.0;