import sys import os # # 获取当前文件的绝对路径 # current_dir = os.path.dirname(os.path.abspath(__file__)) # # 获取项目根目录的绝对路径 # project_root = os.path.abspath(os.path.join(current_dir, os.pardir)) # sys.path.append(project_root) import pyaubo_sdk # from tools.Rate import Rate from scipy.spatial.transform import Rotation as R # from tools.log import CustomLogger # from tools.yaml_operator import read_yaml import threading # from tools.bytes_transform import * # from aubo_message import * import socket import numpy as np import atexit import time import requests import copy # 定义机器人模式字典 robot_mode_types = { -1: "NoController: 提供给示教器使用的, 如果aubo_control进程崩溃则会显示为NoController", 0: "Disconnected: 没有连接到机械臂本体(控制器与接口板断开连接或是 EtherCAT 等总线断开)", 1: "ConfirmSafety: 正在进行安全配置, 断电状态下进行", 2: "Booting: 机械臂本体正在上电初始化", 3: "PowerOff: 机械臂本体处于断电状态", 4: "PowerOn: 机械臂本体上电成功, 刹车暂未松开(抱死), 关节初始状态未获取", 5: "Idle: 机械臂上电成功, 刹车暂未松开(抱死), 电机不通电, 关节初始状态获取完成", 6: "BrakeReleasing: 机械臂上电成功, 刹车正在松开", 7: "BackDrive: 反向驱动：刹车松开, 电机不通电", 8: "Running: 机械臂刹车松开, 运行模式, 控制权由硬件移交给软件", 9: "Maintaince: 维护模式: 包括固件升级、参数写入等", 10: "Error: ", 11: "PowerOffing: 机械臂本体处于断电过程中" } # 定义安全模式字典 safety_mode_types = { 0: "Undefined: 安全状态待定", 1: "Normal: 正常运行模式", 2: "ReducedMode: 缩减运行模式", 3: "Recovery: 启动时如果在安全限制之外, 机器人将进入recovery模式", 4: "Violation: 超出安全限制（根据安全配置, 例如速度超限等）", 5: "ProtectiveStop: 软件触发的停机（保持轨迹, 不抱闸, 不断电）", 6: "SafeguardStop: IO触发的防护停机（不保持轨迹, 抱闸, 不断电）", 7: "SystemEmergencyStop: 系统急停：急停信号由外部输入(可配置输入), 不对外输出急停信号", 8: "RobotEmergencyStop: 机器人急停：控制柜急停输入或者示教器急停按键触发, 对外输出急停信号", 9: "Fault: 机械臂硬件故障或者系统故障" } class AuboC5(): def __init__(self, arm_ip = "192.168.100.20", arm_port = 30004): self.arm_ip = arm_ip self.arm_port = arm_port # self.logger = CustomLogger('aubo_C5',True) self.robot_rpc_client = pyaubo_sdk.RpcClient() self.robot_rpc_client.setRequestTimeout(1000) # 接口调用: 设置 RPC 超时 self.robot_rpc_client.connect(self.arm_ip, self.arm_port) # 连接 RPC 服务 if self.robot_rpc_client.hasConnected(): print("Robot rpc_client connected successfully!") self.robot_rpc_client.login("aubo", "123456") # 登录机械臂 if self.robot_rpc_client.hasLogined(): print("Robot rpc_client logged in successfully!") self.robot_name = self.robot_rpc_client.getRobotNames()[0] # 接口调用: 获取机器人的名字 self.robot = self.robot_rpc_client.getRobotInterface(self.robot_name) self.robot_config = self.robot.getRobotConfig() # 获取状态 robot_mode_type, safety_mode_type = self.getModelType() if safety_mode_type == 8: requests.post( "http://127.0.0.1:5000/on_message", data={"message": "机械臂处于锁定状态
请解除锁定后再使用"} ) instruction = { "is_massaging": False, "massage_service_started": False } requests.post( "http://127.0.0.1:5000/update_massage_status", data=instruction ) sys.exit(0) if safety_mode_type == 5: self.robot.getRobotManage().setUnlockProtectiveStop() # 接口调用: 解除保护停止 if safety_mode_type in [3,4]: self.power_off() time.sleep(1) if safety_mode_type == 9: self.robot.getRobotManage().restartInterfaceBoard() # 启动 self.start_up() # 接口调用: 设置工具中心点（TCP相对于法兰盘中心的偏移） tcp_offset = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.robot_config.setTcpOffset(tcp_offset) self.mc = self.robot.getMotionControl() # 接口调用: 设置机械臂的速度比率 self.mc.setSpeedFraction(1) self.robot_name = self.robot_rpc_client.getRobotNames()[0] self.robot_rpc_client.getRuntimeMachine().stop() self.robot_rpc_client.getRuntimeMachine().start() self.is_exit = False # 退出任务 atexit.register(self.exit_task) self.standby_pos = [90 * (np.pi / 180), 30 * (np.pi / 180), 90 * (np.pi / 180), 0 * (np.pi / 180), 90 * (np.pi / 180), 0.0 * (np.pi / 180) ] self.init_pos = [90 * (np.pi / 180), 70 * (np.pi / 180), 130 * (np.pi / 180), 0 * (np.pi / 180), 90 * (np.pi / 180), 0.0 * (np.pi / 180) ] self.off_pos = [90 * (np.pi / 180), 90 * (np.pi / 180), 150 * (np.pi / 180), 0 * (np.pi / 180), 90 * (np.pi / 180), 0.0 * (np.pi / 180) ] self.pack_pos = [90 * (np.pi / 180), 90 * (np.pi / 180), 150 * (np.pi / 180), 0 * (np.pi / 180), 90 * (np.pi / 180), -90 * (np.pi / 180) ] def init(self): self.is_exit = False time.sleep(2) self.is_standby = True def exit_task(self): self.is_exit = True def pack(self): print("运动到打包位置") self.move_joint(self.pack_pos,wait=True) def getModelType(self): robot_mode_type = self.robot.getRobotState().getRobotModeType() print("机器人的模式状态:", robot_mode_type) if robot_mode_type.value == 8: self.power_off() time.sleep(1) robot_mode_type = self.robot.getRobotState().getRobotModeType() print("机器人的模式状态:", robot_mode_type) print(robot_mode_types.get(robot_mode_type.value, "未知模式")) # 接口调用: 获取安全模式 safety_mode_type = self.robot.getRobotState().getSafetyModeType() print("安全模式:", safety_mode_type) print(safety_mode_types.get(safety_mode_type.value, "未知模式")) return robot_mode_type.value, safety_mode_type.value def start_up(self): if 0 == self.robot_rpc_client.getRobotInterface(self.robot_name).getRobotConfig().setCollisionLevel(0): time.sleep(0.2) print("The robot is requesting turn off collision detection!") print("self.robot_rpc_client.getRobotInterface(self.robot_name).getRobotConfig().getCollisionLevel()",self.robot_rpc_client.getRobotInterface(self.robot_name).getRobotConfig().getCollisionLevel()) if 0 == self.robot_rpc_client.getRobotInterface( self.robot_name).getRobotManage().poweron(): # 接口调用: 发起机器人上电请求 print("The robot is requesting power-on!") if 0 == self.robot_rpc_client.getRobotInterface( self.robot_name).getRobotManage().startup(): # 接口调用: 发起机器人启动请求 print("The robot is requesting startup!") # 循环直至机械臂松刹车成功 while 1: robot_mode = self.robot_rpc_client.getRobotInterface(self.robot_name) \ .getRobotState().getRobotModeType() # 接口调用: 获取机器人的模式类型 print("Robot current mode: %s" % (robot_mode.name)) if robot_mode == pyaubo_sdk.RobotModeType.Running: break time.sleep(1) def power_off(self): robot_name = self.robot_rpc_client.getRobotNames()[0] # 接口调用: 获取机器人的名字 if 0 == self.robot_rpc_client.getRobotInterface( robot_name).getRobotManage().poweroff(): # 接口调用: 机械臂断电 print("The robot is requesting power-off!") def move_joint(self, q ,max_retry_count = 3, wait = False): cnt = 0 while not self.is_exit or wait: cnt += 1 if cnt > max_retry_count: print(f'Failed to arrive at the joint position: {q}') return -1 # self.mc.moveJoint(q, 1.4, 1.04, 0, 0) self.mc.moveJoint(q, 0.5, 0.42, 0, 0) is_arrived = self.waitArrival() if is_arrived == -1: self.robot_rpc_client.getRuntimeMachine().stop() self.robot_rpc_client.getRuntimeMachine().start() time.sleep(0.1) continue elif is_arrived == -2: print(f'Failed to arrive at the joint position: {q}') return -1 else: print(f'Arrived at the joint position: {q}') return 0 print(f'Failed to arrive at the joint position: {q}') def waitArrival(self,max_retry_count = 3): cnt = 0 while self.mc.getExecId() == -1: cnt += 1 if cnt > max_retry_count: print("Motion fail!") return -1 time.sleep(0.05) print("getExecId: ", self.mc.getExecId()) id = self.mc.getExecId() while True: id1 = self.mc.getExecId() # 获取机械臂的模式类型 robot_mode_type = self.robot.getRobotState().getRobotModeType() # print("robot_mode_type: ", type(robot_mode_type)) # 获取安全模式类型 safety_mode_type = self.robot.getRobotState().getSafetyModeType() # print("safety_mode_type: ", type(safety_mode_type)) # 假设 pyaubo_sdk.RobotModeType 和 pyaubo_sdk.SafetyModeType 是枚举类型 # 将错误的模式类型代码列出来进行比较 # 修改判断方式以符合枚举类型 if robot_mode_type in [pyaubo_sdk.RobotModeType.Disconnected, pyaubo_sdk.RobotModeType.PowerOff, pyaubo_sdk.RobotModeType.BackDrive, pyaubo_sdk.RobotModeType.Error]: print(f"机械臂模式错误: code = {robot_mode_type}") self.power_off() return -2 if safety_mode_type in [pyaubo_sdk.SafetyModeType.Undefined, pyaubo_sdk.SafetyModeType.Recovery, pyaubo_sdk.SafetyModeType.Violation, pyaubo_sdk.SafetyModeType.ProtectiveStop, pyaubo_sdk.SafetyModeType.SafeguardStop, pyaubo_sdk.SafetyModeType.SystemEmergencyStop, pyaubo_sdk.SafetyModeType.RobotEmergencyStop, pyaubo_sdk.SafetyModeType.Fault]: print(f"机械臂安全模式错误: code = {safety_mode_type}") self.power_off() return -2 if id != id1: break time.sleep(0.05) return 0 if __name__ == '__main__': auboC5 = AuboC5() auboC5.power_off()