转换 KUKA 机器人的位姿

黄杰, 2017-05-13
root[a]linuxsand.info

目标是将基于用户坐标系 A 的点，转换为基于用户坐标系 B，位姿相对于大地不变。

也许网络上已经有类似代码，但我没去搜索，一是因为好久没写 KRL，想找找感觉，二是觉得：只要理解用户坐标系，掌握坐标变换的运算符，很容易就能做出来。

convert_pos.dat:

DEFDAT convert_pos PUBLIC
DECL GLOBAL E6POS converted_poses[100]
ENDDAT

convert_pos.src:

DEF convert_pos( )
; test
DECL E6POS test_poses[1]
test_poses[1] = {X 110, Y 220, Z 0, A 90, B 0, C 0, S 0, T 0}

; base 4, all zero
; base 5, x = 20, others are zero
; convert point from base 4 to base 5
iconvert_pos(test_poses[], 4, 5)

; check converted_poses[1].x = 90
END

; --- below is converting process  ---

GLOBAL DEF iconvert_pos(from_poses:OUT, from_base_index:IN, to_base_index:IN)
; Huang Jie
; http://blog.linuxsand.info/kuka-convert-pos-from-one-base-to-another.html
DECL E6POS from_poses[]
INT from_base_index, to_base_index, i
DECL FRAME inter_frame

inter_frame = INV_POS(BASE_DATA[to_base_index]) : BASE_DATA[from_base_index]

; I guess 999 is big enough :)
for i = 1 to 999 step 1
  ON_ERROR_PROCEED
  converted_poses[i] = inter_frame : from_poses[i]
  if $ERR.NUMBER == 1444 then
    ; array index overflow
    ; catch this error, exit the loop
    EXIT
  endif
endfor

msgnotify("Pos converted end", "iconvert_pos",,,i-1)
END

我还没有实际测试，但根据推算和简单的测试用例，应该是对的；免费使用，欢迎反馈。

结合代码看，假设基于用户坐标系（假设，数值上等于 BASE_DATA[4]）示教了 20 个点（存放在 test_poses[] 中），现在实际的用户坐标系变化了（数值上，变为 BASE_DATA[5]），那么只要运行 iconvert_pos(test_poses, 4, 5)，dat 文件中的 converted_poses[] 就是转换过后的位姿数值了。

在最近的项目中，会用到这个办法。我的视觉系统在物体某个固定点测算出一个直角坐标系，机器人在该坐标系上示教若干位姿。但是，在调试阶段我可能会变更该坐标系的原点和方向，那么机器人直接运行原有运动语句无法走到预想位姿。—— 重新示教 20 个位姿？费时费力。

P.S. 其实，某些日系的工业机器人系统自带这些实用小功能……只能讲，大家风格不同吧。

使用微信打赏 5 元：