1.增加xbox控制器和飞行手柄控制器

2.增加多臂多控制器模式
3.末端姿态由欧拉角控制切换到四元数控制
4.增加vr手柄控制器

Signed-off-by: 1002142102@qq.com <1002142102@qq.com>

robot_client/robots/mix/__init__.py

@@ -0,0 +1,3 @@
+from .robot_mix import MixRobot
+from .config import MixRobotConfig
+__all__ = ["MixRobot", "MixRobotConfig"]

robot_client/robots/mix/config.py

@@ -0,0 +1,8 @@
+from dataclasses import dataclass, field
+from lerobot.robots.config import RobotConfig
+
+@RobotConfig.register_subclass("mix")
+@dataclass
+class MixRobotConfig(RobotConfig):
+    # Port to connect to the arm
+    robotList: dict[str,RobotConfig] = field(default_factory=dict[str,RobotConfig])

robot_client/robots/mix/robot_mix.py

@@ -0,0 +1,98 @@
+import logging
+from functools import cached_property
+import time
+from typing import Any
+from .config import MixRobotConfig
+from lerobot.robots.robot import Robot
+from lerobot.utils.errors import DeviceNotConnectedError
+from lerobot.robots import Robot,make_robot_from_config
+
+logger = logging.getLogger(__name__)
+class MixRobot(Robot):
+    config_class = MixRobotConfig
+    name = "mix"
+    def __init__(self, config: MixRobotConfig):
+        super().__init__(config)
+        self.config = config
+        self.robotList = {}
+        for name,config in self.config.robotList.items():
+            self.robotList[name] = make_robot_from_config(config)
+    @property
+    def _motors_ft(self) -> dict[str, type]:
+        ret = {}
+        for name,robot in self.robotList.items():
+            for pname,motor in robot._motors_ft:
+                pname = pname.replace(".pos", "")
+                ret[f"{pname}_{name}.pos"] = motor
+        return ret
+
+    @property
+    def _cameras_ft(self) -> dict[str, tuple]:
+        ret = {}
+        for name,robot in self.robotList.items():
+            for pname,cam in robot._cameras_ft:
+                ret[f"{pname}.{name}"] = cam
+        return ret
+
+    @cached_property
+    def observation_features(self) -> dict[str, type | tuple]:
+        return {**self._motors_ft, **self._cameras_ft}
+
+    @cached_property
+    def action_features(self) -> dict[str, type]:
+        ret = {}
+        for name,robot in self.robotList.items():
+            for pname,typeVal in robot.action_features.items():
+                ret[f"{name}.{pname}"] = typeVal
+        return ret
+
+    @property
+    def is_connected(self) -> bool:
+        return all([robot.is_connected for robot in self.robotList.values()])
+    
+    def connect(self, calibrate: bool = True) -> None:
+        for name,robot in self.robotList.items():
+            robot.connect(calibrate)
+    
+    @property
+    def is_calibrated(self) -> bool:
+        return all([robot.is_calibrated for robot in self.robotList.values()])
+    
+    def calibrate(self) -> None:
+        pass
+    
+    def configure(self) -> None:
+        pass
+    
+    def setup_motors(self) -> None:
+        pass
+
+    def get_observation(self) -> dict[str, Any]:
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"{self} is not connected.")
+        obs_dict = {}
+        for name,robot in self.robotList.items():
+            obs_dict.update({f"{name}.{kname}":item for kname,item in robot.get_observation().items()})
+        return obs_dict
+    
+    def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"{self} is not connected.")
+        dic = {}
+        for key,value in action.items():
+            name = key.split(".")[0]
+            if name not in dic:
+                dic[name] = {}
+            dic[name][key.removeprefix(name+".")] = value
+
+        for dname,dvalue in dic.items():
+            if dname not in self.robotList:
+                continue
+            self.robotList[dname].send_action(dvalue)
+
+    def disconnect(self):
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"{self} is not connected.")
+
+        for name,robot in self.robotList.items():
+            robot.disconnect()

robot_client/robots/realman/config.py

@@ -1,5 +1,4 @@
 from dataclasses import dataclass, field
-from lerobot.cameras.configs import CameraConfig
 from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraConfig
 from lerobot.motors.motors_bus import Motor, MotorCalibration
 from lerobot.robots.config import RobotConfig
@@ -9,7 +8,7 @@ from lerobot.robots.config import RobotConfig
 class RealmanRobotConfig(RobotConfig):
     # Port to connect to the arm
     port: str
-    mock: bool = field(default_factory=bool)\
+    mock: bool = field(default_factory=bool)
     #0：关节角度透传模式 1:笛卡尔速度透传
     mode: int = 0
     gripper_range: list[int] = field(default_factory=list)
@@ -18,5 +17,7 @@ class RealmanRobotConfig(RobotConfig):
     # cameras
     cameras: dict[str, RealSenseCameraConfig] = field(default_factory=dict)
     joint: list=field(default_factory=list)
+    udp_ip: str|None = "127.0.0.1"
+    udp_port: int = 8090
     motors: dict[str, Motor] = field(default_factory=dict)
     calibration: dict[str, MotorCalibration] | None = None

robot_client/robots/realman/motors_bus.py

