微调了现有其他接口，将伺服改为只发，不需要接受返回数据
This commit is contained in:
ziwei.he
parent
54a6733861
commit
333aa7a56a
@ -4,6 +4,31 @@ from time import sleep,time
|
||||
import re
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
'''
|
||||
|
||||
MODE DESCRIPTION CN
|
||||
|
||||
1 INIT 初始化
|
||||
2 BREAK_OPEN 抱闸松开
|
||||
3 POWER_OFF 本体下电
|
||||
4 DISABLED 未使能(抱闸未松)
|
||||
5 ENABLE 使能(空闲)
|
||||
6 BACKDRIVE 拖拽
|
||||
7 RUNNING 运行状态(脚本/TCP)
|
||||
8 SINGLE_MOVE 单次运动(点动)
|
||||
9 ERROR 错误状态
|
||||
10 PAUSE 暂停状态
|
||||
11 COLLISION 碰撞状态
|
||||
|
||||
MODE RANK
|
||||
|
||||
报错 I
|
||||
下电 II
|
||||
碰撞 III
|
||||
开抱闸 IV
|
||||
|
||||
'''
|
||||
|
||||
class dobot_nova5:
|
||||
def __init__(self,ip):
|
||||
self.ip = ip
|
||||
@ -78,7 +103,7 @@ class dobot_nova5:
|
||||
print(self.parseResultId(recvmovemess))
|
||||
currentCommandID = self.parseResultId(recvmovemess)[1]
|
||||
print("指令 ID:", currentCommandID)
|
||||
# sleep(0.02)
|
||||
sleep(0.02)
|
||||
while True: # 完成判断循环
|
||||
########## 调试注释 #############
|
||||
# print("robot mode",self.feedbackData.robotMode)
|
||||
@ -86,6 +111,16 @@ class dobot_nova5:
|
||||
# print("指令 ID:", currentCommandID)
|
||||
########## 调试注释 #############
|
||||
currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
|
||||
if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11:
|
||||
self.dashboard.Stop()
|
||||
self.dashboard.EmergencyStop()
|
||||
print("当前工作停止，机械臂处于急停状态")
|
||||
if self.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.dashboard.Continue()
|
||||
code = int(res[0])
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
|
||||
print("该次运动完成")
|
||||
break
|
||||
@ -103,7 +138,6 @@ class dobot_nova5:
|
||||
currentCommandID = self.parseResultId(recvmovemess)[1]
|
||||
print("指令 ID:", currentCommandID)
|
||||
sleep(0.02)
|
||||
start_time = time() # 记录开始时间
|
||||
while True: # 完成判断循环
|
||||
########## 调试注释 #############
|
||||
# print("robot mode",self.feedbackData.robotMode)
|
||||
@ -116,7 +150,6 @@ class dobot_nova5:
|
||||
self.dashboard.Stop()
|
||||
self.dashboard.EmergencyStop()
|
||||
print("当前工作停止，机械臂处于急停状态")
|
||||
|
||||
if self.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.dashboard.Continue()
|
||||
@ -141,6 +174,16 @@ class dobot_nova5:
|
||||
print("指令 ID:", currentCommandID)
|
||||
while True:
|
||||
currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
|
||||
if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11:
|
||||
self.dashboard.Stop()
|
||||
self.dashboard.EmergencyStop()
|
||||
print("当前工作停止，机械臂处于急停状态")
|
||||
if self.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.dashboard.Continue()
|
||||
code = int(res[0])
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
|
||||
print("该次运动完成")
|
||||
break
|
||||
@ -159,14 +202,24 @@ class dobot_nova5:
|
||||
print("指令 ID:", currentCommandID)
|
||||
while True:
|
||||
currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
|
||||
if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11:
|
||||
self.dashboard.Stop()
|
||||
self.dashboard.EmergencyStop()
|
||||
print("当前工作停止，机械臂处于急停状态")
|
||||
if self.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.dashboard.Continue()
|
||||
code = int(res[0])
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
|
||||
print("该次运动完成")
|
||||
break
|
||||
sleep(0.1)
|
||||
|
||||
def ServoJoint(self,joints_list):
|
||||
tcp_command = f"servoj({joints_list[0]},{joints_list[1]},{joints_list[2]},{joints_list[3]},{joints_list[4]},{joints_list[5]},t=0.008,aheadtime=20,gain=200)"
|
||||
self.dashboard.sendRecvMsg(tcp_command)
|
||||
def ServoJoint(self,joints_list,t=-1,aheadtime=-1,gain=-1):
|
||||
tcp_command = f"servoj({joints_list[0]},{joints_list[1]},{joints_list[2]},{joints_list[3]},{joints_list[4]},{joints_list[5]},t={t},aheadtime={aheadtime},gain={gain})"
|
||||
self.dashboard.send_data(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
|
||||
print("ServoJ:",tcp_command)
|
||||
if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11:
|
||||
self.dashboard.Stop()
|
||||
@ -179,19 +232,21 @@ class dobot_nova5:
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
|
||||
|
||||
def ServoPose(self,poses_list,t=-1,aheadtime=-1,gain=-1):
|
||||
# 位姿伺服
|
||||
recvmovemess = self.dashboard.ServoP(*poses_list,t,aheadtime,gain)
|
||||
print("ServoP:",recvmovemess)
|
||||
print(self.parseResultId(recvmovemess))
|
||||
currentCommandID = self.parseResultId(recvmovemess)[1]
|
||||
print("指令 ID", currentCommandID)
|
||||
while True:
|
||||
currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
|
||||
if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
|
||||
print("完成一次伺服动作")
|
||||
break
|
||||
poses_list = list(map(float, poses_list))
|
||||
tcp_command = f"ServoP({poses_list[0]},{poses_list[1]},{poses_list[2]},{poses_list[3]},{poses_list[4]},{poses_list[5]}, t={t}, aheadtime={aheadtime}, gain={gain})"
|
||||
self.dashboard.send_data(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
|
||||
print("ServoP:",tcp_command)
|
||||
if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11:
|
||||
self.dashboard.Stop()
|
||||
self.dashboard.EmergencyStop()
|
||||
print("当前工作停止，机械臂处于急停状态")
|
||||
if self.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.dashboard.Continue()
|
||||
code = int(res[0])
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
|
||||
def setEndEffector(self,i,tool_i):
|
||||
'''
|
||||
@ -377,8 +432,7 @@ class dobot_nova5:
|
||||
del self.dashboard
|
||||
del self.feedFour
|
||||
|
||||
#########
|
||||
|
||||
# 适配中间层接口
|
||||
def get_arm_position(self):
|
||||
pose = self.getPose()
|
||||
x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree
|
||||
@ -386,7 +440,6 @@ class dobot_nova5:
|
||||
quat_rot = R.from_euler('xyz', [rx, ry, rz], degrees=True).as_quat()
|
||||
return pos,quat_rot
|
||||
|
||||
#########
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
@ -399,7 +452,7 @@ if __name__ == "__main__":
|
||||
|
||||
for i in range(400):
|
||||
posJ[2] += 0.04
|
||||
dobot.ServoJoint(posJ)
|
||||
dobot.ServoJoint(posJ,t=0.008,aheadtime=20,gain=200)
|
||||
sleep(0.007)
|
||||
|
||||
dobot.disableRobot()
|
||||
Loading…
x
Reference in New Issue
Block a user