1.例如现场有拆垛和码垛要求，希望对左侧的3*3*2垛拆垛，并在右侧进行3*3*2的码垛

2.由于左右垛型均非平行于机器人Base坐标系，通常做法为先创建工件坐标系并在对应坐标系内进行计算。

3.可以只对ABB机器人示教点1，点3，点7和点9（即示教四个角点），并设置1->3方向的个数，1->7方向的个数，还有层数和每层高度，即可完成自动计算。

! 码垛工位4个角点
CONST robtarget p100:=*;
CONST robtarget p101:=*;
CONST robtarget p102:=*;
CONST robtarget p103:=*;

! 拆垛工位4个角点
CONST robtarget p200:=*;
CONST robtarget p201:=*;
CONST robtarget p202:=*;
CONST robtarget p203:=*;

CONST robtarget pHome1:=*;

PERS num row_num:=3;
PERS num column_num:=3;
PERS num layer_num:=2;
PERS num height1:=-205;
!拆垛层高，由于往下拆垛，层高设置为负数
PERS num height2:=205;
!码垛层高
VAR robtarget pPallet:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];

PROC main()
Reset do_attach;
row_num:=3;
column_num:=3;
MoveJ pHome1,v1000,fine,tVacuum\WObj:=wobj0;
FOR i FROM 1 TO row_num*column_num*layer_num DO
pPallet:=PalletCal(i,row_num,column_num\layer:=layer_num\height:=height1,p200,p201,p202,p203);
MoveJ Offs(pPallet,0,0,50),vmax,z50,tVacuum\WObj:=wobj0;
MoveL pPallet,v1000,fine,tVacuum\WObj:=wobj0;
Set do_attach;
WaitTime 0.1;
MoveL Offs(pPallet,0,0,300),vmax,z50,tVacuum\WObj:=wobj0;

pPallet:=PalletCal(i,row_num,column_num\layer:=layer_num\height:=height2,p100,p101,p102,p103);
MoveJ Offs(pPallet,0,0,50),vmax,z50,tVacuum\WObj:=wobj0;
MoveL pPallet,v1000,fine,tVacuum\WObj:=wobj0;
Reset do_attach;
WaitTime 0.1;
MoveL Offs(pPallet,0,0,50),vmax,z50,tVacuum\WObj:=wobj0;
ENDFOR
MoveJ pHome1,v1000,fine,tVacuum\WObj:=wobj0;
ENDPROC

FUNC robtarget Interpolate_pose(robtarget p_from,robtarget p_to,num alpha)
! -- Linear interpolation of tool position
! -- When alpha is 0, returns p_from. When alpha is 1, returns p_to. As alpha goes from 0 to 1, returns a
! -- pose going in a straight line from p_from to p_to.
! -- If alpha is less than 0, returns a point before p_from on the line.
! -- If alpha is greater than 1, returns a pose after p_to on the line.

VAR robtarget p:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
p:=p_from;
p.trans.x:=p_from.trans.x+(p_to.trans.x-p_from.trans.x)*alpha;
p.trans.y:=p_from.trans.y+(p_to.trans.y-p_from.trans.y)*alpha;
p.trans.z:=p_from.trans.z+(p_to.trans.z-p_from.trans.z)*alpha;
RETURN p;

ENDFUNC

FUNC robtarget PalletCal(num count,num row,num column\num layer\num height,robtarget startpos,robtarget rowpos,robtarget columnpos,robtarget refpos)

! count:当前第几个产品
! row:该垛的***个方向个数
! column:该垛的第二个方向个数
! startpos: ***个角点
! rowpos: ***个方向末端点
! columnpos:第二个方向末端点
! refpos: ***远端角点
!
! startpos ----------->columnpos
! | 第二个方向 |
! | |
! |***个方向 |
! | |
! rowpos -----------refpos

VAR robtarget outpos1;
VAR robtarget outpos2;
VAR robtarget outpos3;

VAR num current_row;
VAR num current_column;
VAR num current_layer;

current_layer:=(count-1) DIV (row*column);
! 获取当前层数
count:=(count-1) MOD (row*column)+1;
! 获取在当前层的第几个
current_row:=(count-1) MOD row;
current_column:=(count-1) DIV row;

IF (row=1) THEN
row:=2;
ENDIF
IF (column=1) THEN
column:=2;
ENDIF
! 避免该方向只有一个产品

outpos1:=Interpolate_pose(startpos,rowpos,current_row/(row-1));
outpos2:=Interpolate_pose(columnpos,refpos,current_row/(row-1));
outpos3:=Interpolate_pose(outpos1,outpos2,current_column/(column-1));
IF Present(height) THEN
outpos3.trans.z:=outpos3.trans.z+current_layer*height;
endif
RETURN outpos3;
ENDFUNC