262 lines
12 KiB
Python
Executable File
262 lines
12 KiB
Python
Executable File
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": "机械臂处于锁定状态<br>请解除锁定后再使用"}
|
||
)
|
||
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() |