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