From 333aa7a56a5f585ccf69b04c4dd9086fb645e1a6 Mon Sep 17 00:00:00 2001 From: "ziwei.he" Date: Mon, 12 May 2025 10:30:34 +0800 Subject: [PATCH] =?UTF-8?q?=E5=BE=AE=E8=B0=83=E4=BA=86=E7=8E=B0=E6=9C=89?= =?UTF-8?q?=E5=85=B6=E4=BB=96=E6=8E=A5=E5=8F=A3=EF=BC=8C=E5=B0=86=E4=BC=BA?= =?UTF-8?q?=E6=9C=8D=E6=94=B9=E4=B8=BA=E5=8F=AA=E5=8F=91=EF=BC=8C=E4=B8=8D?= =?UTF-8?q?=E9=9C=80=E8=A6=81=E6=8E=A5=E5=8F=97=E8=BF=94=E5=9B=9E=E6=95=B0?= =?UTF-8?q?=E6=8D=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../MassageControl/hardware/dobot_nova5.py | 97 ++++++++++++++----- 1 file changed, 75 insertions(+), 22 deletions(-) diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index 05624cb..92f3d10 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/Massage/MassageControl/hardware/dobot_nova5.py @@ -4,6 +4,31 @@ from time import sleep,time import re import numpy as np from scipy.spatial.transform import Rotation as R +''' + + MODE DESCRIPTION CN + + 1 INIT 初始化 + 2 BREAK_OPEN 抱闸松开 + 3 POWER_OFF 本体下电 + 4 DISABLED 未使能(抱闸未松) + 5 ENABLE 使能(空闲) + 6 BACKDRIVE 拖拽 + 7 RUNNING 运行状态(脚本/TCP) + 8 SINGLE_MOVE 单次运动(点动) + 9 ERROR 错误状态 + 10 PAUSE 暂停状态 + 11 COLLISION 碰撞状态 + + MODE RANK + + 报错 I + 下电 II + 碰撞 III + 开抱闸 IV + +''' + class dobot_nova5: def __init__(self,ip): self.ip = ip @@ -78,7 +103,7 @@ class dobot_nova5: print(self.parseResultId(recvmovemess)) currentCommandID = self.parseResultId(recvmovemess)[1] print("指令 ID:", currentCommandID) - # sleep(0.02) + sleep(0.02) while True: # 完成判断循环 ########## 调试注释 ############# # print("robot mode",self.feedbackData.robotMode) @@ -86,6 +111,16 @@ class dobot_nova5: # print("指令 ID:", currentCommandID) ########## 调试注释 ############# currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1] + if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: + self.dashboard.Stop() + self.dashboard.EmergencyStop() + print("当前工作停止，机械臂处于急停状态") + if self.feedbackData.robotMode == 10: # 10 当前工程暂停 + print("机械臂为暂停状态") + res = self.dashboard.Continue() + code = int(res[0]) + if code == 0: + print("机械臂继续已暂停的运动指令") if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID: print("该次运动完成") break @@ -103,7 +138,6 @@ class dobot_nova5: currentCommandID = self.parseResultId(recvmovemess)[1] print("指令 ID:", currentCommandID) sleep(0.02) - start_time = time() # 记录开始时间 while True: # 完成判断循环 ########## 调试注释 ############# # print("robot mode",self.feedbackData.robotMode) @@ -116,7 +150,6 @@ class dobot_nova5: self.dashboard.Stop() self.dashboard.EmergencyStop() print("当前工作停止，机械臂处于急停状态") - if self.feedbackData.robotMode == 10: # 10 当前工程暂停 print("机械臂为暂停状态") res = self.dashboard.Continue() @@ -141,6 +174,16 @@ class dobot_nova5: print("指令 ID:", currentCommandID) while True: currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1] + if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: + self.dashboard.Stop() + self.dashboard.EmergencyStop() + print("当前工作停止，机械臂处于急停状态") + if self.feedbackData.robotMode == 10: # 10 当前工程暂停 + print("机械臂为暂停状态") + res = self.dashboard.Continue() + code = int(res[0]) + if code == 0: + print("机械臂继续已暂停的运动指令") if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID: print("该次运动完成") break @@ -159,14 +202,24 @@ class dobot_nova5: print("指令 ID:", currentCommandID) while True: currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1] + if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: + self.dashboard.Stop() + self.dashboard.EmergencyStop() + print("当前工作停止，机械臂处于急停状态") + if self.feedbackData.robotMode == 10: # 10 当前工程暂停 + print("机械臂为暂停状态") + res = self.dashboard.Continue() + code = int(res[0]) + if code == 0: + print("机械臂继续已暂停的运动指令") if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID: print("该次运动完成") break sleep(0.1) - def ServoJoint(self,joints_list): - tcp_command = f"servoj({joints_list[0]},{joints_list[1]},{joints_list[2]},{joints_list[3]},{joints_list[4]},{joints_list[5]},t=0.008,aheadtime=20,gain=200)" - self.dashboard.sendRecvMsg(tcp_command) + def ServoJoint(self,joints_list,t=-1,aheadtime=-1,gain=-1): + tcp_command = f"servoj({joints_list[0]},{joints_list[1]},{joints_list[2]},{joints_list[3]},{joints_list[4]},{joints_list[5]},t={t},aheadtime={aheadtime},gain={gain})" + self.dashboard.send_data(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收 print("ServoJ:",tcp_command) if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: self.dashboard.Stop() @@ -179,19 +232,21 @@ class dobot_nova5: if code == 0: print("机械臂继续已暂停的运动指令") - def ServoPose(self,poses_list,t=-1,aheadtime=-1,gain=-1): - # 位姿伺服 - recvmovemess = self.dashboard.ServoP(*poses_list,t,aheadtime,gain) - print("ServoP:",recvmovemess) - print(self.parseResultId(recvmovemess)) - currentCommandID = self.parseResultId(recvmovemess)[1] - print("指令 ID", currentCommandID) - while True: - currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1] - if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID: - print("完成一次伺服动作") - break + poses_list = list(map(float, poses_list)) + tcp_command = f"ServoP({poses_list[0]},{poses_list[1]},{poses_list[2]},{poses_list[3]},{poses_list[4]},{poses_list[5]}, t={t}, aheadtime={aheadtime}, gain={gain})" + self.dashboard.send_data(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收 + print("ServoP:",tcp_command) + if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: + self.dashboard.Stop() + self.dashboard.EmergencyStop() + print("当前工作停止，机械臂处于急停状态") + if self.feedbackData.robotMode == 10: # 10 当前工程暂停 + print("机械臂为暂停状态") + res = self.dashboard.Continue() + code = int(res[0]) + if code == 0: + print("机械臂继续已暂停的运动指令") def setEndEffector(self,i,tool_i): ''' @@ -377,8 +432,7 @@ class dobot_nova5: del self.dashboard del self.feedFour -######### - + # 适配中间层接口 def get_arm_position(self): pose = self.getPose() x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree @@ -386,7 +440,6 @@ class dobot_nova5: quat_rot = R.from_euler('xyz', [rx, ry, rz], degrees=True).as_quat() return pos,quat_rot -######### if __name__ == "__main__": @@ -399,7 +452,7 @@ if __name__ == "__main__": for i in range(400): posJ[2] += 0.04 - dobot.ServoJoint(posJ) + dobot.ServoJoint(posJ,t=0.008,aheadtime=20,gain=200) sleep(0.007) dobot.disableRobot() \ No newline at end of file