Робот Кука. Вычислить относительное положение вокруг 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_DIS

TOOL_DATA [1] = {X -22,5370,Y 145,857,Z 177,617,A 0,0,B 0,0,C 0,0}

почему-то я знаю, что геометрический оператор работает, если я правильно изменяю A, B, C в $TOOL. Направление к объекту захвата. Это означает, что для другой ориентации нужны другие A, B, C из $TOOL, и это не очень интуитивно понятно, чтобы получить это...

есть ли более простой способ сделать это или понять это?

Источник

2 ответа

Вы действительно близки! KUKA использует систему Euler ZYX для расчета ориентации TCP. Это означает, что три поворота происходят в определенной последовательности для достижения конечной ориентации. Порядок:

  1. Вращение вокруг мировой оси Z
  2. Вращение B вокруг новой оси Y
  3. Вращение C вокруг новой оси X

следовательно, Эйлер ZYX.

Чтобы выполнить вращение, подобное тому, как работает TOOL TCP jog, вам необходимо выполнить преобразование кадра из вашего текущего положения в целевое положение. Если вы хотите повернуться вокруг текущей оси B, для этого потребуется нечто большее, чем просто корректировка B POS_ACT, потому что вращение 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. Я не буду вдаваться в подробности о матрицах вращения, но они имеют основополагающее значение для таких математических вычислений вращения. Я знаю, что это огромный кусок, и, вероятно, есть лучшие способы, но я никогда не сожалел о том, что добавил этот код в глобальную математическую библиотеку и на всякий случай бросил его на своих роботов.

Источник

Такова ли функция arctan2 из KUE_WEG?

          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[,]). Было бы здорово, если бы вы могли завершить это. Большое спасибо вам

Источник
Другие вопросы по тегам