初始化初步异常处理

Massage/MassageControl/MassageRobot_nova5.py

@@ -113,9 +113,14 @@ class MassageRobot:
         self.arm_state = ArmState()
         self.arm_config = read_yaml(arm_config_path)
         # arm 实例化时机械臂类内部进行通讯连接
+        # 初始化坐标系 TOOL=0 BASE=1
         self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip'])
         self.arm.start()
+<<<<<<< HEAD
+        self.arm.chooseRightFrame()
+=======
         self.arm.init()
+>>>>>>> cc63eb47a5fd86a36d8af9fbf38a4b10a4c2ff7f
 
         self.thermotherapy = None
         self.shockwave = None
@@ -140,6 +145,17 @@ class MassageRobot:
         
         self.controller_manager.switch_controller('admittance')
 
+<<<<<<< HEAD
+        # 读取数据
+        self.gravity_base = None
+        self.force_zero = None
+        self.torque_zero = None
+        self.tool_position = None
+        self.mass_center_position = None
+        self.s_tf_matrix_t = None
+        arm_orientation_set0 = np.array([-180,-30,0])
+        self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
+=======
 
 
         
@@ -1614,32 +1630,32 @@ class MassageRobot:
 
 
 if __name__ == "__main__":
-    waypoints = [
+    # waypoints = [
-        {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
+    #     {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
-         "acceleration": [0, 0, 0],
+    #      "acceleration": [0, 0, 0],
-         "orientation": R.from_euler("xyz", [0, 0, 0], degrees=True).as_quat()},
+    #      "orientation": R.from_euler("xyz", [0, 0, 0], degrees=True).as_quat()},
-        {"time": 5.0, "position": [0.30, -0.135, 0.3043], "velocity": [0, 0, 0],
+    #     {"time": 5.0, "position": [0.30, -0.135, 0.3043], "velocity": [0, 0, 0],
-         "acceleration": [0, 0, 0],
+    #      "acceleration": [0, 0, 0],
-         "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
+    #      "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
-    ] ## 单位 m deg
+    # ] ## 单位 m deg
-    myInterpolate = TrajectoryInterpolator(waypoints)
+    # myInterpolate = TrajectoryInterpolator(waypoints)
-    ts = np.linspace(0,5,100)
+    # ts = np.linspace(0,5,100)
     robot = MassageRobot(arm_config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml")
-    xr_list, vr_list, ar_list = [], [], []
+    # xr_list, vr_list, ar_list = [], [], []
-    for t in ts:
+    # for t in ts:
-        xr, vr, ar = myInterpolate.interpolate(t)  
+    #     xr, vr, ar = myInterpolate.interpolate(t)  
-        xr_list.append(xr)
+    #     xr_list.append(xr)
-        vr_list.append(vr)
+    #     vr_list.append(vr)
-        ar_list.append(ar)  
+    #     ar_list.append(ar)  
-    xr_array = np.array(xr_list)
+    # xr_array = np.array(xr_list)
-    vr_array = np.array(vr_list)
+    # vr_array = np.array(vr_list)
-    ar_array = np.array(ar_list)
+    # ar_array = np.array(ar_list)
 
-    robot.xr = xr_array
+    # robot.xr = xr_array
-    robot.vr = vr_array
+    # robot.vr = vr_array
-    robot.ar = ar_array
+    # robot.ar = ar_array
-    robot.ts = ts
+    # robot.ts = ts
-    robot.dt = ts[1] - ts[0]
+    # robot.dt = ts[1] - ts[0]
 
 
     ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]

Massage/MassageControl/hardware/dobot_nova5.py

@@ -1,3 +1,10 @@
+import os
+import sys
+current_file_path = os.path.abspath(__file__)
+dobot_nova5_path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
+print(dobot_nova5_path)
+# 添加目标文件夹到系统路径
+sys.path.append(dobot_nova5_path)
 try:
     from dobot_api import DobotApiFeedBack,DobotApiDashboard
 except:
@@ -10,6 +17,9 @@ import copy
 import numpy as np
 from scipy.spatial.transform import Rotation as R
 import math
+import time
+import requests
+import atexit
 '''
 
     MODE    DESCRIPTION    CN
@@ -36,10 +46,10 @@ import math
 '''
 
 class dobot_nova5:
-    def __init__(self,arm_ip = '192.168.5.1'):
+    def __init__(self,arm_ip = '192.168.5.1',dashboardPort = 29999,feedFourPort = 30004):
         self.ip = arm_ip ## 机械臂IP地址
-        self.dashboardPort = 29999 ## 一发一收-设置-控制及运动端口号
+        self.dashboardPort = dashboardPort ## 一发一收-设置-控制及运动端口号
-        self.feedFourPort = 30004 ## 第四实时反馈(8ms)反馈数据端口号
+        self.feedFourPort = feedFourPort ## 第四实时反馈(8ms)反馈数据端口号
         self.dashboard = None ## 初始化控制及运动端口实例
         self.feedFour = None ## 初始化数据反馈端口实例
         self.feedInfo = []
@@ -62,6 +72,79 @@ class dobot_nova5:
                 '''
         self.feedbackData = feedbackItem() # 自定义反馈数据结构体
         self.logger = CustomLogger('Dobot_nova5',True)
+        self.dashboard = DobotApiDashboard(self.ip,self.dashboardPort)
+        self.feedFour = DobotApiFeedBack(self.ip,self.feedFourPort)
+
+        self.stop_feedback = threading.Event()
+        self.stop_feedback.clear()
+        # 状态反馈线程
+        self.feedback_thread = threading.Thread(
+            target=self.GetFeedback # 机器状态反馈线程
+        )
+        self.feedback_thread.daemon = True
+        self.feedback_thread.start()
+        time.sleep(1)
+        max_retry_num = 5  #尝试连接次数
+        cnt_num = 0
+        while cnt_num < max_retry_num:
+            if self.feedbackData.robotMode != -1:
+                self.dashboard.EmergencyStop(mode=0) # 松开急停
+                self.clearError() # 清除报警
+                stopId = self.dashboard.Stop()
+                time.sleep(0.5)
+                if (self.feedbackData.robotMode == 3 or self.feedbackData.robotMode == 4) and self.parseResultId(stopId)[0] == 0:
+                    self.dashboard.RequestControl()
+                    if self.feedbackData.robotMode == 3:
+                        self.dashboard.PowerOn() # 复位
+                    self.dashboard.Stop() # 停止
+                    if self.start_up(): # 启动
+                        self.logger.log_info(f"连接机械臂成功")
+                        break
+                    else:
+                        self.logger.log_error(f"启动失败，尝试第{cnt_num+1}次连接")  
+                        cnt_num += 1
+                        time.sleep(1)
+                else:
+                    if self.feedbackData.robotMode == 9 and self.parseResultId(stopId)[0] == -3:
+                        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)
+                    self.disableRobot()
+                    self.clearError() # 清除报警
+                    time.sleep(1)
+
+            else:
+                self.logger.log_error(f"连接机械臂失败，尝试第{cnt_num+1}次连接")
+                cnt_num += 1
+                time.sleep(1)
+        atexit.register(self.exit_task)
+
+    def exit_task(self):
+        self.logger.log_yellow("退出任务")
+        self.disable_servo()
+        instruction = {
+            "is_massaging": False,
+            "massage_service_started": False
+        }
+        requests.post(
+            "http://127.0.0.1:5000/update_massage_status", data=instruction
+        )
+        time.sleep(1)
+        # self.move_joint(self.off_pos)
+
+
+
+
+
+
 
 
     def parseResultId(self,valueRecv):
@@ -73,7 +156,7 @@ class dobot_nova5:
         return [int(num) for num in re.findall(r'-?\d+',valueRecv)] or [2]
     
     def GetFeedback(self):
-        while True:
+        while self.stop_feedback.is_set() == False:
             feedInfo = self.feedFour.feedBackData()
             with self.__globalLockValue:
                 if feedInfo is not None:
@@ -96,22 +179,24 @@ class dobot_nova5:
                         self.feedbackData.ToolValue = feedInfo['ToolValue[6]'][0]
                         ''' 
 
-    def start(self):
+    def start_up(self):
-        self.dashboard = DobotApiDashboard(self.ip,self.dashboardPort)
+        # self.dashboard = DobotApiDashboard(self.ip,self.dashboardPort)
-        self.feedFour = DobotApiFeedBack(self.ip,self.feedFourPort)
+        # self.feedFour = DobotApiFeedBack(self.ip,self.feedFourPort)
-        self.dashboard.EmergencyStop(mode=0) # 松开急停
+        # self.dashboard.EmergencyStop(mode=0) # 松开急停
-        self.clearError() # 清除报警
+        # self.clearError() # 清除报警
+        # self.dashboard.RequestControl()
         if self.parseResultId(self.dashboard.EnableRobot())[0] != 0:
             print("使能失败:检查29999端口是否被占用")
-            return
+            return False 
         print("机械臂使能成功")
+        return True
 
-        # 状态反馈线程
+        # # 状态反馈线程
-        feedback_thread = threading.Thread(
+        # feedback_thread = threading.Thread(
-            target=self.GetFeedback # 机器状态反馈线程
+        #     target=self.GetFeedback # 机器状态反馈线程
-        )
+        # )
-        feedback_thread.daemon = True
+        # feedback_thread.daemon = True
-        feedback_thread.start()
+        # feedback_thread.start()
     
     def disable_servo(self):
         pass
@@ -729,7 +814,7 @@ class dobot_nova5:
         self.last_pose_command = np.zeros(6)
         self.last_input_command = None
         self.tcp_offset = None
-        self.init_J = [0,30,-120,0,90,0]
+        self.init_J = [-45.0009079,55.5785141,-120.68821716,5.11103201,90.00195312,-90.00085449]
         self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
         self.filter_matrix = np.zeros((1,6)) # 角度伺服用
         sleep(1)
@@ -849,6 +934,11 @@ class dobot_nova5:
             return 0, result
         
         return -1, None
+    
+    def chooseRightFrame(self):
+        self.chooseBaseFrame(i=1)
+        # self.chooseEndEffector(i=0)
+        return
 
 
 if __name__ == "__main__":