single arm refactory

lerobot/common/robot_devices/motors/realman.py

@@ -15,6 +15,10 @@ class RealmanMotorsBus:
         self.motors = config.motors
         self.init_joint_position = config.init_joint['joint'] # [6 joints + 1 gripper]
         self.safe_disable_position = config.init_joint['joint']
+        self.rmarm.rm_movej(self.init_joint_position[:-1], 5, 0, 0, 1)
+        time.sleep(3)
+        ret = self.rmarm.rm_get_current_arm_state()
+        self.init_pose = ret[1]['pose']
 
     @property
     def motor_names(self) -> list[str]:
@@ -98,6 +102,18 @@ class RealmanMotorsBus:
         self.rmarm.rm_movej_p(target_endpose, 50, 0, 0, 1)
         self.rmarm.rm_set_gripper_position(gripper, block=False, timeout=2)
 
+    def write_joint_slow(self, target_joint: list):
+        self.rmarm.rm_movej(target_joint, 5, 0, 0, 0)
+
+    def write_joint_canfd(self, target_joint: list):
+        self.rmarm.rm_movej_canfd(target_joint, False)
+
+    def write_endpose_canfd(self, target_pose: list):
+        self.rmarm.rm_movep_canfd(target_pose, False)
+
+    def write_gripper(self, gripper: int):
+        self.rmarm.rm_set_gripper_position(gripper, False, 2)
+
     def read(self) -> Dict:
         """
             - 机械臂关节消息,单位1度;[-1, 1]
@@ -119,6 +135,12 @@ class RealmanMotorsBus:
             "gripper": (gripper_state-500)/500
         }
     
+    def read_current_arm_joint_state(self):
+        return self.rmarm.rm_get_current_arm_state()[1]['joint']
+    
+    def read_current_arm_endpose_state(self):
+        return self.rmarm.rm_get_current_arm_state()[1]['pose']
+
     def safe_disconnect(self):
         """ 
             Move to safe disconnect position

lerobot/common/robot_devices/robots/configs.py

@@ -683,7 +683,7 @@ class RealmanRobotConfig(RobotConfig):
     inference_time: bool = False
     max_gripper: int = 990
     min_gripper: int = 10
-    servo_config_file: str = "/home/maic/LYT/lerobot/realman_src/realman_aloha/shadow_rm_robot/config/servo_arm.yaml"
+    servo_config_file: str = "/home/maic/LYT/lerobot/lerobot/common/robot_devices/teleop/servo_arm.yaml"
     
 
     left_follower_arm: dict[str, MotorsBusConfig] = field(
@@ -701,7 +701,7 @@ class RealmanRobotConfig(RobotConfig):
                     "joint_6": [6, "realman"],
                     "gripper": [7, "realman"],
                 },
-                init_joint = {'joint': [90, -90, -90, 90, -90, 90, 10]}
+                init_joint = {'joint': [-90, 90, 90, 90, 90, -90, 10]}
             )
         }
     )

lerobot/common/robot_devices/robots/realman.py

@@ -14,45 +14,6 @@ from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError,
 from lerobot.common.robot_devices.robots.configs import RealmanRobotConfig
 
 
-def smooth_movep_canfd(arm_controller, current_pose, target_pose, fps=100):
-    """
-    可变帧率版本
-    
-    Args:
-        fps: 目标帧率，决定每次移动的时长
-    """
-    total_duration = 1.0 / fps
-    control_dt = 0.002  # 2ms控制周期
-    num_points = max(1, int(total_duration / control_dt))
-    
-    print(f"{fps}fps移动: {total_duration*1000:.1f}ms, {num_points}个插值点")
-    
-    # 生成轨迹
-    trajectory = []
-    for i in range(num_points):
-        t = i / (num_points - 1) if num_points > 1 else 1.0
-        s = 6 * t**5 - 15 * t**4 + 10 * t**3  # S曲线
-        
-        pose = []
-        for axis in range(6):
-            value = current_pose[axis] + s * (target_pose[axis] - current_pose[axis])
-            pose.append(value)
-        trajectory.append(pose)
-    
-    # 执行
-    start_time = time.perf_counter()
-    for i, pose in enumerate(trajectory):
-        target_time = start_time + i * control_dt
-        current_time = time.perf_counter()
-        
-        sleep_time = target_time - current_time
-        # if sleep_time > 0:
-            # time.sleep(sleep_time)
-        
-        arm_controller.rmarm.rm_movep_canfd(pose, True)
-    
-    return True
-
 class RealmanRobot:
     def __init__(self, config: RealmanRobotConfig | None = None, **kwargs):
         if config is None:
