添加record脚本

robot_client/teleoperators/flight_stick/__init__.py

@@ -0,0 +1,2 @@
+from .flight_stick import FlightStick,FlightStickConfig
+__all__ = ["FlightStick", "FlightStickConfig"]

robot_client/teleoperators/flight_stick/config.py

@@ -0,0 +1,22 @@
+from dataclasses import dataclass
+from lerobot.teleoperators import TeleoperatorConfig
+@dataclass
+@TeleoperatorConfig.register_subclass("flight_stick")
+class FlightStickConfig(TeleoperatorConfig):
+    #控制器索引
+    index: int = 0
+    #定义启动按钮
+    start_button: str = "button_7"
+    #定义精细控制模式切换按钮
+    fine_control_button: str = "button_10"
+    #定义夹爪开按钮
+    gripper_open_button: str = "button_1"
+    #定义夹爪关按钮
+    gripper_close_button: str = "button_0"
+    #定义方向帽子键
+    hat_button: int = 0
+    #RX旋转映射
+    RX_MAP: str = "button_4_5"
+    #RY旋转映射
+    RX_MAP: str = "button_2_3"
+    

robot_client/teleoperators/flight_stick/flight_stick.py

@@ -0,0 +1,60 @@
+import pygame
+from .config import FlightStickConfig
+from ..xbox import Xbox
+"""
+FlightStick类用于检测和监控飞行手柄。
+"""
+class FlightStick(Xbox):
+    config_class = FlightStickConfig
+    name = "flight_stick"
+
+    def get_action(self):
+        pygame.event.pump()
+        pose_speeds = [0.0] * 6
+        """更新末端位姿控制"""
+        # 根据控制模式调整步长
+        current_linear_step = self.linear_step * (0.1 if self.fine_control_mode else 1.0)
+        current_angular_step = self.angular_step * (0.1 if self.fine_control_mode else 1.0)
+        
+        # print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}")
+        print(f"精细控制模式: {self.fine_control_mode}")
+        # print(f"{self.joystick.get_axis(0)},{self.joystick.get_axis(1)},{self.joystick.get_axis(2)},{self.joystick.get_axis(3)}")
+        # 方向键控制XY
+        hat = self.joystick.get_hat(0)
+        hat_up = hat[1] == 1     # Y+
+        hat_down = hat[1] == -1  # Y-
+        hat_left = hat[0] == -1  # X-
+        hat_right = hat[0] == 1  # X+
+        # 计算各轴速度
+        pose_speeds[0] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0)  # X
+        pose_speeds[1] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0)  # Y
+        
+        # print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}")
+        # 油门控制Z轴
+        gas_axis = -self.joystick.get_axis(2)
+        
+        # 设置Z速度
+        z_mapping = self.apply_nonlinear_mapping(gas_axis)
+        # print(f"Z轴非线性映射: {gas_axis} -> {z_mapping}")
+        pose_speeds[2] = z_mapping * current_linear_step  # Z
+
+        
+        # 主摇杆X轴控制RX旋转
+        # 设置RX旋转速度
+        rx_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(1))
+        # print(f"RX轴非线性映射: {x_axis} -> {rx_mapping}")
+        pose_speeds[3] = rx_mapping * current_angular_step * 2  # RX
+        
+        # 主摇杆Y轴控制RY旋转
+        # 设置RY旋转速度
+        ry_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(0))
+        # print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}")
+        pose_speeds[4] = ry_mapping * current_angular_step * 2  # RY
+    
+        # 主摇杆左右旋转轴 控制RZ旋转
+        # 设置RZ旋转速度
+        rz_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(3))
+        # print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}")
+        pose_speeds[5] = rz_mapping * current_angular_step * 2  # RZ
+        
+        return pose_speeds