增加了关节角度和米为单位的一些接口
This commit is contained in:
ziwei.he
parent
b1322495b0
commit
ce34b3a273
@ -144,7 +144,44 @@ class dobot_nova5:
|
||||
break
|
||||
sleep(0.1)
|
||||
return 0
|
||||
|
||||
|
||||
def RunPoint_P_inPose_M_RAD(self,poses_list):
|
||||
'''
|
||||
RUNPOINT_P 以关节运动方式运动到目标点
|
||||
MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ] unit: M/RAD
|
||||
'''
|
||||
# 单位 m/rad -> mm/deg
|
||||
poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(poses_list)
|
||||
# 走点
|
||||
recvmovemess = self.dashboard.MovJ(*poses_list_mm_deg,0)
|
||||
print("MovJ:",recvmovemess)
|
||||
print(self.parseResultId(recvmovemess))
|
||||
currentCommandID = self.parseResultId(recvmovemess)[1]
|
||||
print("指令 ID:", currentCommandID)
|
||||
sleep(0.02)
|
||||
while True: # 完成判断循环
|
||||
########## 调试注释 #############
|
||||
# print("robot mode",self.feedbackData.robotMode)
|
||||
# print("feedback current ID", self.feedbackData.robotCurrentCommandID)
|
||||
# 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(mode=1)
|
||||
print("当前工作停止，机械臂处于急停状态")
|
||||
if self.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.dashboard.Continue()
|
||||
code = self.parseResultId(res)[0]
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
|
||||
print("该次运动完成")
|
||||
break
|
||||
sleep(0.1)
|
||||
return 0
|
||||
|
||||
def RunPoint_P_inJoint(self,joints_list):
|
||||
'''
|
||||
RUNPOINT_P 以关节运动方式运动到目标点
|
||||
@ -180,7 +217,45 @@ class dobot_nova5:
|
||||
break
|
||||
sleep(0.1) # 避免 CPU 占用过高
|
||||
return 0
|
||||
|
||||
|
||||
def RunPoint_P_inJoint_RAD(self,joints_list):
|
||||
'''
|
||||
RUNPOINT_P 以关节运动方式运动到目标点
|
||||
MovJ flag = 1 通过关节角度走点，输入为[J1 J2 J3 J4 J5 J6] unit: rad
|
||||
'''
|
||||
# 单位 rad -> deg
|
||||
joints_list_deg = self.__transform_rad_2_deg(joints_list)
|
||||
# 走点
|
||||
recvmovemess = self.dashboard.MovJ(*joints_list_deg,1)
|
||||
print("MovJ:",recvmovemess)
|
||||
print(self.parseResultId(recvmovemess))
|
||||
currentCommandID = self.parseResultId(recvmovemess)[1]
|
||||
print("指令 ID:", currentCommandID)
|
||||
sleep(0.02)
|
||||
while True: # 完成判断循环
|
||||
########## 调试注释 #############
|
||||
# print("robot mode",self.feedbackData.robotMode)
|
||||
# print("feedback current ID", self.feedbackData.robotCurrentCommandID)
|
||||
# print("指令 ID:", currentCommandID)
|
||||
# print("30004",self.dashboard.GetCurrentCommandID())
|
||||
########## 调试注释 #############
|
||||
currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
|
||||
if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11:
|
||||
self.dashboard.Stop()
|
||||
self.dashboard.EmergencyStop(mode=1)
|
||||
print("当前工作停止，机械臂处于急停状态")
|
||||
if self.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.dashboard.Continue()
|
||||
code = self.parseResultId(res)[0]
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
|
||||
print("该次运动完成")
|
||||
break
|
||||
sleep(0.1) # 避免 CPU 占用过高
|
||||
return 0
|
||||
|
||||
def RunPoint_L_inPose(self,poses_list):
|
||||
'''
|
||||
RUNPOINT_L 以直线运动方式运动到目标点
|
||||
@ -210,6 +285,37 @@ class dobot_nova5:
|
||||
sleep(0.1)
|
||||
return 0
|
||||
|
||||
def RunPoint_L_inPose_M_RAD(self,poses_list):
|
||||
'''
|
||||
RUNPOINT_L 以直线运动方式运动到目标点
|
||||
MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ] unit: m/rad
|
||||
'''
|
||||
# 单位 m/rad -> mm/deg
|
||||
poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(poses_list)
|
||||
# 走点
|
||||
recvmovemess = self.dashboard.MovL(*poses_list_mm_deg,0)
|
||||
print("MovL:",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 == 9 or self.feedbackData.robotMode == 11:
|
||||
self.dashboard.Stop()
|
||||
self.dashboard.EmergencyStop(mode=1)
|
||||
print("当前工作停止，机械臂处于急停状态")
|
||||
if self.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.dashboard.Continue()
|
||||
code = self.parseResultId(res)[0]
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
|
||||
print("该次运动完成")
|
||||
break
|
||||
sleep(0.1)
|
||||
return 0
|
||||
|
||||
def RunPoint_L_inJoint(self,joints_list):
|
||||
'''
|
||||
RUNPOINT_L 以直线运动方式运动到目标点
|
||||
@ -238,9 +344,68 @@ class dobot_nova5:
|
||||
break
|
||||
sleep(0.1)
|
||||
return 0
|
||||
|
||||
def RunPoint_L_inJoint_RAD(self,joints_list):
|
||||
'''
|
||||
RUNPOINT_L 以直线运动方式运动到目标点
|
||||
MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ] unit: rad
|
||||
'''
|
||||
# 单位 rad -> deg
|
||||
joints_list_deg = self.__transform_rad_2_deg(joints_list)
|
||||
# 走点
|
||||
recvmovemess = self.dashboard.MovL(*joints_list_deg,0)
|
||||
print("MovL:",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 == 9 or self.feedbackData.robotMode == 11:
|
||||
self.dashboard.Stop()
|
||||
self.dashboard.EmergencyStop(mode=1)
|
||||
print("当前工作停止，机械臂处于急停状态")
|
||||
if self.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.dashboard.Continue()
|
||||
code = self.parseResultId(res)[0]
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
|
||||
print("该次运动完成")
|
||||
break
|
||||
sleep(0.1)
|
||||
return 0
|
||||
|
||||
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})"
|
||||
def ServoJoint(self,joints_list):
|
||||
'''
|
||||
关节角度伺服
|
||||
输入为[J1 J2 J3 J4 J5 J6] unit: deg
|
||||
'''
|
||||
tcp_command = f"servoj({joints_list[0]},{joints_list[1]},{joints_list[2]},{joints_list[3]},{joints_list[4]},{joints_list[5]}, t=0.01, aheadtime=20, gain=200)"
|
||||
self.dashboard.send_data(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
|
||||
print("ServoJ:",tcp_command)
|
||||
if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11:
|
||||
self.dashboard.Stop()
|
||||
self.dashboard.EmergencyStop(mode=1)
|
||||
print("当前工作停止，机械臂处于急停状态")
|
||||
return -1
|
||||
if self.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.dashboard.Continue()
|
||||
code = self.parseResultId(res)[0]
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
return 0
|
||||
|
||||
def ServoJoint_RAD(self,joints_list):
|
||||
'''
|
||||
关节角度伺服
|
||||
输入为[J1 J2 J3 J4 J5 J6] unit: rad
|
||||
'''
|
||||
# 单位转换 rad -> deg
|
||||
joints_list_deg = self.__transform_rad_2_deg(joints_list)
|
||||
# 构造字符串
|
||||
tcp_command = f"servoj({joints_list_deg[0]},{joints_list_deg[1]},{joints_list_deg[2]},{joints_list_deg[3]},{joints_list_deg[4]},{joints_list_deg[5]}, t=0.01, aheadtime=20, gain=200)"
|
||||
self.dashboard.send_data(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
|
||||
print("ServoJ:",tcp_command)
|
||||
if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11:
|
||||
@ -257,6 +422,10 @@ class dobot_nova5:
|
||||
return 0
|
||||
|
||||
def ServoPose(self,poses_list):
|
||||
'''
|
||||
位姿伺服
|
||||
输入为[X Y Z RX RY RZ] unit: mm/deg
|
||||
'''
|
||||
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=0.01, aheadtime=20, gain=200)"
|
||||
res = self.dashboard.sendRecvMsg(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
|
||||
@ -271,6 +440,30 @@ class dobot_nova5:
|
||||
code = self.parseResultId(res)[0]
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
|
||||
def ServoPose_M_RAD(self,poses_list):
|
||||
'''
|
||||
位姿伺服
|
||||
输入为[X Y Z RX RY RZ] unit: M/RAD
|
||||
'''
|
||||
# 单位 m/rad -> mm/deg
|
||||
poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(poses_list)
|
||||
# 确保数据类型正确
|
||||
poses_list_mm_deg = list(map(float, poses_list_mm_deg))
|
||||
# 构造字符串
|
||||
tcp_command = f"ServoP({poses_list[0]},{poses_list[1]},{poses_list[2]},{poses_list[3]},{poses_list[4]},{poses_list[5]}, t=0.01, aheadtime=20, gain=200)"
|
||||
res = self.dashboard.sendRecvMsg(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
|
||||
print("ServoP msg:",res)
|
||||
if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11:
|
||||
self.dashboard.Stop()
|
||||
self.dashboard.EmergencyStop(mode=1)
|
||||
print("当前工作停止，机械臂处于急停状态")
|
||||
if self.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.dashboard.Continue()
|
||||
code = self.parseResultId(res)[0]
|
||||
if code == 0:
|
||||
print("机械臂继续已暂停的运动指令")
|
||||
|
||||
def setEndEffector(self,i,tool_i):
|
||||
'''
|
||||
@ -469,7 +662,42 @@ class dobot_nova5:
|
||||
def __del__(self):
|
||||
del self.dashboard
|
||||
del self.feedFour
|
||||
## 内部工具函数
|
||||
|
||||
def __transform_m_rad_2_mm_deg(self,input_list):
|
||||
'''
|
||||
接受列表 [X Y Z RX RY RZ]
|
||||
将单位 m/rad 转为 mm/deg
|
||||
'''
|
||||
if len(input_list) != 6:
|
||||
raise ValueError("Input list must contain exactly 6 elements [X, Y, Z, RX, RY, RZ]")
|
||||
|
||||
# 转换位置单位：m -> mm (乘以 1000)
|
||||
x_mm = input_list[0] * 1000.0
|
||||
y_mm = input_list[1] * 1000.0
|
||||
z_mm = input_list[2] * 1000.0
|
||||
|
||||
# 转换角度单位：rad -> deg (乘以 180/pi)
|
||||
rx_deg = math.degrees(input_list[3])
|
||||
ry_deg = math.degrees(input_list[4])
|
||||
rz_deg = math.degrees(input_list[5])
|
||||
|
||||
return [x_mm, y_mm, z_mm, rx_deg, ry_deg, rz_deg]
|
||||
|
||||
def __transform_rad_2_deg(self,input_list):
|
||||
'''
|
||||
接受列表 [J1 J2 J3 J4 J5 J6]
|
||||
将单位rad转为deg
|
||||
'''
|
||||
if len(input_list) != 6:
|
||||
raise ValueError("Input list must contain exactly 6 elements [J1, J2, J3, J4, J5, J6]")
|
||||
# 转换角度单位: rad -> deg
|
||||
J_deg = []
|
||||
for i in range(6):
|
||||
J_deg.append(math.degrees(input_list[i]))
|
||||
return J_deg
|
||||
|
||||
|
||||
## 为反馈线程封装
|
||||
|
||||
def Get_feedInfo_angle(self):
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
45
logs/test.py
45
logs/test.py
@ -1,5 +1,42 @@
|
||||
import random
|
||||
import numpy as np
|
||||
# import random
|
||||
# import numpy as np
|
||||
|
||||
random_array = np.random.rand(3) # 生成长度为 3 的数组
|
||||
print(random_array/1000)
|
||||
# random_array = np.random.rand(3) # 生成长度为 3 的数组
|
||||
# print(random_array/1000)
|
||||
|
||||
import math
|
||||
|
||||
def __transform_mm_deg_2_m_rad(input_list):
|
||||
'''
|
||||
接受列表 [X Y Z RX RY RZ]
|
||||
将单位 m/rad 转为 mm/deg
|
||||
'''
|
||||
if len(input_list) != 6:
|
||||
raise ValueError("Input list must contain exactly 6 elements [X, Y, Z, RX, RY, RZ]")
|
||||
|
||||
# 转换位置单位：m -> mm (乘以 1000)
|
||||
x_mm = input_list[0] * 1000.0
|
||||
y_mm = input_list[1] * 1000.0
|
||||
z_mm = input_list[2] * 1000.0
|
||||
|
||||
# 转换角度单位：rad -> deg (乘以 180/pi)
|
||||
rx_deg = math.degrees(input_list[3])
|
||||
ry_deg = math.degrees(input_list[4])
|
||||
rz_deg = math.degrees(input_list[5])
|
||||
|
||||
return [x_mm, y_mm, z_mm, rx_deg, ry_deg, rz_deg]
|
||||
|
||||
def __transform_rad_2_deg(input_list):
|
||||
'''
|
||||
接受列表 [J1 J2 J3 J4 J5 J6]
|
||||
将单位rad转为deg
|
||||
'''
|
||||
if len(input_list) != 6:
|
||||
raise ValueError("Input list must contain exactly 6 elements [J1, J2, J3, J4, J5, J6]")
|
||||
# 转换角度单位: rad -> deg
|
||||
J_deg = []
|
||||
for i in range(6):
|
||||
J_deg.append(math.degrees(input_list[i]))
|
||||
return J_deg
|
||||
|
||||
print(__transform_rad_2_deg([6.283185307179586, 3.141592653589793, 6.283185307179586,6.283185307179586, 3.141592653589793, 6.283185307179586]))
|
||||
Loading…
x
Reference in New Issue
Block a user