@@ -1,7 +1,12 @@
+import threading
 import time
 from typing import Any, Dict
-from Robotic_Arm.rm_robot_interface import RoboticArm,rm_thread_mode_e
+from Robotic_Arm.rm_robot_interface import *
 from lerobot.motors import Motor, MotorCalibration, MotorsBus
+from .robot_mock import RealmanMock
+from scipy.spatial.transform import Rotation as R
+
+import numpy
 #动作执行成功
 ACTION_SUCCESS = 0
 
@@ -22,6 +27,8 @@ class RealmanMotorsBus(MotorsBus):
         motors:dict[str, Motor],
         gripper_range:list[int],
         joint: list,
+        udp_ip: str,
+        udp_port: int,
         mock: bool = False,
         calibration: dict[str, MotorCalibration] | None = None,
     ):
@@ -34,9 +41,27 @@ class RealmanMotorsBus(MotorsBus):
         self.init_joint_position = joint
         self.safe_disable_position = joint
         self.mock = mock
-        self.rmarm = RoboticArm(rm_thread_mode_e.RM_DUAL_MODE_E)
+        if mock:
+            #self.rmarm = RealmanMock()
+            self.rmarm = RoboticArm(rm_thread_mode_e.RM_DUAL_MODE_E)
+        else:
+            self.rmarm = RoboticArm(rm_thread_mode_e.RM_DUAL_MODE_E)
         self.handle = None
-        self.init_pose = None
+        self.init_pose = numpy.zeros(8)
+        self.init_pose[-2] = 1
+        self.running = False
+        custom = rm_udp_custom_config_t()
+        custom.joint_speed = 0
+        # custom.lift_state = 1
+        custom.hand_state = 1
+        custom.plus_state = 1
+        custom.plus_base=1
+        custom.expand_state = 1
+        custom.arm_current_status = 1
+        custom.expand_state = 1
+        self.udp_config = rm_realtime_push_config_t(1, True, udp_port, 0, udp_ip, custom)
+        self.lock = threading.Lock()
+        self.temp_pose = numpy.zeros(8)
 
     @property
     def motor_names(self) -> list[str]:
@@ -49,6 +74,14 @@ class RealmanMotorsBus(MotorsBus):
     @property
     def motor_indices(self) -> list[int]:
         return [idx for idx, _ in self.motors.values()]
+    
+    def update_arm_state(self, state:rm_realtime_arm_joint_state_t):
+        joint = list(state.joint_status.joint_position)
+        gripper = state.expandState.pos
+        pose = state.waypoint.position
+        euler = state.waypoint.euler
+        pose = [pose.x, pose.y, pose.z, euler.rx, euler.ry, euler.rz, 0]
+        self.current_arm_state = {"joint": joint, "gripper": gripper, "pose": pose}
 
 
     def connect(self, enable=True) -> bool:
