def message_to_pose(message, from_frame='yumi'):
tokens = message.split()
try:
if len(tokens) != 7:
raise Exception("Invalid format for pose! Got:\n{0}".format(message))
pose_vals = [float(token) for token in tokens]
q = pose_vals[3:]
t = pose_vals[:3]
R = RigidTransform.rotation_from_quaternion(q)
pose = RigidTransform(R, t, from_frame=from_frame)
pose.position = pose.position * MM_TO_METERS
return pose
except Exception, e:
logging.error(e)
