DevelopmentTeam/MassageRobot_Dobot
15
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

初始化初步异常处理

Browse Source
This commit is contained in:
swayneleong 2025-05-28 23:10:32 +08:00
parent 2265b1c893
commit f4450a32e3
2 changed files with 147 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
Massage/MassageControl/MassageRobot_nova5.py
View File

@ -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]

124
Massage/MassageControl/hardware/dobot_nova5.py
View File

@ -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": "机械臂处于锁定状态<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):
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__":