1

我想像在慢跑模式下一样围绕 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_DIS

TOOL_DATA[1]={X -22.5370,Y 145.857,Z 177.617,A 0.0,B 0.0,C 0.0}

不知何故，如果我正确更改 $TOOL 的 A、B、C，我知道几何运算符有效。指向抓图对象的方向。这意味着不同的方向确实需要 $TOOL 的其他 A、B、C 并且它不是很直观地得到它......

有没有更简单的方法来做到这一点或理解这一点？

4

2 回答 2

0

你真的很亲近！KUKA 使用 Euler ZYX 系统来计算 TCP 方向。这意味着以特定顺序发生三个旋转以实现最终方向。顺序是：

  1. 围绕世界 Z 轴旋转
  2. B 绕新 Y 轴旋转
  3. 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
END

mat_to_rot 和 rot_to_mat 用于在 3x3 旋转矩阵和 A、B、C 角度之间进行转换。我不会详细介绍旋转矩阵，但它们是进行这样的旋转数学的基础。我知道这是一大口，并且可能有更好的方法，但我从来没有后悔将这段代码添加到全球数学库并将其扔到我的机器人上以防万一。

于 2020-12-11T21:07:24.930 回答
0

KUE_WEG 的 arctan2-function 是这样的吗？

    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[,], reverse[,]) 的东西。如果你能完成这个就太好了。非常感谢你

于 2020-12-14T10:48:20.093 回答