diff --git a/realman_src/realman_aloha/shadow_rm_robot/config/servo_arm.yaml b/realman_src/realman_aloha/shadow_rm_robot/config/servo_arm.yaml index 7822e5f2..8bf45feb 100644 --- a/realman_src/realman_aloha/shadow_rm_robot/config/servo_arm.yaml +++ b/realman_src/realman_aloha/shadow_rm_robot/config/servo_arm.yaml @@ -2,4 +2,4 @@ port: /dev/ttyUSB0 right_port: /dev/ttyUSB1 baudrate: 460800 hex_data: "55 AA 02 00 00 67" -arm_axis: 7 +arm_axis: 6 diff --git a/realman_src/realman_aloha/shadow_rm_robot/src/shadow_rm_robot/servo_robotic_arm.py b/realman_src/realman_aloha/shadow_rm_robot/src/shadow_rm_robot/servo_robotic_arm.py index 9941e28d..c86e2a46 100644 --- a/realman_src/realman_aloha/shadow_rm_robot/src/shadow_rm_robot/servo_robotic_arm.py +++ b/realman_src/realman_aloha/shadow_rm_robot/src/shadow_rm_robot/servo_robotic_arm.py @@ -79,6 +79,7 @@ class ServoArm: 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 @@ -105,7 +106,7 @@ class ServoArm: self.bytes_to_send = self.bytes_to_send[:74] + action_bytes + self.bytes_to_send[78:] if __name__ == "__main__": - servo_arm = ServoArm("/home/rm/code/shadow_rm_aloha/config/servo_left_arm.yaml") + servo_arm = ServoArm("/home/maic/LYT/lerobot/realman_src/realman_aloha/shadow_rm_robot/config/servo_arm.yaml") while True: joint_actions = servo_arm.get_joint_actions() logging.info(joint_actions) diff --git a/realman_src/realman_mix_control.py b/realman_src/realman_mix_control.py new file mode 100644 index 00000000..c19dd246 --- /dev/null +++ b/realman_src/realman_mix_control.py @@ -0,0 +1,562 @@ +#!/usr/bin/env python3 +# -*-coding:utf8-*- +from typing import Optional +import time +import yaml +import serial +import logging +import binascii +import numpy as np +from Robotic_Arm.rm_robot_interface import * +import pygame +import threading +from typing import Dict + +# 配置日志记录 +logging.basicConfig( + level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s" +) + +def enable_fun(arm: RoboticArm): + ''' + 使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序 + ''' + enable_flag = False + # 设置超时时间（秒） + timeout = 5 + # 记录进入循环前的时间 + start_time = time.time() + elapsed_time_flag = False + + while not enable_flag: + elapsed_time = time.time() - start_time + print("--------------------") + + # 获取机械臂状态 + ret = arm.rm_get_current_arm_state() + if ret[0] == 0: # 成功获取状态 + arm_state = ret[1] + enable_flag = True + + print("使能状态:", enable_flag) + print("--------------------") + # 检查是否超过超时时间 + if elapsed_time > timeout: + print("超时....") + elapsed_time_flag = True + enable_flag = True + break + time.sleep(1) + + if elapsed_time_flag: + print("程序自动使能超时,退出程序") + exit(0) + + +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.hex_data = self.config["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.serial_conn.write(self.bytes_to_send) + time.sleep(1) + self.connected = True + logging.info(f"串口连接成功: {self.port}") + except Exception as e: + logging.error(f"串口连接失败: {e}") + self.connected = False + + def _load_config(self, config_file): + """加载配置文件。 + + Args: + config_file (str): 配置文件的路径。 + + Returns: + dict: 配置文件内容。 + """ + try: + with open(config_file, "r") as file: + config = yaml.safe_load(file) + return config + except Exception as e: + logging.error(f"配置文件加载失败: {e}") + # 返回默认配置 + return { + "port": "/dev/ttyUSB0", + "baudrate": 9600, + "hex_data": "00 00 00 00", + "arm_axis": 7 + } + + 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 get_joint_actions(self): + """从串口读取数据并解析关节动作。 + + Returns: + dict: 包含关节数据的字典。 + """ + if not self.connected: + return {} + + try: + self.serial_conn.write(self.bytes_to_send) + bytes_received = self.serial_conn.read(self.serial_conn.inWaiting()) + if len(bytes_received) == 0: + return {} + + hex_received = binascii.hexlify(bytes_received).decode("utf-8").upper() + actions = self._parse_joint_data(hex_received) + return actions + except Exception as e: + 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 close(self): + """关闭串口连接""" + if self.connected and hasattr(self, 'serial_conn'): + self.serial_conn.close() + self.connected = False + logging.info("串口连接已关闭") + + +class HybridController: + def __init__(self, init_info): + # 初始化pygame和手柄 + pygame.init() + pygame.joystick.init() + + # 检查是否有连接的手柄 + if pygame.joystick.get_count() == 0: + raise Exception("未检测到手柄") + + # 初始化手柄 + self.joystick = pygame.joystick.Joystick(0) + self.joystick.init() + # 摇杆死区 + self.deadzone = 0.15 + # 控制模式: True为关节控制（主模式），False为末端控制 + self.joint_control_mode = True + # 精细控制模式 + self.fine_control_mode = False + + # 初始化末端姿态和关节角度 + self.init_joint = init_info['init_joint'] + self.init_pose = init_info['init_pose'] + self.max_gripper = init_info['max_gripper'] + self.min_gripper = init_info['min_gripper'] + servo_config_file = init_info['servo_config_file'] + self.joint = self.init_joint.copy() + self.pose = self.init_pose.copy() + self.pose_speeds = [0.0] * 6 + self.joint_speeds = [0.0] * 6 + + # 主臂关节状态 + self.master_joint_actions = {} + self.use_master_arm = False + + # 末端位姿限制 + self.pose_limits = [ + (-0.850, 0.850), # X (m) + (-0.850, 0.850), # Y (m) + (0.850, 0.850), # Z (m) + (-3.14, 3.14), # RX (rad) + (-3.14, 3.14), # RY (rad) + (-3.14, 3.14) # RZ (rad) + ] + + # 关节角度限制 (度) + self.joint_limits = [ + (-180, 180), # joint 1 + (-180, 180), # joint 2 + (-180, 180), # joint 3 + (-180, 180), # joint 4 + (-180, 180), # joint 5 + (-180, 180) # joint 6 + ] + + # 控制参数 + self.linear_step = 0.01 # 线性移动步长(m) + self.angular_step = 0.1 # 角度步长(rad) + self.joint_step = 0.5 # 关节角度步长(度) + + # 夹爪状态和速度 + self.gripper_speed = 10 + self.gripper = self.min_gripper + + # 初始化串口通信（主臂关节状态获取） + self.servo_arm = None + if servo_config_file: + try: + self.servo_arm = ServoArm(servo_config_file) + self.use_master_arm = True + logging.info("串口主臂连接成功，启用主从控制模式") + except Exception as e: + logging.error(f"串口主臂连接失败: {e}") + self.use_master_arm = False + + # 启动更新线程 + self.running = True + self.thread = threading.Thread(target=self.update_controller) + self.thread.start() + + print("混合控制器已启动") + print("主控制模式: 关节控制") + if self.use_master_arm: + print("主从控制: 启用") + print("Back按钮: 切换控制模式(关节/末端)") + print("L3按钮: 切换精细控制模式") + print("Start按钮: 重置到初始位置") + + def _apply_nonlinear_mapping(self, value): + """应用非线性映射以提高控制精度""" + sign = 1 if value >= 0 else -1 + return sign * (abs(value) ** 2) + + def _normalize_angle(self, angle): + """将角度归一化到[-π, π]范围内""" + import math + while angle > math.pi: + angle -= 2 * math.pi + while angle < -math.pi: + angle += 2 * math.pi + return angle + + def update_controller(self): + while self.running: + try: + pygame.event.pump() + except Exception as e: + print(f"控制器错误: {e}") + self.stop() + continue + + # 检查控制模式切换 (Back按钮) + if self.joystick.get_button(6): # Back按钮 + self.joint_control_mode = not self.joint_control_mode + mode_str = "关节控制" if self.joint_control_mode else "末端位姿控制" + print(f"切换到{mode_str}模式") + time.sleep(0.3) # 防止多次触发 + + # 检查精细控制模式切换 (L3按钮) + if self.joystick.get_button(10): # L3按钮 + self.fine_control_mode = not self.fine_control_mode + print(f"切换到{'精细' if self.fine_control_mode else '普通'}控制模式") + time.sleep(0.3) # 防止多次触发 + + # 检查重置按钮 (Start按钮) + if self.joystick.get_button(7): # Start按钮 + print("重置机械臂到初始位置...") + self.joint = self.init_joint.copy() + self.pose = self.init_pose.copy() + self.pose_speeds = [0.0] * 6 + self.joint_speeds = [0.0] * 6 + self.gripper_speed = 10 + self.gripper = self.min_gripper + print("机械臂已重置到初始位置") + time.sleep(0.3) # 防止多次触发 + + # 从串口获取主臂关节状态 + if self.servo_arm and self.servo_arm.connected: + # print("ok") + try: + self.master_joint_actions = self.servo_arm.get_joint_actions() + # print(self.master_joint_actions) + if self.master_joint_actions: + logging.debug(f"主臂关节状态: {self.master_joint_actions}") + + except Exception as e: + logging.error(f"获取主臂状态错误: {e}") + self.master_joint_actions = {} + # print(self.master_joint_actions) + + # 根据控制模式更新相应的控制逻辑 + if self.joint_control_mode: + # 关节控制模式下，优先使用主臂数据，Xbox作为辅助 + self.update_joint_control() + else: + # 末端控制模式，使用Xbox控制 + self.update_end_pose() + + + time.sleep(0.02) + print('gripper:', self.gripper) + + def update_joint_control(self): + """更新关节角度控制 - 优先使用主臂数据""" + if self.use_master_arm and self.master_joint_actions: + # 主从控制模式：直接使用主臂的关节角度 + try: + # 将主臂关节角度映射到从臂 + for i in range(6): # 假设只有6个关节需要控制 + joint_key = f"joint_{i+1}" + if joint_key in self.master_joint_actions: + # 直接使用主臂的关节角度（已经是度数） + self.joint[i] = self.master_joint_actions[joint_key] + + # 应用关节限制 + min_val, max_val = self.joint_limits[i] + self.joint[i] = max(min_val, min(max_val, self.joint[i])) + + # print(self.joint) + logging.debug(f"主臂关节映射到从臂: {self.joint[:6]}") + + except Exception as e: + logging.error(f"主臂数据映射错误: {e}") + + # 如果有主臂夹爪数据，使用主臂夹爪状态 + if self.use_master_arm and "grasp" in self.master_joint_actions: + self.gripper = self.master_joint_actions["grasp"] * 1000 + + + def update_end_pose(self): + """更新末端位姿控制""" + # 根据控制模式调整步长 + current_linear_step = self.linear_step * (0.1 if self.fine_control_mode else 1.0) + current_angular_step = self.angular_step * (0.1 if self.fine_control_mode else 1.0) + + # 方向键控制XY + hat = self.joystick.get_hat(0) + hat_up = hat[1] == 1 # Y+ + hat_down = hat[1] == -1 # Y- + hat_left = hat[0] == -1 # X- + hat_right = hat[0] == 1 # X+ + + # 右摇杆控制Z + right_y_raw = -self.joystick.get_axis(4) + # 左摇杆控制RZ + left_y_raw = -self.joystick.get_axis(1) + + # 应用死区 + right_y = 0.0 if abs(right_y_raw) < self.deadzone else right_y_raw + left_y = 0.0 if abs(left_y_raw) < self.deadzone else left_y_raw + + # 计算各轴速度 + self.pose_speeds[1] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0) # Y + self.pose_speeds[0] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0) # X + + # 设置Z速度（右摇杆Y轴控制） + z_mapping = self._apply_nonlinear_mapping(right_y) + self.pose_speeds[2] = z_mapping * current_linear_step # Z + + # L1/R1控制RX旋转 + LB = self.joystick.get_button(4) # RX- + RB = self.joystick.get_button(5) # RX+ + self.pose_speeds[3] = (-current_angular_step if LB else (current_angular_step if RB else 0.0)) + + # △/□控制RY旋转 + triangle = self.joystick.get_button(2) # RY+ + square = self.joystick.get_button(3) # RY- + self.pose_speeds[4] = (current_angular_step if triangle else (-current_angular_step if square else 0.0)) + + # 左摇杆Y轴控制RZ旋转 + rz_mapping = self._apply_nonlinear_mapping(left_y) + self.pose_speeds[5] = rz_mapping * current_angular_step * 2 # RZ + + # 夹爪控制（圈/叉） + circle = self.joystick.get_button(1) # 夹爪开 + cross = self.joystick.get_button(0) # 夹爪关 + if circle: + self.gripper = min(self.max_gripper, self.gripper + self.gripper_speed) + elif cross: + self.gripper = max(self.min_gripper, self.gripper - self.gripper_speed) + + # 更新末端位姿 + for i in range(6): + self.pose[i] += self.pose_speeds[i] + + # 角度归一化处理 + for i in range(3, 6): + self.pose[i] = self._normalize_angle(self.pose[i]) + + def update_state(self, end_pose, joint_state): + """更新状态信息（从机械臂获取当前状态）""" + self.pose = end_pose + self.joint = joint_state + + def get_action(self) -> Dict: + """获取当前控制命令""" + return { + '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], + }, + 'joint_angles': self.joint[:6], # 只取前6个关节 + 'gripper': int(self.gripper) + } + + def stop(self): + """停止控制器""" + self.running = False + if self.thread.is_alive(): + self.thread.join() + if self.servo_arm: + self.servo_arm.close() + pygame.quit() + print("混合控制器已退出") + + def reset(self): + """重置到初始状态""" + self.joint = self.init_joint.copy() + self.pose = self.init_pose.copy() + self.pose_speeds = [0.0] * 6 + self.joint_speeds = [0.0] * 6 + self.gripper_speed = 10 + self.gripper = self.min_gripper + print("已重置到初始状态") + + +if __name__ == "__main__": + # 初始化睿尔曼机械臂 + arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) + # 创建机械臂连接 + handle = arm.rm_create_robot_arm("192.168.3.18", 8080) + print(f"机械臂连接ID: {handle.id}") + + # 使能机械臂 + enable_fun(arm=arm) + + # 初始关节角度和位姿 + init_joint = [-90, 90, 90, -90, -90, 90] + max_gripper = 990 + min_gripper = 10 + arm.rm_movej(init_joint, 50, 0, 0, 1) + arm.rm_set_gripper_position(max_gripper, True, 2) + arm.rm_set_gripper_position(min_gripper, True, 2) + init_pose = arm.rm_get_current_arm_state()[1]['pose'] + + init_info = { + 'init_pose': init_pose, + 'init_joint': init_joint, + 'max_gripper': max_gripper, + 'min_gripper': min_gripper, + 'servo_config_file': "/home/maic/LYT/lerobot/realman_src/realman_aloha/shadow_rm_robot/config/servo_arm.yaml" + } + + try: + teleop = HybridController(init_info) + except Exception as e: + print(f"初始化失败: {e}") + exit(1) + + try: + while True: + # 获取当前控制命令 + action = teleop.get_action() + + if action['control_mode'] == 'joint': + # 关节控制模式（主模式） + target_joints = action['joint_angles'] + + 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) + ] + + # 使用笛卡尔空间运动控制末端位姿 + result = arm.rm_movej_p(target_pose, 50, 0, 0, 1) + + # 夹爪控制 + arm.rm_set_gripper_position(action['gripper'], True, 2) + + # 获取当前机械臂状态 + ret = arm.rm_get_current_arm_state() + if ret[0] == 0: # 成功获取状态 + current_pose = ret[1].get('pose', [0]*6) + current_joint = ret[1].get('joint', [0]*7) + + # teleop.update_state(current_pose, current_joint) + + print(f"当前位姿: {current_pose}") + print(f"当前关节: {current_joint[:6]}") # 只显示前6个关节 + else: + print(f"获取机械臂状态失败，错误码: {ret[0]}") + + print("-" * 80) + # time.sleep(0.1) + + except KeyboardInterrupt: + print("程序被用户中断") + finally: + # 清理资源 + teleop.stop() + arm.rm_delete_robot_arm() + print("程序退出完成") diff --git a/realman_src/realman_xbox.py b/realman_src/realman_xbox.py index 58f64a58..c2054fc6 100644 --- a/realman_src/realman_xbox.py +++ b/realman_src/realman_xbox.py @@ -189,8 +189,8 @@ class EndPoseController: # print(f"死区处理后 - 右摇杆: {right_y}, 左摇杆: {left_y}") # 计算各轴速度 - self.pose_speeds[0] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0) # X - self.pose_speeds[1] = current_linear_step if hat_left else (-current_linear_step if hat_right else 0.0) # Y + self.pose_speeds[1] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0) # Y + self.pose_speeds[0] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0) # X # 设置Z速度（右摇杆Y轴控制） z_mapping = self._apply_nonlinear_mapping(right_y) @@ -284,19 +284,20 @@ class EndPoseController: if __name__ == "__main__": # 初始化睿尔曼机械臂 arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) - - init_joint = [-90, 90, 90, 90, -90, -90, 90] - init_pose = [-0.030, 0.255, 0.161, 3.142, 0, -1.57] - # 创建机械臂连接 handle = arm.rm_create_robot_arm("192.168.3.18", 8080) print(f"机械臂连接ID: {handle.id}") - + # 使能机械臂 enable_fun(arm=arm) + # init joint + init_joint = [-90, 90, 90, -90, -90, 90] + arm.rm_movej(init_joint, 50, 0, 0, 1) + init_pose = arm.rm_get_current_arm_state()[1]['pose'] + teleop = EndPoseController(init_joint, init_pose) - + try: while True: # 获取当前控制命令 @@ -314,7 +315,7 @@ if __name__ == "__main__": # 使用笛卡尔空间直线运动控制末端位姿 # 参数: 目标位姿, 速度比例(20%), 交融半径(0), 连接标志(0), 阻塞模式(0-非阻塞) - result = arm.rm_movej_p(target_pose, 50, 0, 0, 1) + result = arm.rm_movej_p(target_pose, 50, 0, 0, 0) if result != 0: print(f"运动控制错误，错误码: {result}") diff --git a/realman_src/single_arm_control_test.py b/realman_src/single_arm_control_test.py index 1f2c3eae..86c41426 100644 --- a/realman_src/single_arm_control_test.py +++ b/realman_src/single_arm_control_test.py @@ -12,10 +12,18 @@ print("Left: ", armleft.rm_get_arm_all_state()) # armleft.rm_movej([-90, 90, 90, -90, -90, 90], 50, 0, 0, 1) # armleft.rm_movej_p([-0.185, 0.315, 0.080, -1.500, -0.800, -0.000], 50, 0, 0, 1)s # armleft.rm_movel([-0.185, 0.315, 0.080, -1.500, -0.800, -0.000], 50, 0, 0, 1) -armleft.rm_set_gripper_position(1000, True, 2) +print(armleft.rm_get_gripper_state()) +# armleft.rm_set_gripper_pick(100, 500, True, 2) +# armleft.rm_set_gripper_release(300, True, 2) +# armleft.rm_set_gripper_pick(100, 500, True, 2) +# armleft.rm_set_gripper_position(500, True, 20) +# armleft.rm_set_gripper_pick(100, 500, True, 2) +# armleft.rm_set_gripper_position(500, True, 20) + +# armleft.rm_set_gripper_position(999, True, 2) import time time.sleep(3) -armleft.rm_set_gripper_position(1, True, 2) +# armleft.rm_set_gripper_position(1, True, 2) # 断开所有连接，销毁线程 RoboticArm.rm_destory() \ No newline at end of file