def drakePoseToAtlasCommand(drakePose): jointIndexMap = robotstate.getRobotStateToDrakePoseJointMap() robotState = np.zeros(len(jointIndexMap)) for jointIdx, drakeIdx in jointIndexMap.iteritems(): robotState[jointIdx] = drakePose[drakeIdx] position = robotState.tolist() msg = newAtlasCommandMessageAtZero() msg.position = position return msg
def atlasCommandToDrakePose(msg): jointIndexMap = robotstate.getRobotStateToDrakePoseJointMap() drakePose = np.zeros(len(robotstate.getDrakePoseJointNames())) for jointIdx, drakeIdx in jointIndexMap.iteritems(): drakePose[drakeIdx] = msg.position[jointIdx] return drakePose.tolist()