Kuka机器人设计通用码垛程序

潘诺西亚的火山

已于 2025-04-14 15:04:48 修改

阅读量6.9k

点赞数 4

文章标签：机器人算法

于 2023-08-27 20:44:50 首次发布

本文链接：https://blog.csdn.net/helldoger/article/details/132527436

版权

假设需要一个码垛程序, 从输送线抓到托盘, 托盘每层4个, 需要码5层, 可以用以下程序架构设计:

在config中定义层数cengshu , 每层码垛的个数(码垛的次数)cishu , 每层的高度levelHeight , 码垛放置点的集合putPoint[,] ,预放点1集合prePut1[,], 预放点2集合prePut2[,]

vbnet
DECL INT cengshu = 0

DECL INT cishu = 0

DECL INT levelHeight = 200

DECL FRAME putPoint[10,10]

DECL FRAME prePut1[10,10]

DECL FRAME prePut2[10,10]

码垛放件程序

注意,

if false then

SPTP putbase Vel=100 % PDAT6 Tool[1] Base[0];

endif

是定义放件最下面层的基准点, 因为条件是if false, 所以机器人永远不会走这个点.

在putbase 前加上x, 编程xputBase, 就可以将机器人的点位putbase 赋值给用户定义的frame
putPoint[cengshu,cishu]

3, 走用户定义的点 putPoint[cengshu,cishu] 不能用机器人点位, 应该用命令

slin putPoint[cengshu,cishu] c_dis

&ACCESS RVO1
&REL 61
&PARAM EDITMASK = *
&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
&PARAM DISKPATH = KRC:\R1\Program\test
DEF put( )
INI
if false then

SPTP putbase Vel=100 % PDAT6 Tool[1] Base[0];
endif

PTP p20 Vel=100 % PDAT7 Tool[1] Base[0];

WAIT SEC 1

if (cengshu<6) then
if cishu == 1 then

putPoint[cengshu,cishu] = xputbase

putPoint[cengshu,cishu].z = putPoint[cengshu,cishu].z+(cengshu-1)*levelHeight

endif
if cishu == 2 then

putPoint[cengshu,cishu] = xputbase

putPoint[cengshu,cishu].x = putPoint[cengshu,cishu].x+100

putPoint[cengshu,cishu].a = putPoint[cengshu,cishu].a+90

putPoint[cengshu,cishu].z = putPoint[cengshu,cishu].z+(cengshu-1)*levelHeight
endif

if cishu == 3 then

putPoint[cengshu,cishu] = xputbase

putPoint[cengshu,cishu].x = putPoint[cengshu,cishu].x+100

putPoint[cengshu,cishu].y = putPoint[cengshu,cishu].y+100

putPoint[cengshu,cishu].z = putPoint[cengshu,cishu].z+(cengshu-1)*levelHeight
endif


if cishu == 4 then

putPoint[cengshu,cishu] = xputbase

putPoint[cengshu,cishu].y = putPoint[cengshu,cishu].y+100

putPoint[cengshu,cishu].z = putPoint[cengshu,cishu].z+(cengshu-1)*levelHeight
endif

endif

slin putPoint[cengshu,cishu] c_dis
END

主程序:

DEF test10( )
INI;

PTP P1 Vel=100 % PDAT1 Tool[1] Base[0]

for cengshu = 1 to 5
for cishu = 1 to 4
put()
endfor
endfor


PTP P2 Vel=100 % PDAT2 Tool[1] Base[0]

END

坐标系偏移法: 在config 里定义Frame:

FRAME p_position[16,2]

定义一个层数, 和每层垛数的计算:

