From 732f70965763c706c99cdef81269d6d5430bee76 Mon Sep 17 00:00:00 2001 From: Ailee Date: Mon, 20 Jul 2026 17:58:35 +0800 Subject: [PATCH] add xbox gamepad --- MUJOCO_LOG.TXT | 2 + README.md | 12 + README_EN.md | 12 + interface/user_command/gamepad_interface.hpp | 336 +++++++++++++++++++ state_machine/state_machine.hpp | 15 +- 5 files changed, 376 insertions(+), 1 deletion(-) create mode 100644 MUJOCO_LOG.TXT create mode 100644 interface/user_command/gamepad_interface.hpp diff --git a/MUJOCO_LOG.TXT b/MUJOCO_LOG.TXT new file mode 100644 index 00000000..9ab9b6b1 --- /dev/null +++ b/MUJOCO_LOG.TXT @@ -0,0 +1,2 @@ +WARNING Mon Jul 20 15:56:16 2026: Nan, Inf or huge value in CTRL at ACTUATOR 0. The simulation is unstable. Time = 79.3880. + diff --git a/README.md b/README.md index 382c58dd..6440d52f 100644 --- a/README.md +++ b/README.md @@ -44,11 +44,23 @@ cd build tips：可以将仿真器窗口设为始终位于最上层，方便可视化 +**键盘控制：** - z： 机器狗站立进入默认状态 - c： 机器狗站立进入rl控制状态 - wasd：前后左右 - qe：顺逆时针旋转 +**Xbox手柄控制：** + +仿真模式下会自动检测Xbox手柄，如果检测到手柄则使用手柄控制，否则使用键盘控制。 + +- 左肩键 LB：机器狗站立（对应键盘z） +- 右肩键 RB：进入RL控制状态（对应键盘c） +- 左扳机 LT：关节阻尼模式（对应键盘r） +- 左摇杆 上下：前后移动（对应键盘ws） +- 左摇杆 左右：左右平移（对应键盘ad） +- 右摇杆 左右：转向（对应键盘qe） + # 仿真-实际 此过程和仿真-仿真几乎一模一样，只需要添加连wifi传输数据步骤，然后修改编译指令即可。目前默认实机操控为retroid手柄模式，如需使用键盘模式，可在state_machine/state_machine.hpp中第121行更改为 diff --git a/README_EN.md b/README_EN.md index 1833388b..5f1a13fe 100644 --- a/README_EN.md +++ b/README_EN.md @@ -46,11 +46,23 @@ cd build tips：right click simulator window and select "always on top" +**Keyboard Control:** - z： default position - c： rl control default position - wasd：forward/leftward/backward/rightward - qe：clockwise/counter clockwise +**Xbox Gamepad Control:** + +In simulation mode, the system will automatically detect an Xbox gamepad. If detected, it will use gamepad control; otherwise, it will use keyboard control. + +- Left Bumper (LB): Stand up (corresponds to keyboard z) +- Right Bumper (RB): Enter RL control mode (corresponds to keyboard c) +- Left Trigger (LT): Joint damping mode (corresponds to keyboard r) +- Left Stick Up/Down: Forward/backward movement (corresponds to keyboard ws) +- Left Stick Left/Right: Side movement (corresponds to keyboard ad) +- Right Stick Left/Right: Turning (corresponds to keyboard qe) + # Sim-to-Real diff --git a/interface/user_command/gamepad_interface.hpp b/interface/user_command/gamepad_interface.hpp new file mode 100644 index 00000000..18e4e7df --- /dev/null +++ b/interface/user_command/gamepad_interface.hpp @@ -0,0 +1,336 @@ +#pragma once + +#include "user_command_interface.h" +#include "custom_types.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define AXIS_STEP 0.1 +#define JOYSTICK_DEADZONE 0.1 + +using namespace interface; +using namespace types; + +class GamepadInterface : public UserCommandInterface +{ + /** + * @brief Xbox手柄按键映射说明 + * + * 左摇杆 X轴 → A/D键 (左右平移速度) + * 左摇杆 Y轴 → W/S键 (前后移动速度) + * 右摇杆 X轴 → Q/E键 (转向速度) + * 左肩键 LB → Z键 (站立) + * 右肩键 RB → C键 (切换到RL控制模式) + * 左扳机 LT → R键 (关节阻尼模式) + */ +private: + UserCommand usr_cmd_; + bool start_thread_flag_; + std::thread gp_thread_; + int js_fd_; + std::mutex mtx_; + + struct GamepadState { + float left_axis_x = 0.0f; + float left_axis_y = 0.0f; + float right_axis_x = 0.0f; + float right_axis_y = 0.0f; + bool lb_pressed = false; + bool rb_pressed = false; + bool lt_pressed = false; + } gp_state_; + + bool connection_mode_detected_ = false; + bool is_bluetooth_ = false; + + void ClipNumber(float &num, float low, float up){ + if(low > up) std::cerr << "error clip" << std::endl; + if(num < low) num = low; + if(num > up) num = up; + } + + float ApplyDeadzone(float value, float deadzone = JOYSTICK_DEADZONE){ + if(std::abs(value) < deadzone) return 0.0f; + return (value - std::copysign(deadzone, value)) / (1.0f - deadzone); + } + + double GetCurrentTimeStamp(){ + static timespec startup_timestamp; + timespec now_timestamp; + if (startup_timestamp.tv_sec + startup_timestamp.tv_nsec == 0) { + clock_gettime(CLOCK_MONOTONIC,&startup_timestamp); + } + clock_gettime(CLOCK_MONOTONIC,&now_timestamp); + return (now_timestamp.tv_sec-startup_timestamp.tv_sec)*1e3 + + (now_timestamp.tv_nsec-startup_timestamp.tv_nsec)/1e6; + } + +public: + GamepadInterface(const std::string& js_device = "/dev/input/js0"){ + std::memset(&usr_cmd_, 0, sizeof(usr_cmd_)); + js_fd_ = -1; + std::cout << "Using Xbox Gamepad Command Interface" << std::endl; + std::cout << "Opening device: " << js_device << std::endl; + + js_fd_ = open(js_device.c_str(), O_RDONLY | O_NONBLOCK); + if(js_fd_ < 0){ + std::cerr << "Failed to open gamepad device: " << js_device << std::endl; + std::cerr << "Make sure the gamepad is connected and you have permission to access it." << std::endl; + } else { + std::cout << "Gamepad opened successfully!" << std::endl; + DetectConnectionMode(); + } + } + + void DetectConnectionMode(){ + js_event js_ev; + std::cout << "[Gamepad] Detecting connection mode..." << std::endl; + std::cout << "[Gamepad] Please move any stick or press any button..." << std::endl; + + while(connection_mode_detected_ == false){ + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + while(read(js_fd_, &js_ev, sizeof(js_event)) == sizeof(js_event)){ + if(js_ev.type == JS_EVENT_AXIS){ + if(js_ev.number == 2){ + if(std::abs(js_ev.value) > 30000){ + is_bluetooth_ = false; + connection_mode_detected_ = true; + std::cout << "[Gamepad] Detected USB connection mode" << std::endl; + } else if(std::abs(js_ev.value) > 100){ + is_bluetooth_ = true; + connection_mode_detected_ = true; + std::cout << "[Gamepad] Detected Bluetooth connection mode" << std::endl; + } + } + } else if(js_ev.type == JS_EVENT_BUTTON){ + if(js_ev.number == 6 || js_ev.number == 7){ + is_bluetooth_ = true; + connection_mode_detected_ = true; + std::cout << "[Gamepad] Detected Bluetooth connection mode" << std::endl; + } else if(js_ev.number == 2 || js_ev.number == 3){ + is_bluetooth_ = false; + connection_mode_detected_ = true; + std::cout << "[Gamepad] Detected USB connection mode" << std::endl; + } + } + } + } + } + + ~GamepadInterface(){ + if(js_fd_ >= 0) close(js_fd_); + } + + virtual void Start(){ + if(js_fd_ < 0){ + std::cerr << "Cannot start gamepad thread: device not opened" << std::endl; + return; + } + start_thread_flag_ = true; + gp_thread_ = std::thread(std::bind(&GamepadInterface::Run, this)); + } + + virtual void Stop(){ + start_thread_flag_ = false; + if(gp_thread_.joinable()){ + gp_thread_.join(); + } + } + + virtual UserCommand GetUserCommand() override { + std::lock_guard lock(mtx_); + return usr_cmd_; + } + + virtual void SetMotionStateFeedback(const MotionStateFeedback& msfb){ + std::lock_guard lock(mtx_); + msfb_ = msfb; + usr_cmd_.target_mode = msfb.current_state; + } + + void Run(){ + js_event js_ev; + double forward_time_record = GetCurrentTimeStamp(); + double side_time_record = GetCurrentTimeStamp(); + double turning_time_record = GetCurrentTimeStamp(); + + bool lb_prev = false; + bool rb_prev = false; + bool lt_prev = false; + + std::cout << "Start Gamepad Listening" << std::endl; + + while (start_thread_flag_) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + while(read(js_fd_, &js_ev, sizeof(js_event)) == sizeof(js_event)){ + std::lock_guard lock(mtx_); + + switch(js_ev.type & ~JS_EVENT_INIT){ + case JS_EVENT_AXIS: + if(is_bluetooth_){ + switch(js_ev.number){ + case 0: // Left stick X + gp_state_.left_axis_x = ApplyDeadzone(js_ev.value / 32767.0f); + break; + case 1: // Left stick Y + gp_state_.left_axis_y = ApplyDeadzone(js_ev.value / 32767.0f); + break; + case 2: // Right stick X + gp_state_.right_axis_x = ApplyDeadzone(js_ev.value / 32767.0f); + break; + case 3: // Right stick Y + gp_state_.right_axis_y = ApplyDeadzone(js_ev.value / 32767.0f); + break; + case 4: // RT (Right Trigger) + break; + case 5: // LT (Left Trigger) + gp_state_.lt_pressed = (js_ev.value > 10000); + break; + } + } else { + switch(js_ev.number){ + case 0: // Left stick X + gp_state_.left_axis_x = ApplyDeadzone(js_ev.value / 32767.0f); + break; + case 1: // Left stick Y + gp_state_.left_axis_y = ApplyDeadzone(js_ev.value / 32767.0f); + break; + case 2: // LT (Left Trigger) + gp_state_.lt_pressed = (js_ev.value > 10000); + break; + case 3: // Right stick Y + gp_state_.right_axis_y = ApplyDeadzone(js_ev.value / 32767.0f); + break; + case 4: // Right stick X + gp_state_.right_axis_x = ApplyDeadzone(js_ev.value / 32767.0f); + break; + case 5: // RT (Right Trigger) + break; + } + } + break; + + case JS_EVENT_BUTTON: + if(is_bluetooth_){ + switch(js_ev.number){ + case 0: // A + break; + case 1: // B + break; + case 3: // X + break; + case 4: // Y + break; + case 6: // LB (Left Bumper) + gp_state_.lb_pressed = (js_ev.value != 0); + break; + case 7: // RB (Right Bumper) + gp_state_.rb_pressed = (js_ev.value != 0); + break; + case 11: // Menu button + break; + case 10: // View button + break; + } + } else { + switch(js_ev.number){ + case 0: // A + break; + case 1: // B + break; + case 2: // X + break; + case 3: // Y + break; + case 4: // LB (Left Bumper) + gp_state_.lb_pressed = (js_ev.value != 0); + break; + case 5: // RB (Right Bumper) + gp_state_.rb_pressed = (js_ev.value != 0); + break; + case 6: // View button + break; + case 7: // Menu button + break; + } + } + break; + } + } + + std::lock_guard lock(mtx_); + + switch(msfb_.current_state) { + case RobotMotionState::WaitingForStand: + if(gp_state_.lb_pressed && !lb_prev){ + usr_cmd_.target_mode = int(RobotMotionState::StandingUp); + std::cout << "[Gamepad] LB pressed -> StandingUp" << std::endl; + } + break; + + case RobotMotionState::StandingUp: + if(gp_state_.rb_pressed && !rb_prev){ + usr_cmd_.target_mode = int(RobotMotionState::RLControlMode); + std::cout << "[Gamepad] RB pressed -> RLControlMode" << std::endl; + } + break; + + case RobotMotionState::RLControlMode: { + if(gp_state_.lt_pressed && !lt_prev){ + usr_cmd_.target_mode = int(RobotMotionState::JointDamping); + std::cout << "[Gamepad] LT pressed -> JointDamping" << std::endl; + } + + double current_time = GetCurrentTimeStamp(); + + float forward_input = -gp_state_.left_axis_y; + float side_input = gp_state_.left_axis_x; + float turning_input = -gp_state_.right_axis_x; + + if(std::abs(forward_input) > 0.01){ + usr_cmd_.forward_vel_scale = forward_input; + forward_time_record = current_time; + } + + if(std::abs(side_input) > 0.01){ + usr_cmd_.side_vel_scale = side_input; + side_time_record = current_time; + } + + if(std::abs(turning_input) > 0.01){ + usr_cmd_.turnning_vel_scale = turning_input; + turning_time_record = current_time; + } + + if(current_time - forward_time_record > 300.) usr_cmd_.forward_vel_scale = 0; + if(current_time - side_time_record > 300.) usr_cmd_.side_vel_scale = 0; + if(current_time - turning_time_record > 300.) usr_cmd_.turnning_vel_scale = 0; + + ClipNumber(usr_cmd_.forward_vel_scale, -1., 1.); + ClipNumber(usr_cmd_.side_vel_scale, -1., 1.); + ClipNumber(usr_cmd_.turnning_vel_scale, -1., 1.); + break; + } + + default: + break; + } + + lb_prev = gp_state_.lb_pressed; + rb_prev = gp_state_.rb_pressed; + lt_prev = gp_state_.lt_pressed; + } + + std::cout << "Gamepad thread stopped" << std::endl; + } +}; diff --git a/state_machine/state_machine.hpp b/state_machine/state_machine.hpp index a60bf39e..7296a9f5 100644 --- a/state_machine/state_machine.hpp +++ b/state_machine/state_machine.hpp @@ -26,6 +26,9 @@ #include "skydroid_gamepad_interface.hpp" #include "retroid_gamepad_interface.hpp" #include "keyboard_interface.hpp" +#include "gamepad_interface.hpp" +#include +#include #ifdef USE_RAISIM #include "simulation/jueying_raisim_simulation.hpp" #endif @@ -115,7 +118,17 @@ class StateMachine{ std::string urdf_path = ""; std::string mjcf_path = ""; #ifdef BUILD_SIMULATION - uc_ptr_ = std::make_shared(); + // 检测Xbox手柄是否存在 + std::string js_device = "/dev/input/js0"; + int js_fd = open(js_device.c_str(), O_RDONLY); + if(js_fd >= 0){ + close(js_fd); + std::cout << "Xbox gamepad detected, using GamepadInterface" << std::endl; + uc_ptr_ = std::make_shared(js_device); + } else { + std::cout << "No Xbox gamepad detected, using KeyboardInterface" << std::endl; + uc_ptr_ = std::make_shared(); + } #else uc_ptr_ = std::make_shared(12121); #endif