库卡机器人。计算TCP周围的相对位置
我想像在慢跑模式中一样，把TCP的位置转过来。
第一次尝试是增加职位的价值：
$TOOL = TOOL_DATA[1]
$BASE = BASE_DATA[3]
e6pOffsetPos.B = e6pOffsetPos.B + 50
PTP e6pOffsetPos C_DIS然后我尝试了几何运算符":“
$TOOL = TOOL_DATA[1]
$BASE = BASE_DATA[3]
f = {X 0.0, Y 0.0, Z 0.0, A 0.0, B 50.0, C 0.0}
PTP e6pOffsetPos:f C_DISTOOL_DATA1={X-22.5370，Y 145.857，Z 177.617，A0.0，B0.0，C0.0}
不知怎的，如果我正确地改变了$TOOL的A，B，C，我知道几何算子是有效的。指向抓取对象。这意味着一个不同的方向确实需要其他的A，B，C的$TOOL，它不是很直观地得到它.
有没有更容易的方法来做这件事，或理解这一点？
回答 2
Stack Overflow用户
发布于 2020-12-11 13:07:24
你真的很接近！KUKA使用Euler ZYX系统计算TCP方向。这意味着三个旋转发生在一个特定的顺序，以实现最终的方向。命令是：
- A旋转世界Z轴
- B旋转新Y轴
- C旋转新X轴
因此，欧拉ZYX。
为了进行类似于TOOL TCP jog操作的旋转，您需要从当前位置到目标位置进行帧转换。如果你想围绕着当前的B轴旋转，那么要达到这个目的，不仅仅需要对POS_ACT进行B的调整，因为B旋转只是一个旋转序列的一部分。
所以我们的主程序应该有这样的代码：
$TOOL = TOOL_DATA[1]
$BASE = BASE_DATA[3]
current_pos = $POS_ACT
offset = {X 0.0, Y 0.0, Z 0.0, A 0.0, B 50.0, C 0.0}
new_pos = transform_frame(offset, current_pos)现在让我们编写支持该函数的代码。我做了一个math_library.src：
DEF math_library()
END
GLOBAL DEFFCT FRAME transform_frame (offset:IN, origin:IN)
DECL FRAME offset, result_frame, origin
DECL REAL rot_coeff[3,3], final[3,3], reverse[3,3]
rot_to_mat(rot_coeff[,], origin.A, origin.B, origin.C)
result_frame.X = rot_coeff[1,1]*offset.X+rot_coeff[1,2]*offset.Y+rot_coeff[1,3]*offset.Z+origin.X
result_frame.Y = rot_coeff[2,1]*offset.X+rot_coeff[2,2]*offset.Y+rot_coeff[2,3]*offset.Z+origin.Y
result_frame.Z = rot_coeff[3,1]*offset.X+rot_coeff[3,2]*offset.Y+rot_coeff[3,3]*offset.Z+origin.Z
rot_to_mat(reverse[,], offset.A, offset.B, offset.C)
mat_mult(final[,], rot_coeff[,], reverse[,])
mat_to_rot(final[,], result_frame.A, result_frame.B, result_frame.C)
return result_frame
ENDFCT
GLOBAL DEF rot_to_mat(t[,]:OUT,a :IN,b :IN,c :IN )
;Conversion of ROT angles A, B, C into a rotation matrix T
;T = Rot_z (A) * Rot_y (B) * Rot_x (C)
;not made by me. This was in KEUWEG2 function
REAL t[,], a, b, c
REAL cos_a, sin_a, cos_b, sin_b, cos_c, sin_c
cos_a=COS(a)
sin_a=SIN(a)
cos_b=COS(b)
sin_b=SIN(b)
cos_c=COS(c)
sin_c=SIN(c)
t[1,1] = cos_a*cos_b
t[1,2] = -sin_a*cos_c + cos_a*sin_b*sin_c
t[1,3] = sin_a*sin_c + cos_a*sin_b*cos_c
t[2,1] = sin_a*cos_b
t[2,2] = cos_a*cos_c + sin_a*sin_b*sin_c
t[2,3] = -cos_a*sin_c + sin_a*sin_b*cos_c
t[3,1] = -sin_b
t[3,2] = cos_b*sin_c
t[3,3] = cos_b*cos_c
END
GLOBAL DEF mat_to_rot (t[,]:OUT, a:OUT, b:OUT, c:OUT)
;Conversion of a rotation matrix T into the angles A, B, C
;T = Rot_z(A) * Rot_y(B) * Rot_x(C)
;not made by me. This was in KEUWEG2 function
REAL t[,], a, b, c
REAL sin_a, cos_a, sin_b, abs_cos_b, sin_c, cos_c
a = ARCTAN2(t[2,1], t[1,1])
sin_a = SIN(a)
cos_a = COS(a)
sin_b = -t[3,1]
abs_cos_b = cos_a*t[1,1] + sin_a*t[2,1]
b = ARCTAN2(sin_b, abs_cos_b)
sin_c = sin_a*t[1,3] - cos_a*t[2,3]
cos_c = -sin_a*t[1,2] + cos_a*t[2,2]
c = ARCTAN2(sin_c, cos_c)
END
GLOBAL DEF mat_mult (a[,]:OUT,b[,]:OUT,c[,]:OUT)
DECL REAL a[,], b[,], c[,]
DECL INT i, j
;multiply two 3x3 matrices
FOR i = 1 TO 3
FOR j = 1 TO 3
a[i, j] = b[i,1]*c[1,j] + b[i,2]*c[2,j] + b[i,3]*c[3,j]
ENDFOR
ENDFOR
ENDmat_to_rot和rot_to_mat用于3x3旋转矩阵和A、B、C角之间的转换。我不会详细讨论旋转矩阵，但是它们是做这样的旋转数学的基础。我知道这是一个巨大的嘴巴，也许还有更好的方法，但我从来没有后悔把这段代码添加到一个全局数学库，并把它扔到我的机器人上以防万一。
Stack Overflow用户
发布于 2020-12-14 02:48:20
KUE_WEG中的arctan2函数是这样的吗？
DEFFCT REAL ARCTAN2 (Y: IN, X: IN)
; Arcustangens mit 2 Argumenten und Check, ob x und y numerisch 0 sind
REAL X, Y
REAL ATAN_EPS
ATAN_EPS = 0.00011
IF ( (ABS(X) < ATAN_EPS) AND (ABS(Y) < ATAN_EPS) ) THEN
RETURN (0)
ELSE
RETURN ( ATAN2(Y, X) )
ENDIF
ENDFCT但是我找不到像mat_mult(final，，rot_coeff，反向，)这样的东西。如果你能完成这件事就太好了。感谢你
https://stackoverflow.com/questions/65075288
复制相似问题
腾讯云开发者
