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 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()
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()
