stash

Massage/Massage.py

@@ -1102,7 +1102,8 @@ class SharedResources:
         self.logger.log_yellow("运动到拍照位置")
         self.robot.arm.disable_servo()
         time.sleep(0.5) 
-        code = self.robot.arm.move_joint(self.robot.arm.cam_pose, max_retry_count = 4,wait=True)
+        # code = self.robot.arm.move_joint(self.robot.arm.cam_pose, max_retry_count = 4,wait=True)
+        code = self.robot.set_joint(joint = self.robot.arm.cam_pose)
         if code == -1:
             self.logger.log_error("运动到拍照位置失败")
             self.stop_event.set()
@@ -1352,7 +1353,8 @@ class SharedResources:
         self.robot.is_waitting = True
         self.robot.arm.disable_servo()
         time.sleep(1)
-        code = self.robot.arm.move_joint(pose, max_retry_count = 4)
+        # code = self.robot.arm.move_joint(pose, max_retry_count = 4)
+        code = self.robot.set_joint(joint = pose)
         if code == -1:
             self.logger.log_error("运动到standby位置失败")
             self.stop_event.set()
@@ -2720,6 +2722,8 @@ class Stop(smach.State):
         time.sleep(1)
         self.resources.robot.arm.disable_servo()
         self.resources.robot.arm.disable_servo()
+        self.resources.robot.stop_thread()
+
         if self.resources.next_points is not None:
             time.sleep(1)
             position,quat_rot = self.resources.robot.arm.get_arm_position()
@@ -2727,21 +2731,32 @@ class Stop(smach.State):
             target_point = np.zeros(6)
             target_point[:3] = copy.deepcopy(position)
             target_point[3:] = copy.deepcopy(R.from_quat(quat_rot).as_euler('xyz',degrees=False))
+            # target_point[:3] = copy.deepcopy(self.resources.robot.arm_state.arm_position_command)
+            # target_point[3:] = copy.deepcopy(R.from_quat(self.resources.robot.arm_state.arm_orientation_command).as_euler('xyz',degrees=False))
             # target_point[3:] = [150 * (np.pi / 180), 0, 180 * (np.pi / 180)]
             target_point[:3] = target_point[:3] + self.resources.tf2base([0, 0, -0.10], self.resources.robot.arm.level_base)
-            self.resources.logger.log_blue(f"set_position: {target_point}")
+            self.resources.logger.log_blue(f"set_position: {target_point},{position}")
             code = self.resources.robot.set_position(pose=target_point,is_wait=True)
+
         self.resources.next_points = None
         time.sleep(1)
         pose = copy.deepcopy(self.resources.robot.arm.standby_pos)
         self.resources.logger.log_yellow("self.resources.robot.arm.disable_servo()")
         time.sleep(1)
         if not self.resources.is_pause:
-            self.resources.robot.arm.move_joint(pose)
+            # self.resources.robot.arm.move_joint(pose)
+            code = self.resources.robot.set_joint(pose)
+            if code == -1:
+                self.resources.logger.log_error(f"运动到pose:{pose}位置失败")
+                return "emergency"
         pose = copy.deepcopy(self.resources.robot.arm.init_pos)
         time.sleep(0.1)
         if not self.resources.is_pause:
-            self.resources.robot.arm.move_joint(pose)
+            # self.resources.robot.arm.move_joint(pose)
+            code = self.resources.robot.set_joint(pose)
+            if code == -1:
+                self.resources.logger.log_error(f"运动到pose:{pose}位置失败")
+                return "emergency"
         time.sleep(1)
 
         if self.resources.choose_task == "thermotherapy":
@@ -2858,6 +2873,8 @@ class Pause(smach.State):
         self.resources.robot.is_waitting = True
         time.sleep(1)
         self.resources.robot.arm.disable_servo()
+        self.resources.robot.stop_thread()
+
         if self.resources.next_points is not None:
             time.sleep(1)
             position,quat_rot = self.resources.robot.arm.get_arm_position()
@@ -2865,18 +2882,29 @@ class Pause(smach.State):
             target_point = np.zeros(6)
             target_point[:3] = copy.deepcopy(position)
             target_point[3:] = copy.deepcopy(R.from_quat(quat_rot).as_euler('xyz',degrees=False))
