CS示教4点码垛实现



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


点击显示全文
赞同0
发表评论
分享

手机扫码分享
1
504
收藏
举报
收起
登录
  • 密码登录
  • 验证码登录
还没有账号,立即注册
还没有账号,立即注册
注册
已有账号,立即登录
选择发帖板块
举报
请选择举报理由
举报
举报说明