@@ -68,14 +29,11 @@ class RealmanRobot:
         # build realman motors
         self.piper_motors = make_motors_buses_from_configs(self.config.left_follower_arm)
         self.arm = self.piper_motors['main']
-        self.arm.rmarm.rm_movej(self.arm.init_joint_position[:-1], 50, 0, 0, 1)
-        time.sleep(2)
-        ret = self.arm.rmarm.rm_get_current_arm_state()
-        init_pose = ret[1]['pose']
+        
         # build init teleop info
         self.init_info = {
             'init_joint': self.arm.init_joint_position,
-            'init_pose': init_pose,
+            'init_pose': self.arm.init_pose,
             'max_gripper': config.max_gripper,
             'min_gripper': config.min_gripper,
             'servo_config_file': config.servo_config_file
@@ -83,7 +41,7 @@ class RealmanRobot:
         
         # build state-action cache
         self.joint_queue = deque(maxlen=2)
-        self.last_endpose = init_pose
+        self.last_endpose = self.arm.init_pose
 
         # build gamepad teleop
         if not self.inference_time:
@@ -102,7 +60,7 @@ class RealmanRobot:
             cam_ft[key] = {
                 "shape": (cam.height, cam.width, cam.channels),
                 "names": ["height", "width", "channels"],
-                "info": None,   
+                "info": None,
             }
         return cam_ft
 
@@ -203,53 +161,37 @@ class RealmanRobot:
 
         if action['control_mode'] == 'joint':
             # 关节控制模式（主模式）
-            ret = self.arm.rmarm.rm_get_current_arm_state()
-            current_pose = ret[1]['pose']
+            current_pose = self.arm.read_current_arm_endpose_state()
             self.teleop.update_endpose_state(current_pose)
-            target_joints = action['joint_angles']
 
+            target_joints = action['joint_angles'][:-1]
+            self.arm.write_gripper(action['gripper'])
+            print(action['gripper'])
+            if action['master_controller_status']['infrared'] == 1:
+                if action['master_controller_status']['button'] == 1:
+                    self.arm.write_joint_canfd(target_joints)
+                else:
+                    self.arm.write_joint_slow(target_joints)
+                
             # do action
             before_write_t = time.perf_counter()
             self.joint_queue.append(list(self.arm.read().values()))
             self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
 
         else:
-            # 末端位姿控制模式
-            target_pose = [
-                action['end_pose']['X'],    # X (m)
-                action['end_pose']['Y'],    # Y (m) 
-                action['end_pose']['Z'],    # Z (m)
-                action['end_pose']['RX'],   # RX (rad)
-                action['end_pose']['RY'],   # RY (rad)
-                action['end_pose']['RZ']    # RZ (rad)
-            ]
-
+            target_pose = list(action['end_pose'])
             # do action
             before_write_t = time.perf_counter()
-            
-            # result = self.arm.rmarm.rm_movej_p(target_pose, 100, 0, 0, 0)
             if self.last_endpose != target_pose:
-                # self.arm.rmarm.rm_movep_follow(target_pose)
-                # self.arm.rmarm.rm_movep_canfd(target_pose, True)
-                self.arm.rmarm.rm_movep_canfd(target_pose, False)
+                self.arm.write_endpose_canfd(target_pose)
                 self.last_endpose = target_pose
-            self.arm.rmarm.rm_set_gripper_position(action['gripper'], False, 2)
+            self.arm.write_gripper(action['gripper'])
             
-            ret = self.arm.rmarm.rm_get_current_arm_state()
-            target_joints = ret[1].get('joint', self.arm.init_joint_position)
+            target_joints = self.arm.read_current_arm_joint_state()
             self.joint_queue.append(list(self.arm.read().values()))
             self.teleop.update_joint_state(target_joints)
             self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t
         
-        # print('-'*80)
-        # print('mode: ', action['control_mode'])
-        # print('state: ', list(state.values()))
-        # print('action: ', target_joints)
-        # print('cache[0]: ', self.joint_queue[0])
-        # print('cache[-1]: ', self.joint_queue[-1])
-        # print('time: ', time.perf_counter() - before_write_t)
-        # print('-'*80)
-        # time.sleep(1)
         if not record_data:
             return
         

lerobot/common/robot_devices/teleop/gamepad.py

@@ -20,12 +20,13 @@ class ServoArm:
         self.config = self._load_config(config_file)
         self.port = self.config["port"]
         self.baudrate = self.config["baudrate"]
-        self.hex_data = self.config["hex_data"]
+        self.joint_hex_data = self.config["joint_hex_data"]
+        self.control_hex_data = self.config["control_hex_data"]
         self.arm_axis = self.config.get("arm_axis", 7)
 
         try:
             self.serial_conn = serial.Serial(self.port, self.baudrate, timeout=0)
-            self.bytes_to_send = binascii.unhexlify(self.hex_data.replace(" ", ""))
+            self.bytes_to_send = binascii.unhexlify(self.joint_hex_data.replace(" ", ""))
             self.serial_conn.write(self.bytes_to_send)
             time.sleep(1)
             self.connected = True
@@ -53,7 +54,8 @@ class ServoArm:
             return {
                 "port": "/dev/ttyUSB0",
                 "baudrate": 460800,
-                "hex_data": "55 AA 02 00 00 67",
+                "joint_hex_data": "55 AA 02 00 00 67",
+                "control_hex_data": "55 AA 08 00 00 B9",
                 "arm_axis": 6
             }
 
@@ -94,6 +96,17 @@ class ServoArm:
 
         joints["grasp"] = grasp_value
         return joints
+    
+    def _parse_controller_data(self, hex_received):
+        status = {
+            'infrared': 0,
+            'button': 0
+        }
+        if len(hex_received) == 18:
+            status['infrared'] = self._bytes_to_signed_int(bytearray.fromhex(hex_received[12:14]))
+            status['button'] = self._bytes_to_signed_int(bytearray.fromhex(hex_received[14:16]))
+            # print(infrared)
+        return status
 
     def get_joint_actions(self):
         """从串口读取数据并解析关节动作。
@@ -106,6 +119,7 @@ class ServoArm:
         
         try:
             self.serial_conn.write(self.bytes_to_send)
+            time.sleep(0.02)
             bytes_received = self.serial_conn.read(self.serial_conn.inWaiting())
             if len(bytes_received) == 0:
                 return {}
@@ -117,20 +131,15 @@ class ServoArm:
             logging.error(f"读取串口数据错误: {e}")
             return {}
 
-    def set_gripper_action(self, action):
-        """设置夹爪动作。
-        Args:
-            action (int): 夹爪动作值。
-        """
-        if not self.connected:
-            return
-        
-        try:
-            action = int(action * 1000)
-            action_bytes = action.to_bytes(4, byteorder="little", signed=True)
-            self.bytes_to_send = self.bytes_to_send[:74] + action_bytes + self.bytes_to_send[78:]
-        except Exception as e:
-            logging.error(f"设置夹爪动作错误: {e}")
+    def get_controller_status(self):
+        bytes_to_send = binascii.unhexlify(self.control_hex_data.replace(" ", ""))
+        self.serial_conn.write(bytes_to_send)
+        time.sleep(0.02)
+        bytes_received = self.serial_conn.read(self.serial_conn.inWaiting())
+        hex_received = binascii.hexlify(bytes_received).decode("utf-8").upper()
+        # print("control status:", hex_received)
+        status = self._parse_controller_data(hex_received)
+        return status
 
     def close(self):
         """关闭串口连接"""
@@ -174,6 +183,7 @@ class HybridController:
         
         # 主臂关节状态
         self.master_joint_actions = {}
+        self.master_controller_status = {}
         self.use_master_arm = False
         
         # 末端位姿限制
@@ -282,6 +292,7 @@ class HybridController:
             if self.servo_arm and self.servo_arm.connected:
                 try:
                     self.master_joint_actions = self.servo_arm.get_joint_actions()
+                    self.master_controller_status = self.servo_arm.get_controller_status()
                     if self.master_joint_actions:
                         logging.debug(f"主臂关节状态: {self.master_joint_actions}")
                     
@@ -406,14 +417,8 @@ class HybridController:
             'control_mode': 'joint' if self.joint_control_mode else 'end_pose',
             'use_master_arm': self.use_master_arm,
             'master_joint_actions': self.master_joint_actions,
-            'end_pose': {
-                'X': self.pose[0],
-                'Y': self.pose[1],
-                'Z': self.pose[2],
-                'RX': self.pose[3],
-                'RY': self.pose[4],
-                'RZ': self.pose[5],
-            },
+            'master_controller_status': self.master_controller_status,
+            'end_pose': self.pose,
             'joint_angles': self.joint,
             'gripper': int(self.gripper),
             'tozero': self.tozero

lerobot/common/robot_devices/teleop/servo_arm.yaml

@@ -1,5 +1,6 @@
 port: /dev/ttyUSB0
 right_port: /dev/ttyUSB1
 baudrate: 460800
-hex_data: "55 AA 02 00 00 67"
+joint_hex_data: "55 AA 02 00 00 67"
+control_hex_data: "55 AA 08 00 00 B9"
 arm_axis: 6