@staticmethod
def mat_to_base_pose(mat):
    pose = poseFromMatrix(mat)
    x = pose[4]
    y = pose[5]
    rot = axisAngleFromRotationMatrix(mat)[2]
    return np.array([x, y, rot])
@staticmethod
def mat_to_base_pose(mat):
    pose = poseFromMatrix(mat)
    x = pose[4]
    y = pose[5]
    rot = axisAngleFromRotationMatrix(mat)[2]
    return np.array([x, y, rot])