From ef45ea96496db120a2cacae16698df72848abbe5 Mon Sep 17 00:00:00 2001 From: yutang Date: Thu, 26 Jun 2025 21:08:22 +0800 Subject: [PATCH] single arm refactory --- .../common/robot_devices/motors/realman.py | 22 +++++ .../common/robot_devices/robots/configs.py | 4 +- .../common/robot_devices/robots/realman.py | 94 ++++--------------- .../common/robot_devices/teleop/gamepad.py | 55 ++++++----- .../robot_devices/teleop/servo_arm.yaml | 3 +- 5 files changed, 74 insertions(+), 104 deletions(-) diff --git a/lerobot/common/robot_devices/motors/realman.py b/lerobot/common/robot_devices/motors/realman.py index d45c4084..ed076eb3 100644 --- a/lerobot/common/robot_devices/motors/realman.py +++ b/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 diff --git a/lerobot/common/robot_devices/robots/configs.py b/lerobot/common/robot_devices/robots/configs.py index 8c4435e2..94ebabb4 100644 --- a/lerobot/common/robot_devices/robots/configs.py +++ b/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]} ) } ) diff --git a/lerobot/common/robot_devices/robots/realman.py b/lerobot/common/robot_devices/robots/realman.py index 37b9466a..e8845037 100644 --- a/lerobot/common/robot_devices/robots/realman.py +++ b/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 diff --git a/lerobot/common/robot_devices/teleop/gamepad.py b/lerobot/common/robot_devices/teleop/gamepad.py index 72ddcda4..330d3ab1 100644 --- a/lerobot/common/robot_devices/teleop/gamepad.py +++ b/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 diff --git a/lerobot/common/robot_devices/teleop/servo_arm.yaml b/lerobot/common/robot_devices/teleop/servo_arm.yaml index 8bf45feb..06d28ee2 100644 --- a/lerobot/common/robot_devices/teleop/servo_arm.yaml +++ b/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