def baxterMoveOver(arm, blockRow, blockCol, blockHeight):
global numBlocks
global lastBaxterRightLoc
global lastBaxterLeftLoc
if arm == 'right':
(row, col) = lastBaxterRightLoc
else:
(row, col) = lastBaxterLeftLoc
(x,y,z) = gridToCartesian.toCartesian(row, col, numBlocks+1)
(BaxX, BaxY, BaxZ) = gridToCartesian.toBaxter(x,y,z)
(IKValid, IKJoints) = baxterIKRequest(BaxX, BaxY, BaxZ, arm)
if IKValid[0] == True:
joints = IKJoints
baxterMover(arm, joints)
else:
print "Baxter MoveOver (" + str(BaxX) + ", " + str(BaxY) + ", " + str(BaxZ) + ") failed UPMOTION!"
(x,y,z) = gridToCartesian.toCartesian(blockRow, blockCol, numBlocks+1)
(BaxX, BaxY, BaxZ) = gridToCartesian.toBaxter(x,y,z)
(IKValid, IKJoints) = baxterIKRequest(BaxX, BaxY, BaxZ, arm)
if IKValid[0] == True:
if arm == 'right':
lastBaxterRightLoc = (blockRow, blockCol)
else:
lastBaxterLeftLoc = (blockRow, blockCol)
joints = IKJoints
baxterMover(arm, joints)
else:
print "Baxter MoveOver (" + str(BaxX) + ", " + str(BaxY) + ", " + str(BaxZ) + ") failed OVERMOTION!"
(x,y,z) = gridToCartesian.toCartesian(blockRow, blockCol, blockHeight+1)
(BaxX, BaxY, BaxZ) = gridToCartesian.toBaxter(x,y,z)
(IKValid, IKJoints) = baxterIKRequest(BaxX, BaxY, BaxZ, arm)
if IKValid[0] == True:
joints = IKJoints
baxterMover(arm, joints)
else:
print "Baxter MoveOver (" + str(x) + ", " + str(y) + ", " + str(z) + ") failed DOWNMOTION!"
return
def baxterMoveTo(arm, blockRow, blockCol, blockHeight):
(x,y,z) = gridToCartesian.toCartesian(blockRow, blockCol, blockHeight)
(BaxX, BaxY, BaxZ) = gridToCartesian.toBaxter(x,y,z)
(IKValid, IKJoints) = baxterIKRequest(BaxX, BaxY, BaxZ, arm)
if IKValid[0] == True:
joints = IKJoints
baxterMover(arm, joints)
else:
print "Baxter MoveTo (" + str(BaxX) + ", " + str(BaxY) + ", " + str(BaxZ) + ") failed!"
