single arm

lerobot/common/robot_devices/robots/configs.py

@@ -683,7 +683,8 @@ 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"
+    axis: int = 6
+    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 +702,27 @@ 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]}
+            )
+        }
+    )
+
+    right_follower_arm: dict[str, MotorsBusConfig] = field(
+        default_factory=lambda: {
+            "main": RealmanMotorsBusConfig(
+                ip = "192.168.3.19",
+                port = 8080,
+                motors={
+                    # name: (index, model)
+                    "joint_1": [1, "realman"],
+                    "joint_2": [2, "realman"],
+                    "joint_3": [3, "realman"],
+                    "joint_4": [4, "realman"],
+                    "joint_5": [5, "realman"],
+                    "joint_6": [6, "realman"],
+                    "gripper": [7, "realman"],
+                },
+                init_joint = {'joint': [-90, 90, 90, 90, 90, -90, 10]}
             )
         }
     )
@@ -721,13 +742,13 @@ class RealmanRobotConfig(RobotConfig):
                 height=480,
                 use_depth=False
             ),
-            # "right": IntelRealSenseCameraConfig(
-            #     serial_number="405622075165",
-            #     fps=30,
-            #     width=640,
-            #     height=480,
-            #     use_depth=False
-            # ),
+            "right": IntelRealSenseCameraConfig(
+                serial_number="405622075165",
+                fps=30,
+                width=640,
+                height=480,
+                use_depth=False
+            ),
             "front": IntelRealSenseCameraConfig(
                 serial_number="145422072751",
                 fps=30,

lerobot/common/robot_devices/robots/realman.py

@@ -206,8 +206,13 @@ class RealmanRobot:
             ret = self.arm.rmarm.rm_get_current_arm_state()
             current_pose = ret[1]['pose']
             self.teleop.update_endpose_state(current_pose)
-            target_joints = action['joint_angles']
-
+            target_joints = action['joint_angles'][:-1]
+            if action['master_controller_status']['infrared'] == 1:
+                if action['master_controller_status']['button'] == 1:
+                    self.arm.rmarm.rm_movej_canfd(target_joints, False)
+                else:
+                    self.arm.rmarm.rm_movej(target_joints, 5, 0, 0, 0)
+                
             # do action
             before_write_t = time.perf_counter()
             self.joint_queue.append(list(self.arm.read().values()))
@@ -215,14 +220,15 @@ class RealmanRobot:
 
         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 = [
+            #     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 = action['end_pose']
 
             # do action
             before_write_t = time.perf_counter()

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}")
                     
@@ -324,7 +335,7 @@ class HybridController:
         
         # 如果有主臂夹爪数据，使用主臂夹爪状态
         if self.use_master_arm and "grasp" in self.master_joint_actions:
-            self.gripper = self.master_joint_actions["grasp"] * 1000
+            self.gripper = self.master_joint_actions["grasp"]# * 1000
             self.joint[-1] = self.gripper
     
 
@@ -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

realman.md

@@ -39,7 +39,7 @@ bash can_activate.sh can0 1000000
 
 cd ..
 python lerobot/scripts/control_robot.py \
-    --robot.type=piper \
+    --robot.type=realman \
     --robot.inference_time=false \
     --control.type=teleoperate
 ```

realman_src/crc_cal.py

@@ -0,0 +1,58 @@
+def crc8_maxim(data):
+    """
+    使用CRC-8/MAXIM算法计算CRC8校验值
+    多项式：X8+X5+X4+1=0x31
+    
+    Args:
+        data: 要校验的字节数据
+    
+    Returns:
+        计算出的CRC8校验值
+    """
+    # CRC8表（与您提供的表相同）
+    CRC8TAB = [
+        0x00, 0x31, 0x62, 0x53, 0xC4, 0xF5, 0xA6, 0x97,
+        0xB9, 0x88, 0xDB, 0xEA, 0x7D, 0x4C, 0x1F, 0x2E,
+        0x43, 0x72, 0x21, 0x10, 0x87, 0xB6, 0xE5, 0xD4,
+        0xFA, 0xCB, 0x98, 0xA9, 0x3E, 0x0F, 0x5C, 0x6D,
+        0x86, 0xB7, 0xE4, 0xD5, 0x42, 0x73, 0x20, 0x11,
+        0x3F, 0x0E, 0x5D, 0x6C, 0xFB, 0xCA, 0x99, 0xA8,
+        0xC5, 0xF4, 0xA7, 0x96, 0x01, 0x30, 0x63, 0x52,
+        0x7C, 0x4D, 0x1E, 0x2F, 0xB8, 0x89, 0xDA, 0xEB,
+        0x3D, 0x0C, 0x5F, 0x6E, 0xF9, 0xC8, 0x9B, 0xAA,
+        0x84, 0xB5, 0xE6, 0xD7, 0x40, 0x71, 0x22, 0x13,
+        0x7E, 0x4F, 0x1C, 0x2D, 0xBA, 0x8B, 0xD8, 0xE9,
+        0xC7, 0xF6, 0xA5, 0x94, 0x03, 0x32, 0x61, 0x50,
+        0xBB, 0x8A, 0xD9, 0xE8, 0x7F, 0x4E, 0x1D, 0x2C,
+        0x02, 0x33, 0x60, 0x51, 0xC6, 0xF7, 0xA4, 0x95,
+        0xF8, 0xC9, 0x9A, 0xAB, 0x3C, 0x0D, 0x5E, 0x6F,
+        0x41, 0x70, 0x23, 0x12, 0x85, 0xB4, 0xE7, 0xD6,
+        0x7A, 0x4B, 0x18, 0x29, 0xBE, 0x8F, 0xDC, 0xED,
+        0xC3, 0xF2, 0xA1, 0x90, 0x07, 0x36, 0x65, 0x54,
+        0x39, 0x08, 0x5B, 0x6A, 0xFD, 0xCC, 0x9F, 0xAE,
+        0x80, 0xB1, 0xE2, 0xD3, 0x44, 0x75, 0x26, 0x17,
+        0xFC, 0xCD, 0x9E, 0xAF, 0x38, 0x09, 0x5A, 0x6B,
+        0x45, 0x74, 0x27, 0x16, 0x81, 0xB0, 0xE3, 0xD2,
+        0xBF, 0x8E, 0xDD, 0xEC, 0x7B, 0x4A, 0x19, 0x28,
+        0x06, 0x37, 0x64, 0x55, 0xC2, 0xF3, 0xA0, 0x91,
+        0x47, 0x76, 0x25, 0x14, 0x83, 0xB2, 0xE1, 0xD0,
+        0xFE, 0xCF, 0x9C, 0xAD, 0x3A, 0x0B, 0x58, 0x69,
+        0x04, 0x35, 0x66, 0x57, 0xC0, 0xF1, 0xA2, 0x93,
+        0xBD, 0x8C, 0xDF, 0xFE, 0x79, 0x48, 0x1B, 0x2A,
+        0xC1, 0xF0, 0xA3, 0x92, 0x05, 0x34, 0x67, 0x56,
+        0x78, 0x49, 0x1A, 0x2B, 0xBC, 0x8D, 0xDE, 0xEF,
+        0x82, 0xB3, 0xE0, 0xD1, 0x46, 0x77, 0x24, 0x15,
+        0x3B, 0x0A, 0x59, 0x68, 0xFF, 0xCE, 0x9D, 0xAC
+    ]
+    
+    crc = 0
+    for byte in data:
+        crc = CRC8TAB[crc ^ byte]
+    
+    return crc
+
+
+# 数据帧：55 AA 02 00 00 67
+data = bytes([0x55, 0xAA, 0x08, 0x00, 0x02])
+crc = crc8_maxim(data)
+print(f"计算出的CRC8校验值: 0x{crc:02X}")

realman_src/servo_arm.yaml

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

realman_src/servo_arm_with_io_status.py

@@ -0,0 +1,140 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+import time
+import yaml
+import serial
+import logging
+import binascii
+import numpy as np
+
+# 配置日志记录
+logging.basicConfig(
+    level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s"
+)
+
+
+class ServoArm:
+    def __init__(self, config_file="config.yaml"):
+        """初始化机械臂的串口连接并发送初始数据。
+
+        Args:
+            config_file (str): 配置文件的路径。
+        """
+        self.config = self._load_config(config_file)
+        self.port = self.config["port"]
+        self.baudrate = self.config["baudrate"]
+        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)
+
+        self.serial_conn = serial.Serial(self.port, self.baudrate, timeout=0)
+
+        self.bytes_to_send = binascii.unhexlify(self.joint_hex_data.replace(" ", ""))
+        self.serial_conn.write(self.bytes_to_send)
+        time.sleep(1)
+
+    def _load_config(self, config_file):
+        """加载配置文件。
+
+        Args:
+            config_file (str): 配置文件的路径。
+
+        Returns:
+            dict: 配置文件内容。
+        """
+        with open(config_file, "r") as file:
+            config = yaml.safe_load(file)
+        return config
+
+    def _bytes_to_signed_int(self, byte_data):
+        """将字节数据转换为有符号整数。
+
+        Args:
+            byte_data (bytes): 字节数据。
+
+        Returns:
+            int: 有符号整数。
+        """
+        return int.from_bytes(byte_data, byteorder="little", signed=True)
+
+    def _parse_joint_data(self, hex_received):
+        """解析接收到的十六进制数据并提取关节数据。
+
+        Args:
+            hex_received (str): 接收到的十六进制字符串数据。
+
+        Returns:
+            dict: 解析后的关节数据。
+        """
+        logging.debug(f"hex_received: {hex_received}")
+        joints = {}
+        for i in range(self.arm_axis):
+            start = 14 + i * 10
+            end = start + 8
+            joint_hex = hex_received[start:end]
+            joint_byte_data = bytearray.fromhex(joint_hex)
+            joint_value = self._bytes_to_signed_int(joint_byte_data) / 10000.0
+            joints[f"joint_{i+1}"] = joint_value
+        grasp_start = 14 + self.arm_axis*10
+        grasp_hex = hex_received[grasp_start:grasp_start+8]
+        grasp_byte_data = bytearray.fromhex(grasp_hex)
+        # 夹爪进行归一化处理
+        grasp_value = self._bytes_to_signed_int(grasp_byte_data)/1000
+        # print(grasp_value)
+
+        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):
+        """从串口读取数据并解析关节动作。
+
+        Returns:
+            dict: 包含关节数据的字典。
+        """
+        self.serial_conn.write(self.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("joints:", hex_received)
+        actions = self._parse_joint_data(hex_received)
+        return actions
+    
+    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 set_gripper_action(self, action):
+        """设置夹爪动作。
+        Args:
+            action (int): 夹爪动作值。
+        """
+        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:]
+
+if __name__ == "__main__":
+    servo_arm = ServoArm("/home/maic/LYT/lerobot/realman_src/servo_arm.yaml")
+    while True:
+        joint_actions = servo_arm.get_joint_actions()
+        logging.info(joint_actions)
+        # time.sleep(0.02)
+        status = servo_arm.get_controller_status()
+        logging.info(status)
+        time.sleep(0.5)