添加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

robot_client/teleoperators/xbox/__init__.py

@@ -0,0 +1,2 @@
+from .xbox import Xbox,XboxConfig
+__all__ = ["Xbox", "XboxConfig"]

robot_client/teleoperators/xbox/config.py

@@ -0,0 +1,7 @@
+from dataclasses import dataclass
+from lerobot.teleoperators import TeleoperatorConfig
+@dataclass
+@TeleoperatorConfig.register_subclass("xbox")
+class XboxConfig(TeleoperatorConfig):
+    #控制器索引
+    index: int = 0

robot_client/teleoperators/xbox/xbox.py

@@ -0,0 +1,140 @@
+import pygame
+import time
+import sys
+from  lerobot.teleoperators import Teleoperator
+from .config import XboxConfig
+"""
+Xbox类用于检测和监控飞行手柄。
+"""
+class Xbox(Teleoperator):
+    config_class = XboxConfig
+    name = "flight_stick"
+    def __init__(self, config: XboxConfig):
+        self.config = config
+        # 控制参数
+        self.linear_step = 0.0015  # 线性移动步长(m)
+        self.angular_step = 0.001  # 角度步长(rad) - 从度转换为弧度
+        # 摇杆死区
+        self.deadzone = 0.15
+        
+        # 精细控制模式
+        self.fine_control_mode = False
+        self.reset()
+    def reset(self):
+        self.joystick = None
+    @property
+    def action_features(self):
+        return {
+            "dtype": "float32",
+            "shape": (len(self.pose.keys()),),
+            "names": self.pose.keys(),
+        }
+
+    @property
+    def feedback_features(self):
+        return {}
+
+    @property
+    def is_connected(self):
+        return self.joystick is not None
+
+    def connect(self, calibrate = True):
+        # 初始化pygame
+        pygame.init()
+        pygame.joystick.init()
+        """检测连接的手柄"""
+        joystick_count = pygame.joystick.get_count()
+        print(f"检测到 {joystick_count} 个手柄设备")
+        
+        if joystick_count == 0:
+            print("未检测到任何手柄设备！")
+            return False
+
+        if joystick_count<self.config.index+1:
+            print(f"未检测到配置中指定的索引设备！") 
+            return False
+        # 初始化所有检测到的手柄
+        self.joystick = pygame.joystick.Joystick(self.config.index)
+        print(f"已连接设备：{self.joystick.get_name()}")
+
+    @property
+    def is_calibrated(self):
+        return True
+
+    def calibrate(self):
+        pass
+
+    def configure(self):
+        pass
+
+    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)
+        if self.joystick.get_button(0):  # A按钮
+                self.fine_control_mode = True
+        if self.joystick.get_button(1):  # B按钮
+                self.fine_control_mode = False
+        
+        # print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}")
+        print(f"精细控制模式: {self.fine_control_mode}")
+        
+        # 方向键控制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+
+        # print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}")
+        # 计算各轴速度
+        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
+
+        # 右摇杆X轴
+        z_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(1))
+        print(f"Z轴非线性映射: {self.joystick.get_axis(1)} -> {z_mapping}")
+        pose_speeds[2] = z_mapping * current_angular_step * 2  # RY
+
+        #左摇杆X轴控制RX旋转
+        # 设置RX旋转速度
+        rx_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(2))
+        # 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(3))
+        # print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}")
+        pose_speeds[4] = ry_mapping * current_angular_step * 2  # RY
+    
+        # 左右扳机控制RZ旋转
+        # 设置RZ旋转速度
+        right_tiger = ((self.joystick.get_axis(5)+1)/2)
+        left_tiger = ((self.joystick.get_axis(4)+1)/2)
+        print(f"左右扳机: {left_tiger} {right_tiger}")
+        rz_mapping = self.apply_nonlinear_mapping(left_tiger - right_tiger)
+        # print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}")
+        pose_speeds[5] = rz_mapping * current_angular_step * 2  # RZ
+        
+        return pose_speeds
+    
+    def apply_nonlinear_mapping(self, value):
+        """应用非线性映射以提高控制精度"""
+        # 保持符号，但对数值应用平方映射以提高精度
+        value = 0.0 if abs(value) < self.deadzone else value
+        sign = 1 if value >= 0 else -1
+        return sign * (abs(value) ** 2)
+    def normalize_value(self, v1,v2):
+        """把两个轴归一化到一个轴上"""
+        return v1-v2
+
+    def send_feedback(self, feedback):
+        pass
+
+    def disconnect(self):
+        self.reset()
+        pygame.joystick.quit()
+        pygame.quit()