CS机器人自带码垛包，采用如上示教4个角点，设置每行/列个数自动完成码垛。

对于以上的代码简化实现，可以直接下载以下链接的代码，并根据实际工艺在脚本中修改。

链接：https://pan.baidu.com/s/1lmBxH6k3nqSe-b1o1cDkWw
提取码：9e8i

1． 导入附件的default.configuration.variables，pallet4.task和Pallet4ponit.script

2． 全局变量pall_count用于实时保存当前码垛个数

3． 码垛关键点路点_1到路点_4设置位置如下图，即p1到p2为第一个方向，p1到p4为第二个方向

4. 实例函数PalletDemo参数包括：

    抓取位置，

    码垛关键点1，

码垛关键点2，

码垛关键点3，

码垛关键点4，

每一层高度（例如下图的0.2m）

第一个方向个数（例如下图的4，对应上图第一个方向为4个）

第二个方向个数（例如下图的3，对应上图第二个方向为3个）

高度方向个数（例如下图的2）

参考用户坐标系（可以不使用，默认参考base），若使用了参考坐标系，高度方向沿着坐标系方向的z方向，前置点沿着坐标系方向的z方向

4点计算码垛位置代码如下：

#******************
# 用户修改区
def GripperClose():
    # 用户根据实际编写
    sleep(0.1)

def GripperOpen():
    # 用户根据实际编写
    sleep(0.1)

# 用户修改区
#******************

def pallet(curr_no,p10,p20,p30,p40,height,no_row,no_col,no_layer,user = [0,0,0,0,0,0]):
    # 输入当前码垛个数，第一到第四个点，其中p10-p20为row方向，p10-p40为col方向
    # p10,p20,p30,p40基于base
    # 码垛个数从0开始
    # height为层高，可以是负数
    # no_row：第一个方向的个数
    # no_col: 第二个方向的个数
    # no_layer: 高度方向个数

    p1 = p10.copy()
    p2 = p20.copy()
    p3 = p30.copy()
    p4 = p40.copy()

    p1_in_user = pose_trans(pose_inv(user),p1)
    p2_in_user = pose_trans(pose_inv(user),p2)
    p3_in_user = pose_trans(pose_inv(user),p3)
    p4_in_user = pose_trans(pose_inv(user),p4)
 
    curr_layer = curr_no //(no_row*no_col) #获取在第几层
    no_in_layer = curr_no %(no_row*no_col) #获取在该层第几个
    curr_row = (no_in_layer) % no_row  # 获取第几行（从0开始）
    curr_col = (no_in_layer) //no_row  # 获取第几列（从0开始）

    if no_row==1 :
        no_row=2
    if no_col==1:
        no_col =2
    
    outpos1 = interpolate_pose(p1_in_user,p2_in_user,curr_row/(no_row-1))
    outpos2 = interpolate_pose(p4_in_user,p3_in_user,curr_row/(no_row-1))
    outpos3 = interpolate_pose(outpos1,outpos2,curr_col/(no_col-1))
    outpos3[2] = outpos3[2] + height*curr_layer
    # 把用户坐标系下的值转会base下并返回
    return pose_trans(user,outpos3)

def Offs(p, x, y, z):
    p1 =p.copy()
    p1[0] = p1[0]+x
    p1[1] = p1[1]+y
    p1[2] = p1[2]+z
    return p1

def OffsInUser(p, x, y, z,user=[0,0,0,0,0,0]):
    p1 =p.copy()
    p1_in_user = pose_trans(pose_inv(user),p1)

    p1_in_user[0] = p1_in_user[0]+x
    p1_in_user[1] = p1_in_user[1]+y
    p1_in_user[2] = p1_in_user[2]+z
    return pose_trans(user,p1_in_user)

def PalletDemo(pPick,p1,p2,p3,p4,height,no_row,no_col,no_layer,user=[0,0,0,0,0,0]):
    global pall_count

    movej(get_inverse_kin(pPick),r=0)
    #pall_count = 0

    while (pall_count<(no_row*no_col*no_layer)):
        movej(get_inverse_kin(pPick),r=0)
        GripperClose()
        # 抓取动作

        pallet_pose = pallet(pall_count, p1, p2, p3, p4,height,no_row, no_col,no_layer,user)
        pre_pose = OffsInUser(pallet_pose, 0, 0, 0.15,user)
        movej(get_inverse_kin(pre_pose),v=d2r(200),r=0.2)
        movel(Offs(pallet_pose, 0, 0, 0),v=0.2,r=0)
        # 放置
        GripperOpen()
        movel(pre_pose,v=0.5,r=0.2)
        pall_count =pall_count+1
    
    #完成码垛，结束循环
    pall_count = 0
    movej(get_inverse_kin(pPick),r=0)    
    return True