@staticmethod
def base_pose_2D_to_mat(pose):
    assert (len(pose) == 2)
    x = pose[0]
    y = pose[1]
    rot = 0
    q = quatFromAxisAngle((0, 0, rot)).tolist()
    pos = [x, y, 0]
    matrix = matrixFromPose((q + pos))
    return matrix
@staticmethod
def base_pose_2D_to_mat(pose):
    assert (len(pose) == 2)
    x = pose[0]
    y = pose[1]
    rot = 0
    q = quatFromAxisAngle((0, 0, rot)).tolist()
    pos = [x, y, 0]
    matrix = matrixFromPose((q + pos))
    return matrix