+            # target_point[:3] = copy.deepcopy(self.resources.robot.arm_state.arm_position_command)
+            # target_point[3:] = copy.deepcopy(R.from_quat(self.resources.robot.arm_state.arm_orientation_command).as_euler('xyz',degrees=False))
             # target_point[3:] = [150 * (np.pi / 180), 0, 180 * (np.pi / 180)]
             target_point[:3] = target_point[:3] + self.resources.tf2base([0, 0, -0.15], self.resources.robot.arm.level_base)
             self.resources.logger.log_blue(f"set_position: {target_point}")
             code = self.resources.robot.set_position(pose=target_point,is_wait=True)
+
         self.resources.next_points = None
         time.sleep(1)
         pose = copy.deepcopy(self.resources.robot.arm.standby_pos)
         time.sleep(0.1)
-        self.resources.robot.arm.move_joint(pose)
+        # self.resources.robot.arm.move_joint(pose)
+        code = self.resources.robot.set_joint(pose)
+        if code == -1:
+            self.resources.logger.log_error(f"运动到pose:{pose}位置失败")
+            return "emergency"
         pose = copy.deepcopy(self.resources.robot.arm.init_pos)
         time.sleep(0.1)
-        self.resources.robot.arm.move_joint(pose)
+        # self.resources.robot.arm.`move_joint`(pose)
+        code = self.resources.robot.set_joint(pose)
+        if code == -1:
+            self.resources.logger.log_error(f"运动到pose:{pose}位置失败")
+            return "emergency"
         time.sleep(1)
 
         if self.resources.choose_task == "thermotherapy":
@@ -3036,6 +3064,7 @@ class EmergencyExitState(smach.State):
         self.resources.send_instruction(instruction="emergency_stop", target="language")
         # 这里可以处理一些清理工作
         self.resources.logger.log_error("Emergency exit triggered, shutting down...")
+        self.resources.robot.stop_thread()
         self.resources.robot.stop()
         # self.resources.robot.arm.power_off()
         self.resources.stop_event.clear()

Massage/MassageControl/MassageRobot_nova5.py

@@ -525,7 +525,7 @@ class MassageRobot:
                     if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停
                         print("机械臂为暂停状态")
                         res = self.arm.dashboard.Continue()
-                        code = self.arm.parseResultId(res)[0]
+                        code = self.arm.parseResultprocess(res)[0]
                         if code == 0:
                             print("机械臂继续已暂停的运动指令")
 
@@ -590,7 +590,7 @@ class MassageRobot:
                         if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停
                             print("机械臂为暂停状态")
                             res = self.arm.dashboard.Continue()
-                            code = self.arm.parseResultId(res)[0]
+                            code = self.arm.parseResultprocess(res)[0]
                             if code == 0:
                                 print("机械臂继续已暂停的运动指令")
 
@@ -643,24 +643,44 @@ class MassageRobot:
             
             self.logger.log_info("MassageRobot启动")
             time.sleep(0.5)
-            
-    def stop(self):
+
+    def stop_thread(self):
         if not self.exit_event.is_set():
             self.exit_event.set()
             self.interrupt_event.clear()
             self.arm_control_thread.join()
             self.arm_measure_thread.join()
-            for i in range(3):
-                self.force_sensor.disable_active_transmission()
-            self.force_sensor.start_background_reading()
-            self.arm.stop_motion()
-            self.logger.log_info("MassageRobot停止")
-            
 
-            self.controller_manager.switch_controller('position')
+    def stop(self):
+        for i in range(3):
+            self.force_sensor.disable_active_transmission()
+        self.force_sensor.start_background_reading()
+        self.arm.stop_motion()
+        self.logger.log_info("MassageRobot停止")
+        
+
+        self.controller_manager.switch_controller('position')
         # else:
         #     self.logger.log_error(f"未找到{name}按摩头")
 
