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

暂存

Browse Source
This commit is contained in:
Ziwei 2025-05-29 21:40:02 +08:00
parent f219a6d788
commit 28a2138dd7
4 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Massage/MassageControl/MassageRobot_nova5.py
View File

@ -570,6 +570,7 @@ class MassageRobot:
time.sleep(sleep_duration) time.sleep(sleep_duration)
print(f"real sleep:{time.time()-b2}") print(f"real sleep:{time.time()-b2}")
self.arm.dashboard.socket_dobot.sendall(tcp_command) self.arm.dashboard.socket_dobot.sendall(tcp_command)
print("currentCommandID",self.arm.feedbackData.robotCurrentCommandID)
if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停 if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停
print("机械臂为暂停状态") print("机械臂为暂停状态")
res = self.arm.dashboard.Continue() res = self.arm.dashboard.Continue()
@ -589,7 +590,7 @@ class MassageRobot:
# 线程 # 线程
self.exit_event.clear() self.exit_event.clear()
self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop) self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop)
self.arm_control_thread = threading.Thread(target=self.arm_command_loop_traj) self.arm_control_thread = threading.Thread(target=self.arm_command_loop)
# 线程启动 # 线程启动
self.arm_measure_thread.start() ## 测量线程 self.arm_measure_thread.start() ## 测量线程
position,quat_rot = self.arm.get_arm_position() position,quat_rot = self.arm.get_arm_position()
@ -1608,7 +1609,7 @@ if __name__ == "__main__":
# robot.dt = ts[1] - ts[0] # robot.dt = ts[1] - ts[0]
ready_pose = [194.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042] ready_pose = [204.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042]
robot.current_head = 'finger_head' robot.current_head = 'finger_head'
robot.force_sensor.disable_active_transmission() robot.force_sensor.disable_active_transmission()
@ -1629,7 +1630,7 @@ if __name__ == "__main__":
atexit.register(robot.force_sensor.disconnect) atexit.register(robot.force_sensor.disconnect)
robot.arm_state.desired_wrench = np.array([0,0,-0.5,0,0,0]) robot.arm_state.desired_wrench = np.array([0,0,-1,0,0,0])
try: try:
robot_thread = threading.Thread(target=robot.start) robot_thread = threading.Thread(target=robot.start)
robot_thread.start() robot_thread.start()

BIN
Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc
View File

Binary file not shown.

BIN
Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc
View File

Binary file not shown.

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

@ -118,7 +118,7 @@ class dobot_nova5:
self.dashboard.EmergencyStop(mode=0) # 松开急停 self.dashboard.EmergencyStop(mode=0) # 松开急停
self.clearError() # 清除报警 self.clearError() # 清除报警
# 关闭碰撞检测 # 关闭碰撞检测
self.dashboard.SetCollisionLevel(level=1) self.dashboard.SetCollisionLevel(level=0)
if self.parseResultId(self.dashboard.EnableRobot())[0] != 0: if self.parseResultId(self.dashboard.EnableRobot())[0] != 0:
print("使能失败:检查29999端口是否被占用") print("使能失败:检查29999端口是否被占用")
return return
@ -737,11 +737,13 @@ class dobot_nova5:
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.init_P = [654.3467,-134.9880,305.3604,-180.0000,-30.0000,0.0042]
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)
code = self.RunPoint_P_inPose(self.init_P)
if code == 0: if code == 0:
print("机械臂初始化成功且到达初始位置") print("机械臂初始化成功且到达初始位置")
else: else:
@ -881,7 +883,7 @@ if __name__ == "__main__":
# print(dobot.get_arm_position()) # print(dobot.get_arm_position())
# # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90]) dobot.RunPoint_L_inPose([179.087702,-136.993776,425.209977,179.418397,-28.9718402,0.06])
# cur_pose = dobot.getPose(user=1,tool=8) # cur_pose = dobot.getPose(user=1,tool=8)
# cur_pose[0] += 30 # cur_pose[0] += 30
# target_pose = cur_pose # target_pose = cur_pose
@ -890,5 +892,5 @@ if __name__ == "__main__":
# dobot.ServoPose(dobot.getPose(user=1,tool=8)) # dobot.ServoPose(dobot.getPose(user=1,tool=8))
# print("new ready joint",dobot.feedbackData.QActual) # print("new ready joint",dobot.feedbackData.QActual)
# # dobot.start_drag() # # dobot.start_drag()
dobot.stop_drag() # dobot.stop_drag()
dobot.disableRobot() dobot.disableRobot()