@@ -56,6 +89,7 @@ class RealmanMotorsBus(MotorsBus):
         使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序
         '''
         self.handle = self.rmarm.rm_create_robot_arm(self.ip, self.port)
+
         if self.mock:
             self.rmarm.rm_set_arm_run_mode(0)#仿真模式
         else:
@@ -97,17 +131,34 @@ class RealmanMotorsBus(MotorsBus):
                 enable_flag = True
                 break
             time.sleep(1)
+        # self.rmarm.rm_set_realtime_push(self.udp_config)
+        # self.arm_state_callback = rm_realtime_arm_state_callback_ptr(self.update_arm_state)
+        # self.rmarm.rm_realtime_arm_state_call_back(self.arm_state_callback)
         self.rmarm.rm_movej(self.init_joint_position[:-1], 5, 0, 0, 1)
-        ret = self.rmarm.rm_get_current_arm_state()
-        self.init_pose = ret[1]['pose']
-        #读取夹状态
-        gripper_msg = self.rmarm.rm_get_gripper_state()[1]
-        gripper_state = gripper_msg['actpos']
-        self.init_pose.append(gripper_state)
+        self.init_pose = self.getQuatPose()
         resp = enable_flag
+        self.running = True
+        threading.Thread(target=self.update_pose).start()
+        threading.Thread(target=self.update_gripper).start()
         print(f"Returning response: {resp}")
         return resp
     
+    def getQuatPose(self)->tuple[int,rm_current_arm_state_t]:
+        state = rm_current_arm_state_t()
+        ret = rm_get_current_arm_state(self.handle, byref(state))
+        return numpy.array([
+            state.pose.position.x, 
+            state.pose.position.y, 
+            state.pose.position.z,
+            state.pose.quaternion.w,
+            state.pose.quaternion.x, 
+            state.pose.quaternion.y, 
+            state.pose.quaternion.z, 
+            self.rmarm.rm_get_gripper_state()[1]['actpos']
+        ])
+    def read_pose(self):
+        return self.init_pose
+    
     def motor_names(self):
         return
     
@@ -129,7 +180,7 @@ class RealmanMotorsBus(MotorsBus):
         return False
     
     def write_arm(self, target_joint: list):
-        ret = self.rmarm.rm_movej_follow(target_joint[:-1])
+        ret = self.rmarm.rm_movej(self.init_joint_position[:-1], 5, 0, 0, 1)
         if ret != 0: 
             print("movej error:", ret)
             return ret
@@ -142,11 +193,7 @@ class RealmanMotorsBus(MotorsBus):
             - 机械臂关节消息,单位1度;[-1, 1]
             - 机械臂夹爪消息,[-1, 1]
         """
-        joint_msg = self.rmarm.rm_get_current_arm_state()[1]
-        joint_state = joint_msg['joint']
-
-        gripper_msg = self.rmarm.rm_get_gripper_state()[1]
-        gripper_state = gripper_msg['actpos']
+        joint_state = self.read_pose()
         
         return {
             "joint_1": joint_state[0]/180,
@@ -155,9 +202,9 @@ class RealmanMotorsBus(MotorsBus):
             "joint_4": joint_state[3]/180,
             "joint_5": joint_state[4]/180,
             "joint_6": joint_state[5]/180,
-            "gripper": (gripper_state-500)/500
+            "gripper": (joint_state[6]-500)/500
         }
-    
+    5
     def disconnect(self, disable_torque = True):
         self.safe_disconnect()
 
@@ -166,8 +213,8 @@ class RealmanMotorsBus(MotorsBus):
             Move to safe disconnect position
         """
         self.write_arm(target_joint=self.safe_disable_position)
+        self.running = False
         # 断开所有连接，销毁线程
-        RoboticArm.rm_destory()
 
     """
     向机械臂写入动作
@@ -176,41 +223,40 @@ class RealmanMotorsBus(MotorsBus):
         if name =="Goal_Position":
             self.write_arm(target_joint=actionData)
         elif name == "Goal_Velocity":#速度透传模式
-            values = list(actionData.values())
-            for k,v in enumerate(self.init_pose):
-                self.init_pose[k]+=values[k]
-            r = self.rmarm.rm_movej_p(self.init_pose[:-1], 50, 0, 0, 0)
-            if r!=0:
-                print("movej error:", r)
-                raise Exception("movej error")
-            print(f"r:{r},pose:{self.init_pose}")
-            # self.init_pose[6]+=actionData['gripper']
-            # self.rmarm.rm_set_gripper_position(int(self.init_pose[6]*1000), False, 0)
+            values = numpy.array(list(actionData.values()))
+            with self.lock:
+                self.temp_pose += numpy.array([*values[:6],0,values[-1]])
+    def update_pose(self):
+        while self.running:
+            with self.lock:
+                self.temp_pose[-1] = self.temp_pose[-1]*1000
+                temp = self.init_pose + self.temp_pose
+                newr = R.from_rotvec(self.temp_pose[3:6].tolist())
+                oldr = R.from_quat(self.init_pose[3:7].tolist())
+                temp[3:7] = (oldr * newr).as_quat()
+                self.temp_pose.fill(0)
+            ret = self.rmarm.rm_movep_canfd(temp[:-1].tolist(), False)
+            if ret == 0:
+                self.init_pose = temp
+            else:
+                print(f"Error: {ret}")
+            time.sleep(0.01)
+    def clamp(self,value, min_value, max_value):
+        return max(min(value, max_value), min_value)
+    def update_gripper(self):
+        while self.running:
+            if self.init_pose[-1]>0:
+                self.rmarm.rm_set_gripper_release(500,False,0)
+                self.init_pose[-1] = 0
+            elif self.init_pose[-1]<0:
+                self.rmarm.rm_set_gripper_pick(500,300,False,0)
+                self.init_pose[-1] = 0
     
     def sync_read(self, data_name, motors = None, *, normalize = True, num_retry = 0):
         if data_name == "Present_Position":
             return self.read()
 
-    def _assert_protocol_is_compatible(self, instruction_name):
-        return True
-
-
-    """
-    配置电机
-    """
-    def configure_motors(self, return_delay_time=0, maximum_acceleration=254, acceleration=254) -> None:
-        self.rmarm.rm_set_gripper_route(self.gripper_range[0],self.gripper_range[1])
-
-    def disable_torque(self, motors: int | str | list[str] | None = None, num_retry: int = 0):
-        for motor in self._get_motors_list(motors):
-            self.rmarm.rm_set_joint_en_state(motor,0)
-
-    def _disable_torque(self, motor: int, model: str, num_retry: int = 0):
-        self.rmarm.rm_set_joint_en_state(motor,0)
-
-    def enable_torque(self, motors = None, num_retry = 0):
-        for motor in self._get_motors_list(motors):
-            self.rmarm.rm_set_joint_en_state(motor,1)
+    
     @property
     def is_connected(self) -> bool:
         return self.handle is not None
@@ -222,48 +268,7 @@ class RealmanMotorsBus(MotorsBus):
         pass
 
     def _get_half_turn_homings(self, positions):
-        offsets = {}
-        for motor_id, current_pos in positions.items():
-            # 确保输入是角度值
-            current_deg = self._ensure_angle_degrees(current_pos, motor_id)
-            # 核心逻辑：让当前位置变成180°
-            offset = self._calculate_angle_offset(current_deg, motor_id)
-            offsets[motor_id] = offset
-        return offsets
-
-    def _calculate_angle_offset(self, current_deg, motor_id):
-        """计算角度偏移量"""
-        
-        # 获取关节角度范围（从配置或SDK）
-        # min_angle, max_angle = self._get_joint_limits(motor_id)
-        
-        # 对于单圈关节，我们想要的目标是范围中点
-        # 但set_half_turn_homings的语义是"变成半圈(180°)"
-        TARGET = 180.0  # 固定目标：180°
-        
-        # 计算基本偏移
-        offset = TARGET - current_deg
-        
-        # 单圈关节处理：偏移应该在[-180, 180)范围内
-        if motor_id <= 6:  # 单圈关节
-            # 优化偏移到最短路径
-            while offset >= 180.0:
-                offset -= 360.0
-            while offset < -180.0:
-                offset += 360.0
-        else:  # 多圈关节（关节7）
-            # 多圈关节：只需要考虑在当前圈内的偏移
-            current_in_first_turn = current_deg % 360.0
-            offset = TARGET - current_in_first_turn
-            
-            # 同样优化到最短路径
-            if offset >= 180.0:
-                offset -= 360.0
-            elif offset < -180.0:
-                offset += 360.0
-        
-        # 保留RealMan精度
-        return round(offset, 3)
+        pass
     
     def _find_single_motor(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
         return 0,self.motors[motor].id
@@ -281,3 +286,21 @@ class RealmanMotorsBus(MotorsBus):
         pass
     def _handshake(self) -> None:
         pass
+    def _assert_protocol_is_compatible(self, instruction_name):
+        return True
+
+
+    """
+    配置电机
+    """
+    def configure_motors(self, return_delay_time=0, maximum_acceleration=254, acceleration=254) -> None:
+        pass
+
+    def disable_torque(self, motors: int | str | list[str] | None = None, num_retry: int = 0):
+        pass
+
+    def _disable_torque(self, motor: int, model: str, num_retry: int = 0):
+        pass
+
+    def enable_torque(self, motors = None, num_retry = 0):
+        pass

robot_client/robots/realman/robot.py

@@ -21,6 +21,8 @@ class RealmanRobot(Robot):
             motors=config.motors,
             joint=config.joint,
             mock=self.config.mock,
+            udp_ip=self.config.udp_ip,
+            udp_port=self.config.udp_port,
             gripper_range=config.gripper_range,
             calibration=self.calibration,
         )

robot_client/robots/realman/robot_mock.py

@@ -0,0 +1,29 @@
+class RealmanMock:
+    def __init__(self):
+        self.joint = [0.0]*6
+        self.pose = [0.0]*6
+        self.gripper = 0
+        pass
+    def rm_create_robot_arm(self, ip: str, port: int, level: int = 3, log_func = None) -> int:
+        return 1
+    def rm_movej(self, joint: list[float], v: int, r: int, connect: int, block: int) -> int:
+        self.joint = joint
+    def rm_get_current_arm_state(self) -> tuple[int, dict[str, any]]:
+        return (0, {"joint": self.joint, "pose": self.pose})
+    
+    def rm_get_gripper_state(self) -> tuple[int, dict[str, any]]:
+        return (0, {"actpos": self.gripper})
+    
+    def rm_movep_canfd(self, pose: list[float], follow: bool, trajectory_mode: int = 0, radio: int = 0) -> int:
+        self.pose = pose
+        return 0
+    def rm_set_gripper_position(self, position: int, block: bool, timeout: int) -> int:
+        self.gripper = position
+        return 0
+    
+    def rm_movej(self, joint: list[float], v: int, r: int, connect: int, block: int) -> int:
+        self.joint = joint
+        return 0
+    
+    def rm_set_arm_run_mode(self, mode: int) ->int:
+        return 0

robot_client/teleoperators/flight_stick/flight_stick.py

@@ -9,7 +9,6 @@ class FlightStick(Xbox):
     name = "flight_stick"
 
     def get_action(self):
-        pygame.event.pump()
         pose_speeds = {}
         """更新末端位姿控制"""
         # 根据控制模式调整步长
@@ -21,7 +20,7 @@ class FlightStick(Xbox):
                 self.fine_control_mode = False
         
         # print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}")
-        print(f"精细控制模式: {self.fine_control_mode}")
+        # 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)
@@ -35,28 +34,28 @@ class FlightStick(Xbox):
         
         # print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}")
         if self.joystick.get_button(8):  # X按钮
-            pose_speeds['z.vel'] = current_linear_step
+            pose_speeds['z.vel'] = current_linear_step/3
         elif self.joystick.get_button(9):  # Y按钮
-            pose_speeds['z.vel'] = -current_linear_step
+            pose_speeds['z.vel'] = -current_linear_step/3
         else:
             pose_speeds['z.vel'] = 0
 
         
         # 主摇杆X轴控制RX旋转
         # 设置RX旋转速度
-        rx_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(1))
+        rx_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(0))
         # print(f"RX轴非线性映射: {x_axis} -> {rx_mapping}")
         pose_speeds['rx.vel'] = rx_mapping * current_angular_step * 2  # RX
         
         # 主摇杆Y轴控制RY旋转
         # 设置RY旋转速度
-        ry_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(0))
+        ry_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(1))
         # print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}")
         pose_speeds['ry.vel'] = ry_mapping * current_angular_step * 2  # RY
     
         # 主摇杆左右旋转轴 控制RZ旋转
         # 设置RZ旋转速度
-        rz_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(3))
+        rz_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(3))
         # print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}")
         pose_speeds['rz.vel'] = rz_mapping * current_angular_step * 2  # RZ
         if self.joystick.get_button(0):  # X按钮
@@ -65,5 +64,5 @@ class FlightStick(Xbox):
             pose_speeds['gripper.vel'] = -current_linear_step
         else:
             pose_speeds['gripper.vel'] = 0
-        print(f"pose_speeds: {pose_speeds}")
+        # print(f"pose_speeds: {pose_speeds}")
         return pose_speeds

robot_client/teleoperators/mix/__init__.py

@@ -0,0 +1,2 @@
+from .mix import Mix,MixConfig
+__all__ = ["Mix", "MixConfig"]

robot_client/teleoperators/mix/config.py

@@ -0,0 +1,6 @@
+from dataclasses import dataclass, field
+from lerobot.teleoperators import TeleoperatorConfig
+@dataclass
+@TeleoperatorConfig.register_subclass("mix")
+class MixConfig(TeleoperatorConfig):
+    teleopList: dict[str,TeleoperatorConfig] = field(default_factory=dict[str,TeleoperatorConfig])

robot_client/teleoperators/mix/mix.py

@@ -0,0 +1,64 @@
+import queue
+import threading
+import pygame
+import time
+import sys
+from  lerobot.teleoperators import Teleoperator,make_teleoperator_from_config
+from .config import MixConfig
+"""
+混合控制器。
+"""
+class Mix(Teleoperator):
+    config_class = MixConfig
+    name = "mix"
+    def __init__(self, config: MixConfig):
+        self.config = config
+        self.teleopList = {}
+        for name,teleop in self.config.teleopList.items():
+            self.teleopList[name] = make_teleoperator_from_config(teleop)
+    @property
+    def action_features(self):
+        action_features = {}
+        for name,teleop in self.teleopList.items():
+            for i,item in enumerate(teleop.action_features()["names"].keys()):
+                action_features[f"{name}.{item}"] = i
+        return {
+            "dtype": "float32",
+            "shape": (len(action_features),),
+            "names": action_features,
+        }
+
+    @property
+    def feedback_features(self):
+        return {}
+
+    @property
+    def is_connected(self):
+        return all([teleop.is_connected for teleop in self.teleopList.values()])
+
+    def connect(self, calibrate = True):
+        for teleop in self.teleopList.values():
+            teleop.connect(calibrate)
+    @property
+    def is_calibrated(self):
+        return True
+
+    def calibrate(self):
+        return all([teleop.calibrate() for teleop in self.teleopList.values()])
+
+    def configure(self):
+        pass
+
+    def get_action(self):
+        data = {}
+        for index,teleop in self.teleopList.items():
+            for key,item in teleop.get_action().items():
+                data[f"{index}.{key}"] = item
+        return data
+
+    def send_feedback(self, feedback):
+        pass
+
+    def disconnect(self):
+        for teleop in self.teleopList.values():
+            teleop.disconnect()

robot_client/teleoperators/xbox/xbox.py

@@ -1,8 +1,11 @@
+import queue
+import threading
 import pygame
 import time
 import sys
 from  lerobot.teleoperators import Teleoperator
 from .config import XboxConfig
+from ...utlis.pygame_event import PygameEvent
 """
 Xbox类用于检测和监控飞行手柄。
 """
@@ -21,6 +24,8 @@ class Xbox(Teleoperator):
         self.fine_control_mode = False
         self.reset()
     def reset(self):
+        self.running = False
+        self.action_data ={"x.vel":0, "y.vel":0, "z.vel":0, "rx.vel.vel":0, "ry.vel":0, "rz.vel":0, "gripper.vel":0}
         self.joystick = None
     @property
     def action_features(self):
@@ -37,8 +42,8 @@ class Xbox(Teleoperator):
     @property
     def is_connected(self):
         return self.joystick is not None
-
-    def connect(self, calibrate = True):
+    
+    def update(self,q:queue.Queue):
         # 初始化pygame
         pygame.init()
         pygame.joystick.init()
@@ -47,16 +52,23 @@ class Xbox(Teleoperator):
         print(f"检测到 {joystick_count} 个手柄设备")
         
         if joystick_count == 0:
-            print("未检测到任何手柄设备！")
+            q.put((False,"未检测到任何手柄设备！"))
             return False
 
         if joystick_count<self.config.index+1:
-            print(f"未检测到配置中指定的索引设备！") 
+            q.put((False,"未检测到配置中指定的索引设备！"))
             return False
         # 初始化所有检测到的手柄
         self.joystick = pygame.joystick.Joystick(self.config.index)
-        print(f"已连接设备：{self.joystick.get_name()}")
+        q.put((True,f"已连接设备：{self.joystick.get_name()}"))
+        while self.running:
+            self.action_data = self.update_action()
+        pygame.joystick.quit()
+        pygame.quit()
 
+    def connect(self, calibrate = True):
+        PygameEvent.start()
+        self.joystick = PygameEvent.getState(self.config.index)
     @property
     def is_calibrated(self):
         return True
@@ -68,7 +80,6 @@ class Xbox(Teleoperator):
         pass
 
     def get_action(self):
-        pygame.event.pump()
         pose_speeds = {}
         """更新末端位姿控制"""
         # 根据控制模式调整步长
@@ -80,7 +91,7 @@ class Xbox(Teleoperator):
                 self.fine_control_mode = False
         
         # print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}")
-        print(f"精细控制模式: {self.fine_control_mode}")
+        # print(f"精细控制模式: {self.fine_control_mode}")
         
         # 方向键控制XY
         hat = self.joystick.get_hat(0)
@@ -95,19 +106,19 @@ class Xbox(Teleoperator):
 
         # 左右扳机控制z轴
         right_tiger = ((self.joystick.get_axis(5)+1)/2)
-        left_tiger = ((self.joystick.get_axis(4)+1)/2)
+        left_tiger = ((self.joystick.get_axis(2)+1)/2)
         z_mapping = self.apply_nonlinear_mapping(left_tiger - right_tiger)
-        pose_speeds['z.vel'] = z_mapping * current_angular_step * 2
+        pose_speeds['z.vel'] = z_mapping * current_linear_step
 
         #右摇杆X轴控制RX旋转
         # 设置RX旋转速度
-        rx_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(2))
+        rx_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(3))
         # print(f"RX轴非线性映射: {x_axis} -> {rx_mapping}")
         pose_speeds['rx.vel'] = rx_mapping * current_angular_step * 2  # RX
 
         # 右摇杆Y轴控制RY旋转
         # 设置RY旋转速度
-        ry_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(3))
+        ry_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(4))
         # print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}")
         pose_speeds['ry.vel'] = ry_mapping * current_angular_step * 2  # RY
     
@@ -122,9 +133,64 @@ class Xbox(Teleoperator):
         gripper_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(1))
         # print(f"gripper轴非线性映射: {self.joystick.get_axis(0)} -> {gripper_mapping}")
         pose_speeds['gripper.vel'] = gripper_mapping * current_angular_step * 2  # RY
-        print(f"pose_speeds: {pose_speeds}")
-        
         return pose_speeds
+
+    # def update_action(self):
+    #     pygame.event.pump()
+    #     pose_speeds = {}
+    #     """更新末端位姿控制"""
+    #     # 根据控制模式调整步长
+    #     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['x.vel'] = current_linear_step if hat_left else (-current_linear_step if hat_right else 0.0)  # X
+    #     pose_speeds['y.vel'] = -current_linear_step if hat_up else (current_linear_step if hat_down else 0.0)  # Y
+
+    #     # 左右扳机控制z轴
+    #     right_tiger = ((self.joystick.get_axis(5)+1)/2)
+    #     left_tiger = ((self.joystick.get_axis(2)+1)/2)
+    #     z_mapping = self.apply_nonlinear_mapping(left_tiger - right_tiger)
+    #     pose_speeds['z.vel'] = z_mapping * current_angular_step * 2
+
+    #     #右摇杆X轴控制RX旋转
+    #     # 设置RX旋转速度
+    #     rx_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(3))
+    #     # print(f"RX轴非线性映射: {x_axis} -> {rx_mapping}")
+    #     pose_speeds['rx.vel'] = rx_mapping * current_angular_step * 2  # RX
+
+    #     # 右摇杆Y轴控制RY旋转
+    #     # 设置RY旋转速度
+    #     ry_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(4))
+    #     # print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}")
+    #     pose_speeds['ry.vel'] = ry_mapping * current_angular_step * 2  # RY
+    
+    #     # 左摇杆X轴RZ旋转
+    #     # 设置RZ旋转速度
+    #     rz_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(0))
+    #     # print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}")
+    #     pose_speeds['rz.vel'] = rz_mapping * current_angular_step * 2  # RZ
+
+    #     # 左摇杆Y轴控制夹爪
+    #     # 设置夹爪开合速度
+    #     gripper_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(1))
+    #     # print(f"gripper轴非线性映射: {self.joystick.get_axis(0)} -> {gripper_mapping}")
+    #     pose_speeds['gripper.vel'] = gripper_mapping * current_angular_step * 2  # RY
+    #     return pose_speeds
     
     def apply_nonlinear_mapping(self, value):
         """应用非线性映射以提高控制精度"""
@@ -141,5 +207,3 @@ class Xbox(Teleoperator):
 
     def disconnect(self):
         self.reset()
-        pygame.joystick.quit()
-        pygame.quit()

robot_client/teleoperators/xr/__init__.py

@@ -0,0 +1,6 @@
+import ctypes
+import platform
+if platform.system() == 'Linux':
+    ctypes.CDLL("/opt/apps/roboticsservice/SDK/x64/libPXREARobotSDK.so")
+from .xr import Xr,XrConfig
+__all__ = ["Xr", "XrConfig"]

robot_client/teleoperators/xr/config.py

@@ -0,0 +1,6 @@
+from dataclasses import dataclass
+from lerobot.teleoperators import TeleoperatorConfig
+@dataclass
+@TeleoperatorConfig.register_subclass("xr")
+class XrConfig(TeleoperatorConfig):
+    pass

robot_client/teleoperators/xr/xr.py

@@ -0,0 +1,213 @@
+import threading
+import numpy as np
+import xrobotoolkit_sdk as xrt
+import time
+import sys
+from  lerobot.teleoperators import Teleoperator
+from .config import XrConfig
+from scipy.spatial.transform import Rotation as R
+import meshcat.transformations as tf
+"""
+Xbox类用于检测和监控飞行手柄。
+"""
+class Xr(Teleoperator):
+    config_class = XrConfig
+    name = "xr"
+    def __init__(self, config: XrConfig):
+        
+        self.R_HEADSET_TO_WORLD = np.array(
+            [
+                [-1, 0, 0],
+                [0, 0, 1],
+                [0, 1, 0],
+            ]
+        )
+        self.R_HEADSET_TO_WORLD2 = np.array(
+            [
+                [1, 0, 0],
+                [0, 1, 0],
+                [0, 0, 1],
+            ]
+        )
+        self.config = config
+        # 控制参数
+        self.linear_step = 0.0015  # 线性移动步长(m)
+        self.current_linear_step = self.linear_step
+        self.angular_step = 0.0003  # 角度步长(rad) - 从度转换为弧度
+        self.current_angular_step = self.angular_step
+        # 摇杆死区
+        self.deadzone = 0.15
+        
+        # 精细控制模式
+        self.fine_control_mode = False
+        self.running = False
+        self.data = {
+            "left": {
+                "pose": np.zeros(7),
+                "init_pose":None,
+                "trigger":0.0,
+                "axis":[0.0]*2,
+                "grip":0.0,
+                "axis_button":0.0
+            },
+            "right": {
+                "pose": np.zeros(7),
+                "init_pose":None,
+                "trigger":0.0,
+                "grip":0.0,
+                "axis":[0.0]*2,
+                "axis_button":0.0
+            }
+        }
+        self.scale_factor = 0.2
+        self.x = 0.0
+        self.y = 0.0
+        self.a = 0.0
+        self.b = 0.0
+    @property
+    def action_features(self):
+        return {
+            "dtype": "float32",
+            "shape": (7,),
+            "names": {"x.vel":0, "y.vel":1, "z.vel":2, "rx.vel.vel":3, "ry.vel":4, "rz.vel":5, "rw.vel":6, "gripper.vel":7},
+        }
+
+    @property
+    def feedback_features(self):
+        return {}
+
+    @property
+    def is_connected(self):
+        return self.joystick is not None
+    
+    def quaternion2euler(self,quaternion:list):
+        r = R.from_quat(quaternion)
+        euler = r.as_euler('xyz', degrees=True)
+        return euler
+
+    def update(self):
+        while self.running:
+            self.a = xrt.get_A_button()
+            self.b = xrt.get_B_button()
+            self.x = xrt.get_X_button()
+            self.y = xrt.get_Y_button()
+            self.data['left']['pose'] = xrt.get_left_controller_pose()
+            self.data['left']['trigger'] = xrt.get_left_trigger()
+            self.data['left']['grip'] = xrt.get_left_grip()
+            self.data['left']['axis'] = xrt.get_left_axis()
+            self.data['left']['axis_button'] = xrt.get_left_axis_click()
+            self.data['right']['pose'] = xrt.get_right_controller_pose()
+            self.data['right']['trigger'] = xrt.get_right_trigger()
+            self.data['right']['grip'] = xrt.get_right_grip()
+            self.data['right']['axis'] = xrt.get_right_axis()
+            self.data['right']['axis_button'] = xrt.get_right_axis_click()
+            time.sleep(0.01)
+    def connect(self, calibrate = True):
+        # 初始化vr
+        xrt.init()
+        # 设置超时时间（秒）
+        timeout = 5
+        # 记录进入循环前的时间
+        start_time = time.time()
+        elapsed_time_flag = False
+        print("等待vr初始化完成")
+        while True:
+            elapsed_time = time.time() - start_time
+            right_pose = xrt.get_right_controller_pose()
+            left_pose = xrt.get_left_controller_pose()
+            if not all(num == 0 for num in right_pose) and not all(num == 0 for num in left_pose):
+                print(f"connect success")
+                self.running = True
+                threading.Thread(target=self.update).start()
+                break
+            # 检查是否超过超时时间
+            if elapsed_time > timeout:
+                print("超时....")
+                elapsed_time_flag = True
+                break
+            time.sleep(1)
+        return not elapsed_time_flag
+    @property
+    def is_calibrated(self):
+        return True
+
+    def calibrate(self):
+        pass
+
+    def configure(self):
+        pass
+
+    def get_pose(self, prefix,pose_speeds):
+        data = self.data[prefix]
+        xr_pose = np.array(data['pose'])
+        """Process the current XR controller pose."""
+        # Get position and orientation
+        controller_xyz = np.array([xr_pose[0], xr_pose[1], xr_pose[2]])
+        controller_xyz = self.R_HEADSET_TO_WORLD @ controller_xyz
+        controller_quat = R.from_quat(xr_pose[3:])
+        if data['trigger'] >0 or data['grip']>0:
+            isLock = data['grip']>0
+            if data['init_pose'] is None:
+                data['init_pose'] = controller_quat
+                data['init_position'] = controller_xyz
+        else:
+            data['init_pose'] = None
+
+        if data['init_pose'] is None:
+            delta_xyz = np.zeros(3)
+            delta_rot = np.array([0.0, 0.0, 0.0])
+        else:
+            delta_xyz =  (controller_xyz - data['init_position']) * self.scale_factor
+            delta_rot = self.quat_diff_as_angle_axis(controller_quat,data['init_pose'])
+            data['init_pose'] = controller_quat
+            data['init_position'] = controller_xyz
+            if isLock :
+                delta_rot = [0.0,0.0,0.0]
+            else:
+                delta_rot *= self.scale_factor
+                delta_rot = self.R_HEADSET_TO_WORLD2 @ delta_rot
+                print(f"{prefix}:{delta_rot}")
+        pose_speeds[f'{prefix}.x.vel'] = delta_xyz[0]
+        pose_speeds[f'{prefix}.y.vel'] = delta_xyz[1]
+        pose_speeds[f'{prefix}.z.vel'] = delta_xyz[2]
+        pose_speeds[f'{prefix}.rx.vel'] = delta_rot[2]
+        pose_speeds[f'{prefix}.ry.vel'] = delta_rot[1]
+        pose_speeds[f'{prefix}.rz.vel'] = -delta_rot[0]
+
+        # 设置夹爪开合速度
+        gripper_mapping = -self.apply_nonlinear_mapping(-data['axis'][0])
+        pose_speeds[f'{prefix}.gripper.vel'] = gripper_mapping * self.current_angular_step * 2
+
+    def quat_diff_as_angle_axis(self,q1: R, q2: R, eps: float = 1e-6) -> np.ndarray:
+        delta_q = q2 * q1.inv()
+        rotvec = delta_q.as_rotvec()
+        angle = np.linalg.norm(rotvec)
+        if angle < eps:
+            return np.zeros(3, dtype=np.float64)
+        return rotvec
+    def get_action(self):
+        pose_speeds = {}
+        """更新末端位姿控制"""
+        # 根据控制模式调整步长
+        if self.a:  # A按钮
+                self.fine_control_mode = True
+                self.scale_factor = 0.07
+        if self.b:  # B按钮
+                self.fine_control_mode = False
+                self.scale_factor = 0.2
+        self.get_pose('left',pose_speeds)
+        self.get_pose('right',pose_speeds)
+        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 send_feedback(self, feedback):
+        pass
+
+    def disconnect(self):
+        self.running = False

robot_client/utlis/pygame_event.py

@@ -0,0 +1,81 @@
+import threading
+import pygame
+import atexit
+
+class PygameEventState(object):
+    def __init__(self,joystick):
+        joystick.init()
+        self.states={
+            'axis':[0]*joystick.get_numaxes(),
+            'button':[0]*joystick.get_numbuttons(),
+            'hat':[[0,0]]*joystick.get_numhats()
+        }
+    def get_axis(self,index):
+        return self.states['axis'][index]
+    def get_numaxes(self):
+        return len(self.states['axis'])
+    def set_axis(self,index,value):
+        self.states['axis'][index]=value
+    
+    def get_button(self,index):
+        return self.states['button'][index]
+    def get_numbuttons(self):
+        return len(self.states['button'])
+    def set_button(self,index,value):
+        self.states['button'][index]=value
+    def get_hat(self,index):
+        return self.states['hat'][index]
+    def get_numhats(self):
+        return len(self.states['hat'])
+    def set_hat(self,index,value):
+        self.states['hat'][index]=value
+            
+
+class _PygameEvent:
+    def __init__(self):
+        self.running = False
+
+    def start(self):
+        if self.running: return
+        self.running = True
+        threading.Thread(target=self.eventLoop).start()
+
+    def getState(self,index):
+        if index >= len(self.states):
+            raise ValueError("不存在的控制器")
+        return self.states[index]
+    def eventLoop(self):
+        # 初始化pygame
+        pygame.init()
+        pygame.joystick.init()
+        """检测连接的手柄"""
+        joystick_count = pygame.joystick.get_count()
+        self.states=[]
+        joysticks = []
+        for i in range(joystick_count):
+            joystick = pygame.joystick.Joystick(i)
+            joystick.init()
+            self.states.append(PygameEventState(joystick))
+            joysticks.append(joystick)
+
+        print(f"检测到 {joystick_count} 个手柄设备")
+        while self.running:
+            event = pygame.event.wait()
+            data = event.dict
+            if 'instance_id' not in data:
+                continue
+            state = self.states[data['instance_id']]
+            if event.type == pygame.JOYAXISMOTION:
+                state.set_axis(data['axis'], data['value'])
+            elif event.type == pygame.JOYHATMOTION:
+                state.set_hat(data['hat'], data['value'])
+            elif event.type == pygame.JOYBUTTONUP:
+                state.set_button(data['button'], 0)
+            elif event.type == pygame.JOYBUTTONDOWN:
+                state.set_button(data['button'], 1)
+    def stop(self):
+        self.running = False
+@atexit.register
+def exit():
+    PygameEvent.stop()
+PygameEvent = _PygameEvent()