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

修改init

Browse Source
This commit is contained in:
swayneleong 2025-05-29 22:50:13 +08:00
parent 7a59ee5693
commit 15eed49134
1 changed files with 65 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -137,6 +137,54 @@ class dobot_nova5:
time.sleep(1) time.sleep(1)
atexit.register(self.exit_task) atexit.register(self.exit_task)
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]
# 为状态管理而封装的初始化函数
def init(self):
self.is_exit = False
self.traj_list = None
self.last_pose_command = np.zeros(6)
self.last_input_command = None
self.tcp_offset = None
self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
self.filter_matrix = np.zeros((1,6)) # 角度伺服用
sleep(1)
self.is_standby = False
code = self.move_joint(self.init_pos)
if code == 0:
if self.parseResultId(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:
print("retry_count",retry_count)
break
else:
time.sleep(0.1)
if retry_count == max_retries:
self.logger.log_error("机械臂初始化失败")
requests.post(
"http://127.0.0.1:5000/on_message", data={"message": "机械臂初始化失败"}
)
self.is_exit = True
sys.exit(0)
else:
self.logger.log_error("机械臂初始化失败")
requests.post(
"http://127.0.0.1:5000/on_message", data={"message": "机械臂初始化失败"}
)
self.is_exit = True
sys.exit(0)
self.last_print_time=0
self.last_reocrd_time =0
def exit_task(self): def exit_task(self):
self.logger.log_yellow("退出任务") self.logger.log_yellow("退出任务")
# self.disable() # self.disable()
@ -587,23 +635,23 @@ class dobot_nova5:
## 适配中间层的再封装接口 ## 适配中间层的再封装接口
# 为状态管理而封装的初始化函数 # # 为状态管理而封装的初始化函数
def init(self): # def init(self):
self.is_exit = False # self.is_exit = False
self.traj_list = None # self.traj_list = None
self.last_pose_command = np.zeros(6) # self.last_pose_command = np.zeros(6)
self.last_input_command = None # self.last_input_command = None
self.tcp_offset = None # self.tcp_offset = None
self.init_J = [-45.0009079,55.5785141,-120.68821716,5.11103201,90.00195312,-90.00085449] # self.init_J = [-45.0009079,55.5785141,-120.68821716,5.11103201,90.00195312,-90.00085449]
self.filter_matirx = np.zeros((1,7)) # 位姿伺服用 # self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
self.filter_matrix = np.zeros((1,6)) # 角度伺服用 # self.filter_matrix = np.zeros((1,6)) # 角度伺服用
sleep(1) # sleep(1)
self.is_standby = False # self.is_standby = False
code = self.RunPoint_P_inJoint(self.init_J) # code = self.RunPoint_P_inJoint(self.init_J)
if code == 0: # if code == 0:
print("机械臂初始化成功且到达初始位置") # print("机械臂初始化成功且到达初始位置")
else: # else:
print("机械臂初始化失败") # print("机械臂初始化失败")
def get_pose_at_flange(self): def get_pose_at_flange(self):
''' '''