能够在三维空间位姿的欧拉表示法和矩阵表示法之间转换的python程序，顺带画图

 需要先安装 numpy 和 matplotlib 库

import numpy as np
import math
import matplotlib
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from sympy import symbols, Matrix

def cartesian_to_matrix(px, py, pz, roll, pitch, yaw):
    # 笛卡尔坐标转换为旋转矩阵
    R_x = Matrix([[1, 0, 0],
                  [0, np.cos(roll), -np.sin(roll)],
                  [0, np.sin(roll), np.cos(roll)]])

    R_y = Matrix([[np.cos(pitch), 0, np.sin(pitch)],
                  [0, 1, 0],
                  [-np.sin(pitch), 0, np.cos(pitch)]])

    R_z = Matrix([[np.cos(yaw), -np.sin(yaw), 0],
                  [np.sin(yaw), np.cos(yaw), 0],
                  [0, 0, 1]])

    # 合并旋转矩阵
    R = R_z * R_y * R_x

    # 构建位姿矩阵
    T = Matrix([[R[0, 0], R[0, 1], R[0, 2], px],
                [R[1, 0], R[1, 1], R[1, 2], py],
                [R[2, 0], R[2, 1], R[2, 2], pz],
                [0, 0, 0, 1]])

    return T


def matrix_to_cartesian(T):
    # 提取旋转矩阵
    R = T[:3, :3]

    # 提取平移向量
    p = T[:3, 3]

    # 计算欧拉角
    roll = math.atan2(R[2, 1], R[2, 2])*180/np.pi
    pitch = math.atan2(-R[2, 0], math.sqrt(R[2, 1]**2 + R[2, 2]**2))*180/np.pi
    yaw = math.atan2(R[1, 0], R[0, 0])*180/np.pi

    # 返回笛卡尔坐标
    return p[0], p[1], p[2], roll, pitch, yaw


# 示例使用
px = 1.0
py = 2.0
pz = 3.0
roll = np.pi/4
pitch = np.pi/6
yaw = np.pi/3

# 笛卡尔坐标转换为矩阵表示
T1 = cartesian_to_matrix(px, py, pz, roll, pitch, yaw)
print("矩阵表示:")
print(T1)

# 矩阵表示转换为笛卡尔坐标
px, py, pz, roll, pitch, yaw = matrix_to_cartesian(T1)
print("笛卡尔表示:")
print("px:", px)
print("py:", py)
print("pz:", pz)
print("roll:", roll)
print("pitch:", pitch)
print("yaw:", yaw)


# 坐标系画图
def plot_coordinate_system(px, py, pz, roll, pitch, yaw):
    # Convert Euler angles to rotation matrix
    R_x = np.array([[1, 0, 0],
                    [0, np.cos(roll), -np.sin(roll)],
                    [0, np.sin(roll), np.cos(roll)]])

    R_y = np.array([[np.cos(pitch), 0, np.sin(pitch)],
                    [0, 1, 0],
                    [-np.sin(pitch), 0, np.cos(pitch)]])

    R_z = np.array([[np.cos(yaw), -np.sin(yaw), 0],
                    [np.sin(yaw), np.cos(yaw), 0],
                    [0, 0, 1]])

    R = np.dot(R_z, np.dot(R_y, R_x))

    # Create a figure and axis
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')

    # Plot the coordinate system
    origin = np.array([px, py, pz])
    x_axis = np.array([1, 0, 0])
    y_axis = np.array([0, 1, 0])
    z_axis = np.array([0, 0, 1])

    ax.quiver(*origin, *x_axis, color='r')
    ax.quiver(*origin, *y_axis, color='g')
    ax.quiver(*origin, *z_axis, color='b')

    # Plot the transformed coordinate system
    transformed_x = np.dot(R, x_axis)
    transformed_y = np.dot(R, y_axis)
    transformed_z = np.dot(R, z_axis)

    ax.quiver(*origin, *transformed_x, color='r', linestyle='dashed')
    ax.quiver(*origin, *transformed_y, color='g', linestyle='dashed')
    ax.quiver(*origin, *transformed_z, color='b', linestyle='dashed')

    # Set plot limits and labels
    ax.set_xlim([-5, 5])
    ax.set_ylim([-5, 5])
    ax.set_zlim([-5, 5])
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')

    # Show the plot
    plt.show()


# Example usage
px = 1.0
py = 2.0
pz = 3.0
roll = np.pi/4
pitch = np.pi/6
yaw = np.pi/3

plot_coordinate_system(px, py, pz, roll, pitch, yaw)

效果图如下：