From f4450a32e33354153d058aa49d3026cea27d5dca Mon Sep 17 00:00:00 2001 From: swayneleong <15723182159@163.com> Date: Wed, 28 May 2025 23:10:32 +0800 Subject: [PATCH] =?UTF-8?q?=E5=88=9D=E5=A7=8B=E5=8C=96=E5=88=9D=E6=AD=A5?= =?UTF-8?q?=E5=BC=82=E5=B8=B8=E5=A4=84=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Massage/MassageControl/MassageRobot_nova5.py | 64 +++++---- .../MassageControl/hardware/dobot_nova5.py | 124 +++++++++++++++--- 2 files changed, 147 insertions(+), 41 deletions(-) diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py index 9857a44..47b61e7 100644 --- a/Massage/MassageControl/MassageRobot_nova5.py +++ b/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 = [ - {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0], - "acceleration": [0, 0, 0], - "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], - "acceleration": [0, 0, 0], - "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()}, - ] ## 单位 m deg - myInterpolate = TrajectoryInterpolator(waypoints) - ts = np.linspace(0,5,100) + # waypoints = [ + # {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0], + # "acceleration": [0, 0, 0], + # "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], + # "acceleration": [0, 0, 0], + # "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()}, + # ] ## 单位 m deg + # myInterpolate = TrajectoryInterpolator(waypoints) + # 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 = [], [], [] - for t in ts: - xr, vr, ar = myInterpolate.interpolate(t) - xr_list.append(xr) - vr_list.append(vr) - ar_list.append(ar) - xr_array = np.array(xr_list) - vr_array = np.array(vr_list) - ar_array = np.array(ar_list) + # xr_list, vr_list, ar_list = [], [], [] + # for t in ts: + # xr, vr, ar = myInterpolate.interpolate(t) + # xr_list.append(xr) + # vr_list.append(vr) + # ar_list.append(ar) + # xr_array = np.array(xr_list) + # vr_array = np.array(vr_list) + # ar_array = np.array(ar_list) - robot.xr = xr_array - robot.vr = vr_array - robot.ar = ar_array - robot.ts = ts - robot.dt = ts[1] - ts[0] + # robot.xr = xr_array + # robot.vr = vr_array + # robot.ar = ar_array + # robot.ts = ts + # robot.dt = ts[1] - ts[0] ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0] diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index 447b9a6..6cf45ed 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/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.feedFourPort = 30004 ## 第四实时反馈(8ms)反馈数据端口号 + self.dashboardPort = dashboardPort ## 一发一收-设置-控制及运动端口号 + 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": "机械臂处于锁定状态
请解除锁定后再使用"} + ) + 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): - self.dashboard = DobotApiDashboard(self.ip,self.dashboardPort) - self.feedFour = DobotApiFeedBack(self.ip,self.feedFourPort) - self.dashboard.EmergencyStop(mode=0) # 松开急停 - self.clearError() # 清除报警 + def start_up(self): + # self.dashboard = DobotApiDashboard(self.ip,self.dashboardPort) + # self.feedFour = DobotApiFeedBack(self.ip,self.feedFourPort) + # self.dashboard.EmergencyStop(mode=0) # 松开急停 + # 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( - target=self.GetFeedback # 机器状态反馈线程 - ) - feedback_thread.daemon = True - feedback_thread.start() + # # 状态反馈线程 + # feedback_thread = threading.Thread( + # target=self.GetFeedback # 机器状态反馈线程 + # ) + # feedback_thread.daemon = True + # 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__":