• 在一个板子各个格子间搬移物体
  1. 移动到源位置
  2. 闭合爪子
  3. 移动到目标位置
  4. 松开爪子

  5. 退出目标位置

     
     image.png

代码实现

if __name__ == "__main__":
    arm = sinanju.Sinanju("Sinanju")
    cylinder = Cylinder()
    grid = Grid()

    robo = TianruiRobo( arm, grid, cylinder )
    robo.preStudy()

    posCount = robo.posSize()

    for id in range( 0, posCount ):
        robo.move( id, ( id + 1 )% posCount )

• 创建机械臂对象
• 创建格子里的物体，描述物理尺寸
• 创建网格，描述各个网格的位置
• 创建搬移机器人，搬移机器人由以下部分组成
  • 机械臂
  • 网格
  • 物体

网格

• 网格里面定义了网格点的位置
• 根据序号得到网格的坐标

class Grid():
    def __init__( self ):
        self.mGrids = [
                        ( 400,150 ),
                        ( 200,150 ),
                        ( 200,-150 ),
                        ( 400,-150 )
                ]
        self.mZ = 270                  

    def takeAt( self, index ):
        if ( index >= len(self.mGrids) ):
            raise Exception("over range")
        xy =  self.mGrids[index]           
        coordItem = ( xy[0], xy[1], self.mZ, 0 )            
        return coordItem

搬移机器人

几个method的说明

• guessMoveTime
  • 根据空间位置，计算位置间的移动时间

def guessMoveTime( self, a, b ):
        distance = math.sqrt( math.pow( (a[0]-b[0]),2 ) 
                             + math.pow( (a[1]-b[1]),2 )
                             + math.pow( (a[2]-b[2]),2 ) )
        t = distance / self.mSpeed
        if ( t < 0.5 ):
            t = 0.5
        return t

• posSize
  • 网格点个数
• preStudy
  • 预先计算爪子的张开和闭合动作

def preStudy( self ):
        # open
        self.mRobo.preMove( 0, self.mOpenPage, 
                            (0,0,0, self.mHandAction[0] ), 
                            (0,0,0, self.mHandAction[1] ),
                            self.mHandTime )
        self.mRobo.waitEnd( 0, self.mOpenPage )

        # close 
        self.mRobo.preMove( 0, self.mClosePage, 
                            (0,0,0, self.mHandAction[1] ), 
                            (0,0,0, self.mHandAction[0] ),
                            self.mHandTime )
        self.mRobo.waitEnd( 0, self.mClosePage )  

• openHand
  • 张开爪子

def openHand( self, op1 = None, op2 = None ):
        self.mRobo.call( 0, self.mOpenPage )

        # inter op
        if ( op1 != None ):
            op1

        if ( op2 != None ):
            op2

        self.mRobo.waitIdle( 0, self.mOpenPage )

• closeHand
  • 闭合爪子

def closeHand( self, op1 = None, op2 = None ):
        self.mRobo.call( 0, self.mClosePage )

        # inter op
        if ( op1 != None ):
            op1

        if ( op2 != None ):
            op2

        self.mRobo.waitIdle( 0, self.mClosePage ) 

• move
  • 在两个序号间移动
    1. 得到当前位置
    2. 移动到目标位置
    3. 闭合爪子
    4. 搬移
    5. 展开爪子
    6. 退出格子
• 展开爪子和闭合爪子过程中使用了一个小技巧，在张开、闭合过程中计算下一步动作

def move( self, idFrom, idTo ):
        """
        get an item from idFrom to idTo 
        """
        # get a, b
        coordFrom = self.mGrid.takeAt( idFrom )
        coordTo = self.mGrid.takeAt( idTo ) 

        # now 
        nowPos = self.mRobo.pose()

        # to from
        t = self.guessMoveTime( nowPos, coordFrom )
        self.mRobo.preMovej( 0, self.mMovePage, 
                            nowPos,
                            coordFrom,
                            t,
                            self.mJHeight,
                            self.mJTime
                        )
        print( nowPos, coordFrom )                        
        self.mRobo.waitEnd( 0, self.mMovePage )

        self.mRobo.call( 0, self.mMovePage )
        self.mRobo.waitIdle( 0, self.mMovePage, t * 20 )

        # to to
        nowPos = self.mRobo.pose()
        t = self.guessMoveTime( nowPos, coordTo )

        # close
        print( nowPos, coordTo )  
        self.closeHand( self.mRobo.preMovej( 0, self.mMovePage, nowPos, coordTo, t, self.mJHeight, self.mJTime ) )
        self.mRobo.waitEnd( 0, self.mMovePage )

        # go 
        self.mRobo.call( 0, self.mMovePage )
        self.mRobo.waitIdle( 0, self.mMovePage, t * 20 )
        
        # +height
        nowPos = self.mRobo.pose()
        t = self.guessMoveTime( nowPos, ( nowPos[0], nowPos[1], nowPos[2] + self.mJHeight) )
        # open
        jZ = ( nowPos[0], nowPos[1], nowPos[2] + self.mJHeight,0)
        print( nowPos, coordTo, jZ )  
        self.openHand( self.mRobo.preMove( 0, self.mMovePage, nowPos, jZ, self.mJTime ) )
        self.mRobo.waitEnd( 0, self.mMovePage )

        self.mRobo.call( 0, self.mMovePage )
        self.mRobo.waitIdle( 0, self.mMovePage )