+
+    # def stop(self):
+    #     if not self.exit_event.is_set():
+    #         self.exit_event.set()
+    #         self.interrupt_event.clear()
+    #         self.arm_control_thread.join()
+    #         self.arm_measure_thread.join()
+    #         for i in range(3):
+    #             self.force_sensor.disable_active_transmission()
+    #         self.force_sensor.start_background_reading()
+    #         self.arm.stop_motion()
+    #         self.logger.log_info("MassageRobot停止")
+            
+
+    #         self.controller_manager.switch_controller('position')
+    #     # else:
+    #     #     self.logger.log_error(f"未找到{name}按摩头")
+
     def log_thread(self):
         while True:
             self.logger_data.log_info(f"机械臂位置:{self.arm_state.arm_position},机械臂姿态:{self.arm_state.arm_orientation}",is_print=False)
@@ -688,20 +708,31 @@ class MassageRobot:
     
     @track_function("set_position",True)
     def set_position(self,pose,is_wait = False):
-        print("self.robotmode1:",self.arm.feedbackData.robotMode)
-        self.logger.log_info(f"set postion:{pose}")
+
         x,y,z = pose[0],pose[1],pose[2]
         roll,pitch,yaw = pose[3],pose[4],pose[5]
         pose_command = np.concatenate([np.array([x,y,z]), np.array([roll,pitch,yaw])])
-        time.sleep(0.1)
-        print("self.robotmode2:",self.arm.feedbackData.robotMode)
-        # inverse_joint = self.arm.getInverseKin(poses_list = pose_command,tool = self.tool_num)
 
-        code = self.arm.RunPoint_P_inPose_M_RAD(pose = pose_command)
+        temp_pose = copy.deepcopy(pose_command)
+        pose_processed = copy.deepcopy(pose_command)
+        target_matrix = R.from_euler('xyz', temp_pose[3:], degrees=False).as_matrix() @ self.arm.tcp_euler_inv
+        pose_processed[:3] = temp_pose[:3] - target_matrix @ np.array(self.arm.tcp_offset[:3])
+        pose_processed[3:] = R.from_matrix(target_matrix).as_euler("xyz",degrees = False)
+    
+        self.logger.log_info(f"set postion:{pose},{pose_processed}")
+
         time.sleep(0.1)
-        # print("inverse_joint:",inverse_joint,np.array(inverse_joint)*np.pi/180)
-        # code = self.arm.move_joint(q = np.array(inverse_joint)*np.pi/180)
-        # return code
+        code = self.arm.RunPoint_P_inPose_M_RAD(pose = pose_processed)
+        time.sleep(0.1)
+        return code
+    
+    @track_function("set_joint",True)
+    def set_joint(self,joint,is_wait = False):
+        self.logger.log_info(f"set joint:{joint}")
+        time.sleep(0.1)
+        code = self.arm.move_joint(q = joint)
+        time.sleep(0.1)
+        return code
 
     @track_function("move_to_point",True)
     def move_to_point(self, t, pose, is_interrupt=True, wait=True, timeout=0.5, interpolation:Literal["linear","circle","cloud_point"] ='linear', algorithm:Literal["admittance","position","hybrid","admithybrid","hybridPid","hybridAdmit","admittanceZ"] = "admittance",is_switch_controller = False):
@@ -1718,25 +1749,27 @@ if __name__ == "__main__":
     time.sleep(3)
     robot.switch_payload("finger_head")
     robot.controller_manager.switch_controller('admittance')
-    position,quat_rot = robot.arm.get_arm_position()
-    euler_rot = R.from_quat(quat_rot).as_euler('xyz')
-    # target_point = [position[0], position[1], position[2], euler_rot[0], euler_rot[1], euler_rot[2]]
-    target_point = [0.12191350715303009, -0.1636467057977184, 0.007338312730729687, 8.37758040957278e-05, 1.5708137800874167, -1.5708329787091884]
+    # position,quat_rot = robot.arm.get_arm_position()
+    # euler_rot = R.from_quat(quat_rot).as_euler('xyz')
+    # # target_point = [position[0], position[1], position[2], euler_rot[0], euler_rot[1], euler_rot[2]]
+    # target_point = [0.12191350715303009, -0.1636467057977184, 0.007338312730729687, 8.37758040957278e-05, 1.5708137800874167, -1.5708329787091884]
     