&ACCESS RVO1
&REL 909
&PARAM EDITMASK = *
&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
DEF calculation1( )
;FOLD INI
  ;FOLD BASISTECH INI
    GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
    INTERRUPT ON 3 
    BAS (#INITMOV,0 )
  ;ENDFOLD (BASISTECH INI)
  ;FOLD USER INI
    ;Make your modifications here

  ;ENDFOLD (USER INI)
;ENDFOLD (INI)

;LEVEL 1
P_POSITION[1,1].X=0
P_POSITION[1,1].Y=0
P_POSITION[1,1].Z=0
P_POSITION[1,1].A=0
P_POSITION[1,1].B=0.0
P_POSITION[1,1].C=0.0

P_POSITION[1,2].X=0+L
P_POSITION[1,2].Y=0-11
P_POSITION[1,2].Z=0+2
P_POSITION[1,2].A=0
P_POSITION[1,2].B=2
P_POSITION[1,2].C=0.0
;LEVEL 2
P_POSITION[2,1].X=0-5
P_POSITION[2,1].Y=0-5
P_POSITION[2,1].Z=0+H-10
P_POSITION[2,1].A=0
P_POSITION[2,1].B=0.0
P_POSITION[2,1].C=0.0

P_POSITION[2,2].X=0+L
P_POSITION[2,2].Y=0-7
P_POSITION[2,2].Z=0+H-10
P_POSITION[2,2].A=0
P_POSITION[2,2].B=0
P_POSITION[2,2].C=0.0

;LEVEL 3
P_POSITION[3,1].X=0+L/2
P_POSITION[3,1].Y=0
P_POSITION[3,1].Z=H*2
P_POSITION[3,1].A=0
P_POSITION[3,1].B=0.0
P_POSITION[3,1].C=0

P_POSITION[3,2].X=0+W*2
P_POSITION[3,2].Y=0+30
P_POSITION[3,2].Z=H*2
P_POSITION[3,2].A=90
P_POSITION[3,2].B=0.0
P_POSITION[3,2].C=0.0
;LEVEL 4
P_POSITION[4,1].X=0+L/2
P_POSITION[4,1].Y=0
P_POSITION[4,1].Z=H*3-10
P_POSITION[4,1].A=0
P_POSITION[4,1].B=0.0
P_POSITION[4,1].C=0

P_POSITION[4,2].X=0+W*2
P_POSITION[4,2].Y=0+30
P_POSITION[4,2].Z=H*3-10
P_POSITION[4,2].A=90
P_POSITION[4,2].B=0
P_POSITION[4,2].C=0.0

;LEVEL 5

P_POSITION[5,1].X=0
P_POSITION[5,1].Y=0
P_POSITION[5,1].Z=H*4
P_POSITION[5,1].A=0
P_POSITION[5,1].B=0
P_POSITION[5,1].C=0


P_POSITION[5,2].X=0+L
P_POSITION[5,2].Y=0-11
P_POSITION[5,2].Z=H*4+2
P_POSITION[5,2].A=0
P_POSITION[5,2].B=2
P_POSITION[5,2].C=0.0


;LEVEL 6

P_POSITION[6,1].X=0-5
P_POSITION[6,1].Y=0-5
P_POSITION[6,1].Z=H*5-10
P_POSITION[6,1].A=0.0
P_POSITION[6,1].B=0.0
P_POSITION[6,1].C=0

P_POSITION[6,2].X=0+L
P_POSITION[6,2].Y=0-7
P_POSITION[6,2].Z=H*5-10
P_POSITION[6,2].A=0.0
P_POSITION[6,2].B=0
P_POSITION[6,2].C=0.0

;LEVEL 7
P_POSITION[7,1].X=0+L/2
P_POSITION[7,1].Y=0
P_POSITION[7,1].Z=H*6
P_POSITION[7,1].A=0.0
P_POSITION[7,1].B=0.0
P_POSITION[7,1].C=0.0

P_POSITION[7,2].X=0+W*2
P_POSITION[7,2].Y=0+30
P_POSITION[7,2].Z=H*6
P_POSITION[7,2].A=90
P_POSITION[7,2].B=0
P_POSITION[7,2].C=0.0

;LEVEL 8
P_POSITION[8,1].X=0+L/2
P_POSITION[8,1].Y=0
P_POSITION[8,1].Z=H*7-10
P_POSITION[8,1].A=0
P_POSITION[8,1].B=0.0
P_POSITION[8,1].C=0

P_POSITION[8,2].X=0+W*2
P_POSITION[8,2].Y=0+30
P_POSITION[8,2].Z=H*7-10
P_POSITION[8,2].A=90
P_POSITION[8,2].B=0
P_POSITION[8,2].C=0

;LEVEL 9

P_POSITION[9,1].X=0
P_POSITION[9,1].Y=0
P_POSITION[9,1].Z=H*8
P_POSITION[9,1].A=0
P_POSITION[9,1].B=0.0
P_POSITION[9,1].C=0.0


P_POSITION[9,2].X=0+L
P_POSITION[9,2].Y=0-11
P_POSITION[9,2].Z=H*8+2
P_POSITION[9,2].A=0
P_POSITION[9,2].B=2
P_POSITION[9,2].C=0.0

;LEVEL 10
P_POSITION[10,1].X=0-5
P_POSITION[10,1].Y=0-5
P_POSITION[10,1].Z=H*9-10
P_POSITION[10,1].A=0.0
P_POSITION[10,1].B=0.0
P_POSITION[10,1].C=0.0

P_POSITION[10,2].X=0+L
P_POSITION[10,2].Y=0-7
P_POSITION[10,2].Z=H*9-10
P_POSITION[10,2].A=0
P_POSITION[10,2].B=0
P_POSITION[10,2].C=0.0

;LEVEL 11
P_POSITION[11,1].X=0+L/2
P_POSITION[11,1].Y=0
P_POSITION[11,1].Z=H*10
P_POSITION[11,1].A=0.0
P_POSITION[11,1].B=0.0
P_POSITION[11,1].C=0.0

P_POSITION[11,2].X=0+W*2
P_POSITION[11,2].Y=0+30
P_POSITION[11,2].Z=H*10
P_POSITION[11,2].A=90.0
P_POSITION[11,2].B=0.0
P_POSITION[11,2].C=0.0

;LEVEL 12
P_POSITION[12,1].X=0+L/2
P_POSITION[12,1].Y=0
P_POSITION[12,1].Z=H*11-10
P_POSITION[12,1].A=0.0
P_POSITION[12,1].B=0.0
P_POSITION[12,1].C=0.0

P_POSITION[12,2].X=0+W*2
P_POSITION[12,2].Y=0+30
P_POSITION[12,2].Z=H*11-10
P_POSITION[12,2].A=90
p_POSITION[12,2].B=0
P_POSITION[12,2].C=0

;LEVEL 13

P_POSITION[13,1].X=0
P_POSITION[13,1].Y=0
P_POSITION[13,1].Z=H*12
P_POSITION[13,1].A=0
P_POSITION[13,1].B=0.0
P_POSITION[13,1].C=0.0

P_POSITION[13,2].X=0+L
P_POSITION[13,2].Y=0-11
P_POSITION[13,2].Z=H*12+2
P_POSITION[13,2].A=0
P_POSITION[13,2].B=2
P_POSITION[13,2].C=0.0

;LEVEL 14
P_POSITION[14,1].X=0-5
P_POSITION[14,1].Y=0-5
P_POSITION[14,1].Z=H*13-10
P_POSITION[14,1].A=0.0
P_POSITION[14,1].B=0.0
P_POSITION[14,1].C=0.0

P_POSITION[14,2].X=0+L
P_POSITION[14,2].Y=0-7
P_POSITION[14,2].Z=H*13-10
P_POSITION[14,2].A=0
P_POSITION[14,2].B=0
P_POSITION[14,2].C=0.0

;LEVEL 15
P_POSITION[15,1].X=0+L/2
P_POSITION[15,1].Y=0
P_POSITION[15,1].Z=H*14
P_POSITION[15,1].A=0.0
P_POSITION[15,1].B=0.0
P_POSITION[15,1].C=0.0

P_POSITION[15,2].X=0+W*2
P_POSITION[15,2].Y=0+30
P_POSITION[15,2].Z=H*14
P_POSITION[15,2].A=90.0
P_POSITION[15,2].B=0
P_POSITION[15,2].C=0.0

;LEVEL 16
P_POSITION[16,1].X=0+L/2
P_POSITION[16,1].Y=0
P_POSITION[16,1].Z=H*15-10
P_POSITION[16,1].A=0.0
P_POSITION[16,1].B=0.0
P_POSITION[16,1].C=0.0

P_POSITION[16,2].X=0+W*2
P_POSITION[16,2].Y=0+30
P_POSITION[16,2].Z=H*15-10
P_POSITION[16,2].A=90
P_POSITION[16,2].B=0.0
P_POSITION[16,2].C=0.0

if false then
;LEVEL 16
P_POSITION[16,1].X=0.0
P_POSITION[16,1].Y=0.0
P_POSITION[16,1].Z=H*15-3*H
P_POSITION[16,1].A=0.0
P_POSITION[16,1].B=0.0
P_POSITION[16,1].C=0.0

P_POSITION[16,2].X=w+SP
P_POSITION[16,2].Y=0.0
P_POSITION[16,2].Z=H*15-3*H
P_POSITION[16,2].A=0.0
P_POSITION[16,2].B=0.0
P_POSITION[16,2].C=0.0

;LEVEL 17
P_POSITION[17,1].X=0.0
P_POSITION[17,1].Y=0.0
P_POSITION[17,1].Z=H*16-3*H
P_POSITION[17,1].A=0.0
P_POSITION[17,1].B=0.0
P_POSITION[17,1].C=0.0

P_POSITION[17,2].X=w+SP
P_POSITION[17,2].Y=0.0
P_POSITION[17,2].Z=H*16-3*H
P_POSITION[17,2].A=0.0
P_POSITION[17,2].B=0.0
P_POSITION[17,2].C=0.0

;LEVEL 18
P_POSITION[18,1].X=0.0
P_POSITION[18,1].Y=0.0
P_POSITION[18,1].Z=H*17
P_POSITION[18,1].A=0.0
P_POSITION[18,1].B=0.0
P_POSITION[18,1].C=0.0

P_POSITION[18,2].X=w+SP
P_POSITION[18,2].Y=0.0
P_POSITION[18,2].Z=H*17
P_POSITION[18,2].A=0.0
P_POSITION[18,2].B=0.0
P_POSITION[18,2].C=0.0

;LEVEL 19
P_POSITION[19,1].X=0.0
P_POSITION[19,1].Y=0.0
P_POSITION[19,1].Z=H*18
P_POSITION[19,1].A=0.0
P_POSITION[19,1].B=0.0
P_POSITION[19,1].C=0.0

P_POSITION[19,2].X=w+SP
P_POSITION[19,2].Y=0.0
P_POSITION[19,2].Z=H*18
P_POSITION[19,2].A=0.0
P_POSITION[19,2].B=0.0
P_POSITION[19,2].C=0.0

;LEVEL 20
P_POSITION[20,1].X=0.0
P_POSITION[20,1].Y=0.0
P_POSITION[20,1].Z=H*19
P_POSITION[20,1].A=0.0
P_POSITION[20,1].B=0.0
P_POSITION[20,1].C=0.0

P_POSITION[20,2].X=w+SP
P_POSITION[20,2].Y=0.0
P_POSITION[20,2].Z=H*19
P_POSITION[20,2].A=0.0
P_POSITION[20,2].B=0.0
P_POSITION[20,2].C=0.0

;LEVEL 21
P_POSITION[21,1].X=0.0
P_POSITION[21,1].Y=0.0
P_POSITION[21,1].Z=H*20
P_POSITION[21,1].A=0.0
P_POSITION[21,1].B=0.0
P_POSITION[21,1].C=0.0

P_POSITION[21,2].X=w+SP
P_POSITION[21,2].Y=0.0
P_POSITION[21,2].Z=H*20
P_POSITION[21,2].A=0.0
P_POSITION[21,2].B=0.0
P_POSITION[21,2].C=0.0

;LEVEL 22
P_POSITION[22,1].X=0.0
P_POSITION[22,1].Y=0.0
P_POSITION[22,1].Z=H*21
P_POSITION[22,1].A=0.0
P_POSITION[22,1].B=0.0
P_POSITION[22,1].C=0.0

P_POSITION[22,2].X=w+SP
P_POSITION[22,2].Y=0.0
P_POSITION[22,2].Z=H*21
P_POSITION[22,2].A=0.0
P_POSITION[22,2].B=0.0
P_POSITION[22,2].C=0.0

;LEVEL 23
P_POSITION[23,1].X=0.0
P_POSITION[23,1].Y=0.0
P_POSITION[23,1].Z=H*22
P_POSITION[23,1].A=0.0
P_POSITION[23,1].B=0.0
P_POSITION[23,1].C=0.0

P_POSITION[23,2].X=w+SP
P_POSITION[23,2].Y=0.0
P_POSITION[23,2].Z=H*22
P_POSITION[23,2].A=0.0
P_POSITION[23,2].B=0.0
P_POSITION[23,2].C=0.0

;LEVEL 24
P_POSITION[24,1].X=0.0
P_POSITION[24,1].Y=0.0
P_POSITION[24,1].Z=H*23
P_POSITION[24,1].A=0.0
P_POSITION[24,1].B=0.0
P_POSITION[24,1].C=0.0

P_POSITION[24,2].X=w+SP
P_POSITION[24,2].Y=0.0
P_POSITION[24,2].Z=H*23
P_POSITION[24,2].A=0.0
P_POSITION[24,2].B=0.0
P_POSITION[24,2].C=0.0

;LEVEL 25
P_POSITION[25,1].X=0.0
P_POSITION[25,1].Y=0.0
P_POSITION[25,1].Z=H*24
P_POSITION[25,1].A=0.0
P_POSITION[25,1].B=0.0
P_POSITION[25,1].C=0.0

P_POSITION[25,2].X=w+SP
P_POSITION[25,2].Y=0.0
P_POSITION[25,2].Z=H*24
P_POSITION[25,2].A=0.0
P_POSITION[25,2].B=0.0
P_POSITION[25,2].C=0.0

;LEVEL 26
P_POSITION[26,1].X=0.0
P_POSITION[26,1].Y=0.0
P_POSITION[26,1].Z=H*25
P_POSITION[26,1].A=0.0
P_POSITION[26,1].B=0.0
P_POSITION[26,1].C=0.0

P_POSITION[26,2].X=w+SP
P_POSITION[26,2].Y=0.0
P_POSITION[26,2].Z=H*25
P_POSITION[26,2].A=0.0
P_POSITION[26,2].B=0.0
P_POSITION[26,2].C=0.0

;LEVEL 27
P_POSITION[27,1].X=0.0
P_POSITION[27,1].Y=0.0
P_POSITION[27,1].Z=H*26
P_POSITION[27,1].A=0.0
P_POSITION[27,1].B=0.0
P_POSITION[27,1].C=0.0

P_POSITION[27,2].X=w+SP
P_POSITION[27,2].Y=0.0
P_POSITION[27,2].Z=H*26
P_POSITION[27,2].A=0.0
P_POSITION[27,2].B=0.0
P_POSITION[27,2].C=0.0

;LEVEL 28
P_POSITION[28,1].X=0.0
P_POSITION[28,1].Y=0.0
P_POSITION[28,1].Z=H*27
P_POSITION[28,1].A=0.0
P_POSITION[28,1].B=0.0
P_POSITION[28,1].C=0.0

P_POSITION[28,2].X=w+SP
P_POSITION[28,2].Y=0.0
P_POSITION[28,2].Z=H*27
P_POSITION[28,2].A=0.0
P_POSITION[28,2].B=0.0
P_POSITION[28,2].C=0.0

;LEVEL 29
P_POSITION[29,1].X=0.0
P_POSITION[29,1].Y=0.0
P_POSITION[29,1].Z=H*28
P_POSITION[29,1].A=0.0
P_POSITION[29,1].B=0.0
P_POSITION[29,1].C=0.0

P_POSITION[29,2].X=w+SP
P_POSITION[29,2].Y=0.0
P_POSITION[29,2].Z=H*28
P_POSITION[29,2].A=0.0
P_POSITION[29,2].B=0.0
P_POSITION[29,2].C=0.0

;LEVEL 30
P_POSITION[30,1].X=0.0
P_POSITION[30,1].Y=0.0
P_POSITION[30,1].Z=H*29
P_POSITION[30,1].A=0.0
P_POSITION[30,1].B=0.0
P_POSITION[30,1].C=0.0

P_POSITION[30,2].X=w+SP
P_POSITION[30,2].Y=0.0
P_POSITION[30,2].Z=H*29
P_POSITION[30,2].A=0.0
P_POSITION[30,2].B=0.0
P_POSITION[30,2].C=0.0

;LEVEL 31
P_POSITION[31,1].X=0.0
P_POSITION[31,1].Y=0.0
P_POSITION[31,1].Z=H*30
P_POSITION[31,1].A=0.0
P_POSITION[31,1].B=0.0
P_POSITION[31,1].C=0.0

P_POSITION[31,2].X=w+SP
P_POSITION[31,2].Y=0.0
P_POSITION[31,2].Z=H*30
P_POSITION[31,2].A=0.0
P_POSITION[31,2].B=0.0
P_POSITION[31,2].C=0.0

;LEVEL 32
P_POSITION[32,1].X=0.0
P_POSITION[32,1].Y=0.0
P_POSITION[32,1].Z=H*31
P_POSITION[32,1].A=0.0
P_POSITION[32,1].B=0.0
P_POSITION[32,1].C=0.0

P_POSITION[32,2].X=w+SP
P_POSITION[32,2].Y=0.0
P_POSITION[32,2].Z=H*31
P_POSITION[32,2].A=0.0
P_POSITION[32,2].B=0.0
P_POSITION[32,2].C=0.0

;LEVEL 33
P_POSITION[33,1].X=0.0
P_POSITION[33,1].Y=0.0
P_POSITION[33,1].Z=H*32
P_POSITION[33,1].A=0.0
P_POSITION[33,1].B=0.0
P_POSITION[33,1].C=0.0

P_POSITION[33,2].X=w+SP
P_POSITION[33,2].Y=0.0
P_POSITION[33,2].Z=H*32
P_POSITION[33,2].A=0.0
P_POSITION[33,2].B=0.0
P_POSITION[33,2].C=0.0

;LEVEL 34
P_POSITION[34,1].X=0.0
P_POSITION[34,1].Y=0.0
P_POSITION[34,1].Z=H*33
P_POSITION[34,1].A=0.0
P_POSITION[34,1].B=0.0
P_POSITION[34,1].C=0.0

P_POSITION[34,2].X=w+SP
P_POSITION[34,2].Y=0.0
P_POSITION[34,2].Z=H*33
P_POSITION[34,2].A=0.0
P_POSITION[34,2].B=0.0
P_POSITION[34,2].C=0.0

;LEVEL 35
P_POSITION[35,1].X=0.0
P_POSITION[35,1].Y=0.0
P_POSITION[35,1].Z=H*34
P_POSITION[35,1].A=0.0
P_POSITION[35,1].B=0.0
P_POSITION[35,1].C=0.0

P_POSITION[35,2].X=w+SP
P_POSITION[35,2].Y=0.0
P_POSITION[35,2].Z=H*34
P_POSITION[35,2].A=0.0
P_POSITION[35,2].B=0.0
P_POSITION[35,2].C=0.0

;LEVEL 36
P_POSITION[36,1].X=0.0
P_POSITION[36,1].Y=0.0
P_POSITION[36,1].Z=H*35
P_POSITION[36,1].A=0.0
P_POSITION[36,1].B=0.0
P_POSITION[36,1].C=0.0

P_POSITION[36,2].X=w+SP
P_POSITION[36,2].Y=0.0
P_POSITION[36,2].Z=H*35
P_POSITION[36,2].A=0.0
P_POSITION[36,2].B=0.0
P_POSITION[36,2].C=0.0

;LEVEL 37
P_POSITION[37,1].X=0.0
P_POSITION[37,1].Y=0.0
P_POSITION[37,1].Z=H*36
P_POSITION[37,1].A=0.0
P_POSITION[37,1].B=0.0
P_POSITION[37,1].C=0.0

P_POSITION[37,2].X=w+SP
P_POSITION[37,2].Y=0.0
P_POSITION[37,2].Z=H*36
P_POSITION[37,2].A=0.0
P_POSITION[37,2].B=0.0
P_POSITION[37,2].C=0.0

;LEVEL 38
P_POSITION[38,1].X=0.0
P_POSITION[38,1].Y=0.0
P_POSITION[38,1].Z=H*37
P_POSITION[38,1].A=0.0
P_POSITION[38,1].B=0.0
P_POSITION[38,1].C=0.0

P_POSITION[38,2].X=w+SP
P_POSITION[38,2].Y=0.0
P_POSITION[38,2].Z=H*37
P_POSITION[38,2].A=0.0
P_POSITION[38,2].B=0.0
P_POSITION[38,2].C=0.0

;LEVEL 39
P_POSITION[39,1].X=0.0
P_POSITION[39,1].Y=0.0
P_POSITION[39,1].Z=H*38
P_POSITION[39,1].A=0.0
P_POSITION[39,1].B=0.0
P_POSITION[39,1].C=0.0

P_POSITION[39,2].X=w+SP
P_POSITION[39,2].Y=0.0
P_POSITION[39,2].Z=H*38
P_POSITION[39,2].A=0.0
P_POSITION[39,2].B=0.0
P_POSITION[39,2].C=0.0

;LEVEL 40
P_POSITION[40,1].X=0.0
P_POSITION[40,1].Y=0.0
P_POSITION[40,1].Z=H*39
P_POSITION[40,1].A=0.0
P_POSITION[40,1].B=0.0
P_POSITION[40,1].C=0.0

P_POSITION[40,2].X=w+SP
P_POSITION[40,2].Y=0.0
P_POSITION[40,2].Z=H*39
P_POSITION[40,2].A=0.0
P_POSITION[40,2].B=0.0
P_POSITION[40,2].C=0.0
endif

END

在码垛程序中让base偏移:

;FOLD PTP ready Vel=100 % PDAT13 Tool[2]:grip Base[1]:duo1;%{PE}%R 8.3.44,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:ready, 3:, 5:100, 7:PDAT13
$BWDSTART=FALSE
PDAT_ACT=PPDAT13
FDAT_ACT=Fready
BAS(#PTP_PARAMS,100)
PTP Xready 
;ENDFOLD
;change base to base1
BASE_DATA[1]=BASE_DATA[11]
BASE_DATA[1]=BASE_DATA[1]:P_POSITION[lev1,Num1]

BASE_DATA[1] 是基座标系1的位置, 它里面的位置信息是当前base 和世界坐标系偏移/旋转的位姿.

它乘以旋转/平移矩阵得到新的坐标系, 得到新的码垛点. 具体公式用C# 写如下:

using NumSharp;
using System;

namespace MyMath
{
    public class MyCover
    {
        /// <summary>
        /// 传入参数说明
        /// </summary>
        /// <param name="px">图纸上该点的x坐标</param>
        /// <param name="py">图纸上该点的y坐标</param>
        /// <param name="pz">图纸上该点的z坐标</param>
        /// <param name="x">在X轴上位移了x长度</param>
        /// <param name="y">在Y轴上位移了y长度</param>
        /// <param name="z">在Z轴上位移了z长度</param>
        /// <param name="a">工件绕Z轴旋转的角度</param>
        /// <param name="b">工件绕Y轴旋转的角度</param>
        /// <param name="r">工件绕X轴旋转的角度</param>
        public MyCover(float px, float py, float pz, float x, float y, float z, float a, float b, float r)
        {
            this.x = x;
            this.y = y;
            this.z = z;
            this.a = a;
            this.b = b;
            this.r = r;
            this.rot = new float[4, 4]
            {
                {
                    this.Cos(this.a)*this.Cos(this.b),
                    this.Cos(this.a)*this.Sin(this.b)*this.Sin(this.r)-this.Sin(this.a)*this.Cos(this.r),
                    this.Cos(this.a)*this.Sin(this.b)*this.Cos(this.r)+this.Sin(this.a)*this.Sin(this.r),
                    this.x
                },
                {
                    this.Sin(this.a)*this.Cos(this.b),
                    this.Sin(this.a)*this.Sin(this.b)*this.Sin(this.r)+this.Cos(this.a)*this.Cos(this.r),
                    this.Sin(this.a)*this.Sin(this.b)*this.Cos(this.r)-this.Cos(this.a)*this.Sin(this.r),
                    this.y
                },
                {
                    -this.Sin(this.b),
                    this.Cos(this.b)*this.Sin(this.r),
                    this.Cos(this.b)*this.Cos(this.r),
                    this.z
                },
                {
                    0,
                    0,
                    0,
                    1
                },
            };
            this.line = new float[4, 1] { { px }, { py }, { pz }, { 1 } };
        }
        #region 字段和属性


        private float[,] rot;
        /// <summary>
        /// 位移+旋转矩阵
        /// </summary>
        public float[,] Rot
        {
            get { return rot; }
            set { rot = value; }
        }

        private float[,] line;

        /// <summary>
        /// 图纸上的坐标，四行 1列
        /// </summary>
        public float[,] Line
        {
            get { return line; }
            set { line = value; }
        }

        private float x;

        public float X
        {
            get { return x; }
            set { x = value; }
        }

        private float y;

        public float Y
        {
            get { return y; }
            set { y = value; }
        }

        private float z;

        public float Z
        {
            get { return z; }
            set { z = value; }
        }

        private float px;

        public float Px
        {
            get { return px; }
            set { px = value; }
        }

        private float py;

        public float Py
        {
            get { return py; }
            set { py = value; }
        }

        private float pz;

        public float Pz
        {
            get { return pz; }
            set { pz = value; }
        }

        private float a;

        public float A
        {
            get { return a; }
            set { a = value; }
        }

        private float b;

        public float B
        {
            get { return b; }
            set { b = value; }
        }

        private float r;

        public float R
        {
            get { return r; }
            set { r = value; }
        }

        #endregion

        public float Cos(float angle)
        {
            return (float)Math.Round(Math.Cos(angle * 1.00 * (Math.PI) / 180), 5);
        }

        public float Sin(float angle)
        {
            return (float)Math.Round(Math.Sin(angle * 1.00 * (Math.PI) / 180), 5);
        }

        /// <summary>
        /// 位移旋转矩阵*点的坐标
        /// 得到位移旋转后点的坐标
        /// </summary>
        /// <returns></returns>
        public NDArray TestNdArray()
        {
            //float[,] rotMatrix, float[,] pointMatrix
            float[,] rotMatrix = this.rot;
            float[,] pointMatrix = line;
            NDArray ndRotMatrix = np.array(rotMatrix);
            NDArray ndPointMatrix = np.array(pointMatrix);

            return np.matmul(ndRotMatrix, ndPointMatrix);
        }
    }
}

/*
    //int[,] Rot = new int[4, 4];
    //int[,] Lin = new int[4, 1];
    //NDArray ndarr = np.array(Rot);

    //Console.WriteLine(Math.Round(Math.Sin(90 * 1.00 * (Math.PI) / 180), 5));
    //Console.ReadKey();

    MyCover myCover = new MyCover(3, 7, 0, 10, 5, 0, 30, 0, 0);

    NDArray arr1 = myCover.TestNdArray();
    NDArray arrRot = np.array(myCover.Rot);
    //Console.WriteLine(arr1.ToString());
    //[[0],
    //[-0.5],
    //[0.86603],
    //[1]]

    Console.WriteLine("=================");
    Console.WriteLine(arrRot.ToString());
    Console.WriteLine("======****===========");
    Console.WriteLine(arrRot["0,0"].ToString());
    Console.WriteLine("=================");

    Console.WriteLine(arr1.ToString());
    Console.WriteLine("=================");
    Console.WriteLine(arr1["0, 0"].ToString()); //0
    Console.WriteLine(arr1["1, 0"].ToString()); //-0.5
    Console.WriteLine(arr1["2, 0"].ToString()); //0.86603
    Console.ReadKey();
 
 */