【CSEN052】Simple palletizing of CS series: teaching 4-point method


The CS robot system has a built-in palletizing process application. It uses the 4 corner points taught as above and sets the number of each row/column to automatically complete palletizing.For the simplified implementation of the above code, you can directly download the code from the following link and modify it in the script according to the actual process.

Download Link: 

Steps:
1. Import attachment files:  default.configuration.variables, pallet4.task and Pallet4ponit.script

2. Set the 4 points in clockwise or anticlockwise, Waypoint_1 to Waypoint_4 are the stackpoints for a square tray. And the assign them before the palletizing task tree, MoveJ command is suggested. 

     
3.The "pPick_p" point at below is the pick point, and the "Ctrl_base_frame" is usually set as robot coordinate


var1= PalletDemo(pPick_p,Waypoint_1_p,Waypoint_2_p,Waypoint_3_p,Waypoint_4_p,0.2,2,2,2,Ctrl_base_frame)



4.Here attached the Pallet4ponit.script

# author: chenliao@elibot.cn
# Mar.22, 2023

#******************
# for user revisement
def GripperClose():
    # please set accoding to the exactly time that cost by gripper closing
    sleep(0.1)

def GripperOpen():
    # please set accoding to the exactly time that cost by gripper opening
    sleep(0.1)

# for user revisement
#******************

def pallet(curr_no,p10,p20,p30,p40,height,no_row,no_col,no_layer,user = [0,0,0,0,0,0]):
    # the para of function pallet: stack points count number, p10, p20,p30,p40 are 4 points, from p10 to p20 is the direction of row, from p10 to p40 is the col.
    # the coordinate of p10,p20,p30,p40 are based on base coordinate.
    # the stack points count number starts from 0
    # height is the height, which could not be zero, but either positive or negtive.
    # no_row:the number of stack points in row
    # no_col: the number of stack points in col
    # no_layer: the number of stack layers

    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) # get the current layer number
    no_in_layer = curr_no %(no_row*no_col) # get the ordinal number in current layer
    curr_row = (no_in_layer) % no_row  # get the ordinal number in row(starts from 0)
    curr_col = (no_in_layer) //no_row  # get the ordinal number in col(starts from 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
    # transfer the coordinates from User coordinate to Base coordinate
    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]):
    #The global variable "pall_count" is used to save the current number of pallets in real time.
    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()
        # Pick

        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)
        # Place
        GripperOpen()
        movel(pre_pose,v=0.5,r=0.2)
        pall_count =pall_count+1
    
    #Completed,and continue to next cycle
    pall_count = 0
    movej(get_inverse_kin(pPick),r=0)    
    return True
点击显示全文
赞同0
发表评论
分享

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