用户可以使用以下代码，计算坐标系，p1为原点，p1p2为x方向，p1p2p3构成xy平面

# author: chenliao@elibot.cn
import math
def dot_product(v1, v2):  
    return sum(a*b for a, b in zip(v1, v2))  
  
def cross_product(v1, v2):  
    return [v1[1]*v2[2] - v1[2]*v2[1],   
            v1[2]*v2[0] - v1[0]*v2[2],   
            v1[0]*v2[1] - v1[1]*v2[0]]  

def Mat2RPY(rotation_matrix):
    # 计算 yaw 角
    yaw = math.atan2(rotation_matrix[1][0], rotation_matrix[0][0])
    # 计算 pitch 角
    pitch = math.atan2(-rotation_matrix[2][0], 
                    math.sqrt(rotation_matrix[2][1]**2 + rotation_matrix[2][2]**2))
    # 计算 roll 角
    roll = math.atan2(rotation_matrix[2][1], rotation_matrix[2][2])
    return ([roll,pitch,yaw])

def cal_user(p1,p2,p3):
    # p1为原点， p1p2为x方向， p1p2p3构成xy平面
    # 返回user

    # 计算两个向量
    v1 = [0,0,0]
    v2 = [0,0,0]
    for i in range(0,3):
        v1[i] = p2[i]-p1[i]
        v2[i] = p3[i]-p1[i]

    # 计算法向量    
    normal  = cross_product(v1,v2)
    # 确保法向量为单位向量
    normal = normalize(normal)   

    # 计算机器人坐标系的 x、y、z 轴单位向量
    x_axis = normalize(v1)
    z_axis = normal
    y_axis = cross_product(z_axis, x_axis)
    y_axis = normalize(y_axis)
    mat_rot = [[x_axis[0],y_axis[0],z_axis[0]],
                [x_axis[1],y_axis[1],z_axis[1]],
                [x_axis[2],y_axis[2],z_axis[2]]]
    user_rad = Mat2RPY(mat_rot)
    return [p1[0],p1[1],p1[2],user_rad[0],user_rad[1],user_rad[2]]

def alignTCP(user=[0,0,0,0,0,0]):
    # 将当前末端姿态的z垂直用户坐标系的xy平面
    p_in_base =get_actual_tcp_pose()
    p_in_user = pose_trans(pose_inv(user),p_in_base)
    rx = p_in_user[3]
    ry = p_in_user[4]
    rz = p_in_user[5]
    if (rx>=90) or (rx<=-90):
        rx = 180
    else:
        rx = 0
    ry = 0
    p_in_user[3] = rx
    p_in_user[4] = ry
    movel(pose_trans(user,p_in_user),v=0.1)

# # 三个关键点的坐标
p1 = [0.4870015119080839, -0.14749998942204118, 0.49750784416160615]
p2 = [0.5620267993968734, -0.1928286859223005, 0.49750488511249974]
p3 = [0.49155327034810975, -0.278376837186979, 0.5244138572890163]
user = cal_user(p1, p2, p3)
# 返回坐标系单位m，rad