all

class TianruiRobo():
    # configs
    mOpenPage = 8
    mClosePage = 9

    mMovePage = 0

    # hand config
    mHandAction=( 10.0,30.0 )
    mHandTime = 1.0

    mSpeed = 80.0*2
    mJHeight = 50.0
    mJTime = 1.0

    def __init__(self, robo, grid, cylinder ):
        self.mRobo = robo 
        self.mGrid = grid 
        self.mCylinder = cylinder 
        pass 

    def guessMoveTime( self, a, b ):
        distance = math.sqrt( math.pow( (a[0]-b[0]),2 ) 
                             + math.pow( (a[1]-b[1]),2 )
                             + math.pow( (a[2]-b[2]),2 ) )
        t = distance / self.mSpeed
        if ( t < 0.5 ):
            t = 0.5
        return t

    def posSize( self ):
        return len( self.mGrid.mGrids )        

    def preStudy( self ):
        # open
        self.mRobo.preMove( 0, self.mOpenPage, 
                            (0,0,0, self.mHandAction[0] ), 
                            (0,0,0, self.mHandAction[1] ),
                            self.mHandTime )
        self.mRobo.waitEnd( 0, self.mOpenPage )

        # close 
        self.mRobo.preMove( 0, self.mClosePage, 
                            (0,0,0, self.mHandAction[1] ), 
                            (0,0,0, self.mHandAction[0] ),
                            self.mHandTime )
        self.mRobo.waitEnd( 0, self.mClosePage )                            

    def openHand( self, op1 = None, op2 = None ):
        self.mRobo.call( 0, self.mOpenPage )

        # inter op
        if ( op1 != None ):
            op1

        if ( op2 != None ):
            op2

        self.mRobo.waitIdle( 0, self.mOpenPage )

    def closeHand( self, op1 = None, op2 = None ):
        self.mRobo.call( 0, self.mClosePage )

        # inter op
        if ( op1 != None ):
            op1

        if ( op2 != None ):
            op2

        self.mRobo.waitIdle( 0, self.mClosePage )        

    def move( self, idFrom, idTo ):
        """
        get an item from idFrom to idTo 
        """
        # get a, b
        coordFrom = self.mGrid.takeAt( idFrom )
        coordTo = self.mGrid.takeAt( idTo ) 

        # now 
        nowPos = self.mRobo.pose()

        # to from
        t = self.guessMoveTime( nowPos, coordFrom )
        self.mRobo.preMovej( 0, self.mMovePage, 
                            nowPos,
                            coordFrom,
                            t,
                            self.mJHeight,
                            self.mJTime
                        )
        print( nowPos, coordFrom )                        
        self.mRobo.waitEnd( 0, self.mMovePage )

        self.mRobo.call( 0, self.mMovePage )
        self.mRobo.waitIdle( 0, self.mMovePage, t * 20 )

        # to to
        nowPos = self.mRobo.pose()
        t = self.guessMoveTime( nowPos, coordTo )

        # close
        print( nowPos, coordTo )  
        self.closeHand( self.mRobo.preMovej( 0, self.mMovePage, nowPos, coordTo, t, self.mJHeight, self.mJTime ) )
        self.mRobo.waitEnd( 0, self.mMovePage )

        # go 
        self.mRobo.call( 0, self.mMovePage )
        self.mRobo.waitIdle( 0, self.mMovePage, t * 20 )
        
        # +height
        nowPos = self.mRobo.pose()
        t = self.guessMoveTime( nowPos, ( nowPos[0], nowPos[1], nowPos[2] + self.mJHeight) )
        # open
        jZ = ( nowPos[0], nowPos[1], nowPos[2] + self.mJHeight,0)
        print( nowPos, coordTo, jZ )  
        self.openHand( self.mRobo.preMove( 0, self.mMovePage, nowPos, jZ, self.mJTime ) )
        self.mRobo.waitEnd( 0, self.mMovePage )

        self.mRobo.call( 0, self.mMovePage )
        self.mRobo.waitIdle( 0, self.mMovePage )