码垛的代码实现

从不同的位置移动物体，分解动作为

1. 将夹子从垂直方向移入物体
2. 收拢夹子
3. 垂直搬移物体到目标位置
4. 松开夹子
5. 从物体中退出并移动到新物体上方

2234.gif

MOVEJ动作的使用

image.png

• 移入物体中间和从物体中移除时使用的是MOVEJ动作，限定在竖直方向移出，移入

code

两种实现方式：

• 串行：前一个动作执行完成后，再下载下一个动作，然后执行
• 并行：前一个动作执行过程中开始下载下一个动作到不同的空间中
  • 4个动作分别放置在了4个不同的空间中
  • 张开夹子
  • 闭合夹子
  • 搬移物体-去程
  • 搬移物体-回程
• 移动过程是一个循环，每次循环中计算路径的起止位置

# -*- coding = utf-8 -*-

import sinanju 

zeropt = [ 255,0,512, 30 ]

P0=[ 200, 100, 160,0 ]
P1=[ 200, -100, 160,0 ]

# width, depth, height
cubic = ( 100, 100, 50 )
# cubic = ( 100, 100, 60 )
cubicCount = 3

# open,close
handAction=( 150, 110 )

openPage = 8
closePage = 9

handTime = 1
moveTime = 2

jSetting=( 100, 1 )

def serialDock():
    # connect
    robo = sinanju.Sinanju( "Sinanju2" )
    idn = robo.IDN
    print(idn)

    # prepare the hand
    robo.preMove( 0, openPage, 
                    (0,0,0, handAction[1] ), 
                    (0,0,0, handAction[0] ),
                    handTime
                    )
    robo.waitEnd( 0, openPage )

    robo.preMove( 0, closePage, 
                    (0,0,0, handAction[0] ),
                    (0,0,0, handAction[1] ),
                    handTime
                   )            
    robo.waitEnd( 0, closePage )                           

    dstPt = zeropt
    dstPt[3] = 0
    for aimIndex in range( cubicCount-1, -1, -1 ):
        # print( aimIndex, P0 )
        # move to first
        aimPt = P0[:] 

        aimPt[ 2 ] = aimPt[2] + cubic[2] * ( aimIndex ) + cubic[2]/2  
        print( dstPt, aimPt )
        robo.movej( 0, 0, dstPt, aimPt, moveTime, jSetting[0], jSetting[1] )
        robo.waitIdle( 0, 0 )

        # drag
        robo.call( 0, closePage )
        robo.waitIdle( 0, closePage )

        # move the dst
        dstPt = P1[:] 

        dstPt[2] = dstPt[2] + cubic[2] * ( cubicCount - 1 - aimIndex ) + cubic[2]/2
        print( aimPt, dstPt )
        robo.movej( 0, 0, aimPt, dstPt, moveTime, jSetting[0], jSetting[1] )
        robo.waitIdle( 0, 0 )

        # drop
        robo.call( 0, openPage )
        robo.waitIdle( 0, openPage )

    robo.movej( 0,0, dstPt, zeropt, moveTime, jSetting[0], jSetting[1] )
    robo.waitIdle( 0, 0 )

def parallelDock():
    # connect
    robo = sinanju.Sinanju( "Sinanju2" )
    idn = robo.IDN
    print(idn)

    # prepare the hand
    robo.preMove( 0, openPage, 
                    (0,0,0, handAction[1] ), 
                    (0,0,0, handAction[0] ),
                    handTime
                    )
    robo.waitEnd( 0, openPage )

    robo.preMove( 0, closePage, 
                    (0,0,0, handAction[0] ),
                    (0,0,0, handAction[1] ),
                    handTime
                   )            
    robo.waitEnd( 0, closePage )                           

    dstPt = zeropt
    dstPt[3] = 0

    # first
    aimIndex = cubicCount - 1
    aimPt = P0[:] 
    aimPt[ 2 ] = aimPt[2] + cubic[2] * ( aimIndex ) + cubic[2]/2  
    print( dstPt, aimPt )
    robo.preMovej( 0, 0, dstPt, aimPt, moveTime, jSetting[0], jSetting[1] )

    for aimIndex in range( cubicCount-1, -1, -1 ):
        # print( aimIndex, P0 )

        if ( aimIndex != cubicCount - 1 ):
            robo.waitIdle( 0, openPage )
        robo.waitEnd( 0, 0 )
        robo.call( 0, 0 )

        # prog b
        dstPt = P1[:] 

        dstPt[2] = dstPt[2] + cubic[2] * ( cubicCount - 1 - aimIndex ) + cubic[2]/2
        print( aimPt, dstPt )
        robo.preMovej( 0, 2, aimPt, dstPt, moveTime, jSetting[0], jSetting[1] )
        
        # drag
        robo.waitIdle(0,0)
        robo.call( 0, closePage )
        robo.waitIdle( 0, closePage )

        # move the dst
        robo.waitEnd( 0, 2 )
        robo.call( 0, 2 )

        # prog a
        aimPt = P0[:] 
        aimPt[ 2 ] = aimPt[2] + cubic[2] * ( aimIndex - 1 ) + cubic[2]/2  
        print( dstPt, aimPt )
        robo.preMovej( 0, 0, dstPt, aimPt, moveTime, jSetting[0], jSetting[1] )

        # drop
        robo.waitIdle( 0, 2 )
        robo.call( 0, openPage )

    robo.movej( 0,0, dstPt, zeropt, moveTime, jSetting[0], jSetting[1] )
    robo.waitIdle( 0, 0 )    


if __name__ == "__main__":
    # seiralDock()
    parallelDock()