已知 [x,y,z,rx,ry,rz ] 求 RT矩阵 ，给出 python 代码 rx ry rz 是 欧拉角 xyz 使用 numpy

import numpy as np

def euler_xyz_to_rotation_matrix(rx, ry, rz, degrees=False):
    """
    手动计算 XYZ 欧拉角到旋转矩阵的转换
    
    Args:
        rx, ry, rz: 绕X、Y、Z轴的旋转角度
        degrees: 如果为True，输入角度为度数；如果为False，输入为弧度
    
    Returns:
        3x3 旋转矩阵
    """
    if degrees:
        rx, ry, rz = np.radians(rx), np.radians(ry), np.radians(rz)
    
    # 绕X轴旋转的矩阵
    Rx = np.array([
        [1, 0, 0],
        [0, np.cos(rx), -np.sin(rx)],
        [0, np.sin(rx), np.cos(rx)]
    ])
    
    # 绕Y轴旋转的矩阵
    Ry = np.array([
        [np.cos(ry), 0, np.sin(ry)],
        [0, 1, 0],
        [-np.sin(ry), 0, np.cos(ry)]
    ])
    
    # 绕Z轴旋转的矩阵
    Rz = np.array([
        [np.cos(rz), -np.sin(rz), 0],
        [np.sin(rz), np.cos(rz), 0],
        [0, 0, 1]
    ])
    
    # XYZ顺序：先X，再Y，最后Z (R = Rz * Ry * Rx)
    rotation_matrix = Rz @ Ry @ Rx
    
    return rotation_matrix

def manual_euler_xyz_to_rt_matrix(pose, degrees=False):
    """
    手动实现 XYZ 欧拉角到 RT 矩阵的转换
    """
    # 提取平移部分
    translation = np.array(pose[:3])
    
    # 提取欧拉角
    rx, ry, rz = pose[3:]
    
    # 计算旋转矩阵
    rotation_matrix = euler_xyz_to_rotation_matrix(rx, ry, rz, degrees)
    
    # 构建 4x4 RT 矩阵
    rt_matrix = np.eye(4)
    rt_matrix[:3, :3] = rotation_matrix
    rt_matrix[:3, 3] = translation
    
    return rt_matrix

# 示例使用
pose = [1.0, 2.0, 3.0, 0.1, 0.2, 0.3]  # 弧度
rt_matrix = manual_euler_xyz_to_rt_matrix(pose, degrees=False)
print("手动计算的 RT 矩阵 (XYZ欧拉角):")
print(rt_matrix)

手动计算的 RT 矩阵 (XYZ欧拉角):
[[ 0.93629336 -0.27509585  0.21835066  1.        ]
 [ 0.28962948  0.95642509 -0.03695701  2.        ]
 [-0.19866933  0.0978434   0.97517033  3.        ]
 [ 0.          0.          0.          1.        ]]