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!"