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