输入三点坐标p1,p2,p3的xyz，单位mm，返回原点在p1，p1p2为x方向，p1p2p3为xy平面

import numpy as np
import math

def Mat2RPY(mat):
    # 旋转矩阵
    rotation_matrix = np.array(mat)

    # 计算 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])

    # 将角度转换为度数
    roll_deg = math.degrees(roll)
    pitch_deg = math.degrees(pitch)
    yaw_deg = math.degrees(yaw)
    
    return ([roll_deg,pitch_deg,yaw_deg])

def cal_user(p1,p2,p3):
    # 计算两个向量
    v1 = p2 - p1
    v2 = p3 - p1

    # 计算法向量
    normal = np.cross(v1, v2)

    # 确保法向量为单位向量
    normal = normal / np.linalg.norm(normal)

    # 计算机器人坐标系的 x、y、z 轴单位向量
    x_axis = v1 / np.linalg.norm(v1)
    z_axis = normal
    y_axis = np.cross(z_axis, x_axis)

    # 通过三个单位向量构建变换矩阵
    mat1 = np.column_stack((x_axis, y_axis, z_axis, p1))

    mat_rot = [mat1[0][:3],mat1[1][:3],mat1[2][:3]]
    angle = Mat2RPY(mat_rot)

    return [mat1[0][3],mat1[1][3],mat1[2][3],angle[0],angle[1],angle[2]]

# 三个关键点的坐标
p1 = np.array([1093.97,  34.20 , 1100.00])
p2 = np.array([1059.77 , 128.17 , 1100.00])
p3 = np.array([1000.00 , 0.00,  1100.00])

out = cal_user(p1,p2,p3)
# 返回坐标系xyzabc，单位mm，deg
print(out)