-    # target_point = [0.3543467,-0.1349880,0.4553604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
-    # robot.move_to_point(pose=target_point,t=60)
-    # robot.arm.move_joint(q = robot.arm.off_pos)
-    # robot.arm.move_joint(q = robot.arm.init_pos)
-    pose1 = [0.353467,-0.1349880,0.403604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
-    # robot.set_position(pose = pose1)
-    print("arm_position1:",robot.arm.get_arm_position())
-    print("end_position1:",robot.arm.get_end_position())
-    print("joint1:",robot.arm.getAngel())
-    robot.arm.move_joint(q=robot.arm.init_pos)
-    # robot.set_position([0.12189544299531646, -0.31364670468715944, 0.00733569473686102, 2.153932270623249, 0.6209756952505509, 0.0])
-    print("arm_position2:",robot.arm.get_arm_position())
-    print("end_position2:",robot.arm.get_end_position())
-    print("joint2:",robot.arm.getAngel())
+    # # target_point = [0.3543467,-0.1349880,0.4553604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
+    # # robot.move_to_point(pose=target_point,t=60)
+    # # robot.arm.move_joint(q = robot.arm.off_pos)
+    # # robot.arm.move_joint(q = robot.arm.init_pos)
+    # pose1 = [0.353467,-0.1349880,0.403604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
+    # # robot.set_position(pose = pose1)
+    # print("arm_position1:",robot.arm.get_arm_position())
+    # print("end_position1:",robot.arm.get_end_position())
+    # print("joint1:",robot.arm.getAngel())
+    # robot.arm.move_joint(q=robot.arm.init_pos)
+    # # robot.set_position([0.12189544299531646, -0.31364670468715944, 0.00733569473686102, 2.153932270623249, 0.6209756952505509, 0.0])
+    # print("arm_position2:",robot.arm.get_arm_position())
+    # print("end_position2:",robot.arm.get_end_position())
+    # print("joint2:",robot.arm.getAngel())
+
+    pose={593.358197,165.740423,216.449797,-173.468246,-33.240047,2.178380}
 
 
     # robot.controller_manager.switch_controller('position')

Massage/MassageControl/hardware/dobot_api.py

@@ -6,6 +6,7 @@ import json
 import threading
 import time
 from time import sleep
+import select
 
 alarmControllerFile = "files/alarmController.json"
 alarmServoFile = "files/alarmServo.json"
@@ -172,6 +173,22 @@ class DobotApi:
                 data_str = str(data, encoding="utf-8")
             # self.log(f'Receive from {self.ip}:{self.port}: {data_str}')
             return data_str
+        
+    def flush_recv_buffer(self):
+        """非阻塞清空接收缓冲区"""
+        self.socket_dobot.setblocking(0)
+        try:
+            while True:
+                ready = select.select([self.socket_dobot], [], [], 0.01)
+                if ready[0]:
+                    try:
+                        self.socket_dobot.recv(1024)
+                    except:
+                        break
+                else:
+                    break
+        finally:
+            self.socket_dobot.setblocking(1)
 
     def close(self):
         """
@@ -209,7 +226,8 @@ class DobotApi:
         with self.__globalLock:
             self.send_data(string)
             recvData = self.wait_reply()
-            self.ParseResultId(recvData)
+            if not self.ParseResultId(recvData):
+                return None
             return recvData
 
     def __del__(self):
@@ -1908,6 +1926,7 @@ class DobotApiDashboard(DobotApi):
         for ii in params:
             string = string + ','+ii
         string = string + ')'
+        self.flush_recv_buffer()
         return self.sendRecvMsg(string)
 
     def MovL(self, a1, b1, c1, d1, e1, f1, coordinateMode, user=-1, tool=-1, a=-1, v=-1, speed=-1, cp=-1, r=-1):
@@ -2768,11 +2787,12 @@ class DobotApiDashboard(DobotApi):
         """
         if valueRecv.find("Not Tcp") != -1:  # 通过返回值判断机器是否处于tcp模式 Judge whether the robot is in TCP mode by the return value
             print("Control Mode Is Not Tcp")
-            return
+            return False
         recvData = re.findall(r'-?\d+', valueRecv)
         recvData = [int(num) for num in recvData]
         if len(recvData) > 0:
             if recvData[0] != 0:
+                print("recvData is ", recvData)
                 # 根据返回值来判断机器处于什么状态 Judge what status the robot is in based on the return value
                 if recvData[0] == -1:
                     print("Command execution failed")
@@ -2784,8 +2804,11 @@ class DobotApiDashboard(DobotApi):
                     print("The robot is in power down state")
                 else:
                     print("ErrorId is ", recvData[0])
+                return False
         else:
             print("ERROR VALUE")
+            return False
+        return True
 
     ###################################460新增#############################
     

Massage/MassageControl/hardware/dobot_nova5.py

@@ -105,7 +105,7 @@ class dobot_nova5:
                         # stopId = self.dashboard.Stop()
                         time.sleep(0.5)
                         print("self.self.feedbackData.robotMode:",self.feedbackData.robotMode)
-                        if (self.feedbackData.robotMode == 3 or self.feedbackData.robotMode == 4): #and self.parseResultId(stopId)[0] == 0:
+                        if (self.feedbackData.robotMode == 3 or self.feedbackData.robotMode == 4): #and self.parseResultprocess(stopId)[0] == 0:
                             # self.dashboard.RequestControl()
                             if self.feedbackData.robotMode == 3:
                                 self.dashboard.PowerOn() # 复位
@@ -118,7 +118,7 @@ class dobot_nova5:
                                 cnt_num += 1
                                 time.sleep(1)
                         else:
-                            if self.feedbackData.robotMode == 9:# and self.parseResultId(stopId)[0] == -3:
+                            if self.feedbackData.robotMode == 9:# and self.parseResultprocess(stopId)[0] == -3:
                                 print(robot_mode_types.get(self.feedbackData.robotMode, "未知模式"))
 
                                 requests.post(
@@ -152,7 +152,7 @@ class dobot_nova5:
         atexit.register(self.exit_task)
         self.chooseRightFrame()
         self.init_pos = [-45.0009079*np.pi/180,55.5785141*np.pi/180,-120.68821716*np.pi/180,5.11103201*np.pi/180,90.00195312*np.pi/180,-90.00085449*np.pi/180]
-        self.standby_pos = [-45.0009079*np.pi/180,55.5785141*np.pi/180,-120.68821716*np.pi/180,5.11103201*np.pi/180,90.00195312*np.pi/180,-90.00085449*np.pi/180]
+        self.standby_pos = [-35.0009079*np.pi/180,45.5785141*np.pi/180,-110.68821716*np.pi/180,2.11103201*np.pi/180,80.00195312*np.pi/180,-80.00085449*np.pi/180]
         self.off_pos = [-44.996*np.pi/180,87.0092*np.pi/180,-147.0087*np.pi/180,-0.0048*np.pi/180,90.0010*np.pi/180,-90.0021*np.pi/180]
         self.cam_pose = [-45.0050*np.pi/180,30.0682*np.pi/180,-66.1887*np.pi/180,-21.8836*np.pi/180,-90.3064*np.pi/180,-89.9977*np.pi/180]
         self.cam_length = 1.75
@@ -176,19 +176,19 @@ class dobot_nova5:
         self.filter_matrix = np.zeros((1,6)) # 角度伺服用
         sleep(1)
         self.is_standby = False
-        self.move_joint_jog(q=self.init_pos)
+        self.move_joint_jog_range(q=np.zeros(6))
         time.sleep(0.5)
         code = self.move_joint(self.init_pos)
 
         if code == 0:
-            if self.parseResultId(self.dashboard.SetCollisionLevel(level=1))[0] == 0:
+            if self.parseResultprocess(self.dashboard.SetCollisionLevel(level=1))[0] == 0:
                 self.logger.log_info("机械臂初始化")
             else:
                 time.sleep(0.2)
                 max_retries = 5
                 retry_count = 0
                 while retry_count < max_retries:
-                    if self.parseResultId(self.dashboard.SetCollisionLevel(level=1))[0] == 0:
+                    if self.parseResultprocess(self.dashboard.SetCollisionLevel(level=1))[0] == 0:
                         print("retry_count",retry_count)
                         break
                     else:
@@ -235,7 +235,10 @@ class dobot_nova5:
         self.dashboard.Disconnect()
         self.feedFour.Disconnect()
 
-    def parseResultId(self,valueRecv):
+    def parseResultprocess(self,valueRecv):
+        if valueRecv is None:
+            print("Empty response or None received")
+            return [1]
         # 解析返回值
         if "Not Tcp" in valueRecv:
             print("Control Mode is not TCP")
@@ -274,10 +277,10 @@ class dobot_nova5:
         while cnt < max_try:
             cnt += 1
             # 关闭碰撞检测
-            # if self.parseResultId(self.dashboard.SetCollisionLevel(level=0))[0] == 0:
+            # if self.parseResultprocess(self.dashboard.SetCollisionLevel(level=0))[0] == 0:
             time.sleep(0.1)
             self.logger.log_info("关闭碰撞检测成功")
-            code = self.parseResultId(self.dashboard.EnableRobot())[0]
+            code = self.parseResultprocess(self.dashboard.EnableRobot())[0]
             if code == 0:
                 while 1:
                     if self.feedbackData.robotMode in [3, 6, 7, 8, 9, 10, 11]:
@@ -289,15 +292,16 @@ class dobot_nova5:
                     time.sleep(0.1)
     
     def disable_servo(self):
+        # time.sleep(10)
         pass
 
     def enable_servo(self):
         pass
     
     def waitArrival(self, command):
-        sendCommandID = self.parseResultId(command)[1]
+        sendCommandID = self.parseResultprocess(command)[1]
         # print("self.feedbackData.robotMode6:",self.feedbackData.robotMode)
-        # print("self.parseResultId(command)",self.parseResultId(command))
+        # print("self.parseResultprocess(command)",self.parseResultprocess(command))
         while True:
             if self.feedbackData.robotMode in [3, 4, 6, 9, 10, 11]: 
                 self.logger.log_error(f"机械臂异常:{self.feedbackData.robotMode}")
@@ -328,6 +332,8 @@ class dobot_nova5:
         while cnt < max_retry_count:
             cnt += 1
             command = self.dashboard.MovJ(*joints_list_deg,1)
+            if command == None:
+                return -1
 
             is_arrived = self.waitArrival(command)
             if is_arrived == 0:
@@ -341,12 +347,37 @@ class dobot_nova5:
         print(f'After {cnt} retries, still failed to arrive at the joint position: {q}')
         return -1
 
-    def move_joint_jog(self,q,**params):
+    # def move_joint_jog(self,q,**params):
+    #     '''
+    #         move_joint_jog 以JOINT jOG运动方式运动到目标点
+    #         MovJ flag = 1 通过关节角度走点，输入为[J1 J2 J3 J4 J5 J6]  unit: rad
+    #     '''
+    #     threshold = params.get('threshold', 10)
+    #     currentAngle = self.getAngel()
+    #     targetAngle = self.__transform_rad_2_deg(q)
+    #     currentAngle = np.array(currentAngle)
+    #     targetAngle = np.array(targetAngle)
+    #     self.dashboard.SpeedFactor(50)
+
+    #     for i in range(len(targetAngle)):
+    #         while np.abs(currentAngle[i] - targetAngle[i]) >threshold:
+    #             if currentAngle[i] < targetAngle[i]:
+    #                 self.dashboard.MoveJog(f"J{i+1}+")
+    #             else:
+    #                 self.dashboard.MoveJog(f"J{i+1}-")
+    #             time.sleep(0.5)
+
+    #             currentAngle = self.getAngel()
+    #             currentAngle = np.array(currentAngle)
+    #         self.dashboard.MoveJog("")
+    #     # self.dashboard.SpeedFactor(95)
+
+    def move_joint_jog_range(self,q,**params):
         '''
             move_joint_jog 以JOINT jOG运动方式运动到目标点
             MovJ flag = 1 通过关节角度走点，输入为[J1 J2 J3 J4 J5 J6]  unit: rad
         '''
-        threshold = params.get('threshold', 10)
+        threshold = params.get('threshold', np.array([350,170,155,350,350,350]))
         currentAngle = self.getAngel()
         targetAngle = self.__transform_rad_2_deg(q)
         currentAngle = np.array(currentAngle)
@@ -354,7 +385,7 @@ class dobot_nova5:
         self.dashboard.SpeedFactor(50)
 
         for i in range(len(targetAngle)):
-            while np.abs(currentAngle[i] - targetAngle[i]) >threshold:
+            while np.abs(np.abs(currentAngle[i]) - targetAngle[i]) >threshold[i]:
                 if currentAngle[i] < targetAngle[i]:
                     self.dashboard.MoveJog(f"J{i+1}+")
                 else:
@@ -364,7 +395,6 @@ class dobot_nova5:
                 currentAngle = self.getAngel()
                 currentAngle = np.array(currentAngle)
             self.dashboard.MoveJog("")
-        # self.dashboard.SpeedFactor(95)
 
     def RunPoint_P_inPose_M_RAD(self,pose,**params):
         '''
@@ -380,6 +410,8 @@ class dobot_nova5:
             cnt += 1
             print("self.robotmode4:",self.feedbackData.robotMode)
             command = self.dashboard.MovJ(*poses_list_mm_deg,0)
+            if command == None:
+                return -1
             print("self.robotmode5:",self.feedbackData.robotMode)
 
             is_arrived = self.waitArrival(command = command)
@@ -409,7 +441,7 @@ class dobot_nova5:
         if self.feedbackData.robotMode == 10: # 10 当前工程暂停
             print("机械臂为暂停状态")
             res = self.dashboard.Continue()
-            code = self.parseResultId(res)[0]
+            code = self.parseResultprocess(res)[0]
             if code == 0:
                 print("机械臂继续已暂停的运动指令")
         return 0
@@ -433,7 +465,7 @@ class dobot_nova5:
         if self.feedbackData.robotMode == 10: # 10 当前工程暂停
             print("机械臂为暂停状态")
             res = self.dashboard.Continue()
-            code = self.parseResultId(res)[0]
+            code = self.parseResultprocess(res)[0]
             if code == 0:
                 print("机械臂继续已暂停的运动指令")
         return 0
@@ -455,7 +487,7 @@ class dobot_nova5:
         if self.feedbackData.robotMode == 10: # 10 当前工程暂停
             print("机械臂为暂停状态")
             res = self.dashboard.Continue()
-            code = self.parseResultId(res)[0]
+            code = self.parseResultprocess(res)[0]
             if code == 0:
                 print("机械臂继续已暂停的运动指令")
     
@@ -479,7 +511,7 @@ class dobot_nova5:
         if self.feedbackData.robotMode == 10: # 10 当前工程暂停
             print("机械臂为暂停状态")
             res = self.dashboard.Continue()
-            code = self.parseResultId(res)[0]
+            code = self.parseResultprocess(res)[0]
             if code == 0:
                 print("机械臂继续已暂停的运动指令")
 
@@ -498,7 +530,7 @@ class dobot_nova5:
         
         res = self.dashboard.SetTool(i,tool_i)
         print(res)
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             print(f"设置工具坐标系{i}的偏移量成功")
         else:
@@ -514,7 +546,7 @@ class dobot_nova5:
             return
         res = self.dashboard.Tool(i)
         print(res)
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             print(f"切换为工具坐标系{i}成功")
         else:
@@ -535,7 +567,7 @@ class dobot_nova5:
             return
         
         res = self.dashboard.SetUser(i,base_i)
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             print(f"设置用户坐标系{i}成功")
         else:
@@ -550,7 +582,7 @@ class dobot_nova5:
             print("工具坐标系序号超出范围")
             return
         res = self.dashboard.User(i)
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             print(f"切换为用户基坐标系{i}成功")
         else:
@@ -562,7 +594,7 @@ class dobot_nova5:
             print("必须同时设置或同时不设置坐标系参数")
         res = self.dashboard.GetPose(user,tool) # 返回字符串
         print("get pose",res)
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             pass
             # print("success")
@@ -578,7 +610,7 @@ class dobot_nova5:
     
     def getAngel(self):
         res = self.dashboard.GetAngle()
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             pass
             # print("success")
@@ -595,7 +627,7 @@ class dobot_nova5:
     
     def getForwardKin(self,joints_list,user=-1,tool=-1):
         res = self.dashboard.PositiveKin(*joints_list,user,tool)
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             pass
             # print("success")
@@ -618,7 +650,7 @@ class dobot_nova5:
         '''
         res = self.dashboard.InverseKin(*poses_list,user,tool,useJointNear,JointNear)
         print(res)
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             pass
             # print("success")
@@ -634,7 +666,7 @@ class dobot_nova5:
     
     def disableRobot(self):
         res = self.dashboard.DisableRobot()
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             print("下使能机械臂成功")
         else:
@@ -643,7 +675,7 @@ class dobot_nova5:
     
     def clearError(self):
         res = self.dashboard.ClearError()
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             print("清楚报警成功")
         else:
@@ -652,7 +684,7 @@ class dobot_nova5:
 
     def start_drag(self):
         res = self.dashboard.StartDrag()
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             print("拖拽模式开启成功")
         else:
@@ -661,7 +693,7 @@ class dobot_nova5:
     
     def stop_drag(self):
         res = self.dashboard.StopDrag()
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             print("拖拽模式关闭成功")
         else:
@@ -670,7 +702,7 @@ class dobot_nova5:
     
     def stop_motion(self):
         res = self.dashboard.Stop()
-        code = self.parseResultId(res)[0]
+        code = self.parseResultprocess(res)[0]
         if code == 0:
             print("机械臂停止下发运动指令队列")
         else:
@@ -886,6 +918,7 @@ if __name__ == "__main__":
     
     dobot = dobot_nova5("192.168.5.1")
     dobot.init()
+    dobot.dashboard.LogExportUSB(0)
 
     # # dobot.move_joint_jog(q=dobot.init_pos)
 
@@ -898,14 +931,15 @@ if __name__ == "__main__":
 
     # # # dobot.start()
     # # posJ = [0,30,-120,0,90,0]
-    # # time.sleep(1)
+    time.sleep(1)
+    print('successs!')
     # joint = [-45.0050*np.pi/180,30.0682*np.pi/180,-66.1887*np.pi/180,-21.8836*np.pi/180,-90.3064*np.pi/180,-89.9977*np.pi/180]
     # # dobot.move_joint(q = joint)
     # time.sleep(1)
 
     # joint1 = [-44.8416*np.pi/180, 39.5626*np.pi/180, -80.5683*np.pi/180, -18.0396*np.pi/180, -89.4168*np.pi/180, -90.0026*np.pi/180]
     # try:
-    #     dobot.start_drag()
+    dobot.start_drag()
     #     # dobot.move_joint(q = joint)
     #     # dobot.move_joint(q = joint1)
 
@@ -917,9 +951,11 @@ if __name__ == "__main__":
             time.sleep(1)
     except KeyboardInterrupt:
         # robot_thread.join()
+        dobot.stop_drag()
         print("用户中断操作。")
     except Exception as e:
         # robot_thread.join()
+        dobot.stop_drag()
         print("Exception occurred at line:", e.__traceback__.tb_lineno)
         print("发生异常：", e)
     # # dobot.RunPoint_P_inJoint(posJ)

Massage/test_manual.py

@@ -10,10 +10,10 @@ async def websocket_client():
     async with websockets.connect(WEBSOCKET_URL) as websocket:
         print("Connected to WebSocket server.")
 
-        # # 发送初始化命令
-        # command_1 = "begin:finger:1:back_shoulder:0:manual"
-        # await websocket.send(command_1)
-        # print(f"Sent: {command_1}")
+        # 发送初始化命令
+        command_1 = "begin:finger:1:back_shoulder:0:manual"
+        await websocket.send(command_1)
+        print(f"Sent: {command_1}")
 
         # 发送获取图片命令
         # command_2 = "get_picture"
@@ -30,9 +30,9 @@ async def websocket_client():
         # await websocket.send(command_4)
         # print(f"Sent: {command_4}")
 
-        command_5 = "skip_queue"
-        await websocket.send(command_5)
-        print(f"Sent: {command_5}")
+        # command_5 = "skip_queue"
+        # await websocket.send(command_5)
+        # print(f"Sent: {command_5}")
 
 
 # 主入口