@staticmethod
def obj_pose_from_transform(transform):
    trans = transform[:3, 3]
    rot_matrix = transform[:3, :3]
    (yaw, pitch, roll) = OpenRAVEBody._ypr_from_rot_matrix(rot_matrix)
    return np.array((trans[0], trans[1], trans[2], yaw, pitch, roll))
@staticmethod
def obj_pose_from_transform(transform):
    trans = transform[:3, 3]
    rot_matrix = transform[:3, :3]
    (yaw, pitch, roll) = OpenRAVEBody._ypr_from_rot_matrix(rot_matrix)
    return np.array((trans[0], trans[1], trans[2], yaw, pitch, roll))