微调了现有其他接口，将伺服改为只发，不需要接受返回数据

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):
+    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=0.008,aheadtime=20,gain=200)"
+        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.sendRecvMsg(tcp_command)
+        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):
-        # 位姿伺服
+        poses_list = list(map(float, poses_list))
-        recvmovemess = self.dashboard.ServoP(*poses_list,t,aheadtime,gain)
+        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})"
-        print("ServoP:",recvmovemess)
+        self.dashboard.send_data(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
-        print(self.parseResultId(recvmovemess))
+        print("ServoP:",tcp_command)
-        currentCommandID = self.parseResultId(recvmovemess)[1]
+        if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
-        print("指令 ID", currentCommandID)
+            self.dashboard.Stop()
-        while True:
+            self.dashboard.EmergencyStop()
-            currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
+            print("当前工作停止，机械臂处于急停状态")
-            if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
+        if self.feedbackData.robotMode == 10: # 10 当前工程暂停
-                print("完成一次伺服动作")
+            print("机械臂为暂停状态")
-                break
+            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()