diff --git a/lerobot/common/robot_devices/robots/configs.py b/lerobot/common/robot_devices/robots/configs.py index 8c4435e2f..9de5f2959 100644 --- a/lerobot/common/robot_devices/robots/configs.py +++ b/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, diff --git a/lerobot/common/robot_devices/robots/realman.py b/lerobot/common/robot_devices/robots/realman.py index 37b9466ac..f869d5218 100644 --- a/lerobot/common/robot_devices/robots/realman.py +++ b/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() diff --git a/lerobot/common/robot_devices/teleop/gamepad.py b/lerobot/common/robot_devices/teleop/gamepad.py index 72ddcda48..4e259a406 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}") @@ -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 diff --git a/lerobot/common/robot_devices/teleop/servo_arm.yaml b/lerobot/common/robot_devices/teleop/servo_arm.yaml index 8bf45febb..06d28ee29 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 diff --git a/realman.md b/realman.md index bad50b93a..da92f702b 100644 --- a/realman.md +++ b/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 ```