- 在一个板子各个格子间搬移物体
- 移动到源位置
- 闭合爪子
- 移动到目标位置
- 松开爪子
-
退出目标位置
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
- 在两个序号间移动
- 得到当前位置
- 移动到目标位置
- 闭合爪子
- 搬移
- 展开爪子
- 退出格子
- 在两个序号间移动
- 展开爪子和闭合爪子过程中使用了一个小技巧，在张开、闭合过程中计算下一步动作
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 )
网友评论