41 lines
1.3 KiB
Python
41 lines
1.3 KiB
Python
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
from mpl_toolkits.mplot3d import Axes3D
|
|
from scipy.spatial.transform import Rotation as R
|
|
|
|
# 定义你的坐标系（原点 + 欧拉角，单位为角度）
|
|
frames = [
|
|
{"origin": [0, 0, 0], "euler": [0, 0, 0]},
|
|
{"origin": [2, 2, 2], "euler": [0, 150, 90]},
|
|
{"origin": [3, 3, 3], "euler": [-180, -30, 0]},
|
|
]
|
|
|
|
def draw_frame(ax, origin, rot_matrix, length=0.5):
|
|
origin = np.array(origin)
|
|
x_axis = origin + rot_matrix @ np.array([length, 0, 0])
|
|
y_axis = origin + rot_matrix @ np.array([0, length, 0])
|
|
z_axis = origin + rot_matrix @ np.array([0, 0, length])
|
|
ax.quiver(*origin, *(x_axis - origin), color='r')
|
|
ax.quiver(*origin, *(y_axis - origin), color='g')
|
|
ax.quiver(*origin, *(z_axis - origin), color='b')
|
|
|
|
fig = plt.figure()
|
|
ax = fig.add_subplot(111, projection='3d')
|
|
ax.set_box_aspect([1,1,1])
|
|
|
|
for frame in frames:
|
|
origin = frame["origin"]
|
|
euler_deg = frame["euler"]
|
|
rotation = R.from_euler('xyz', euler_deg, degrees=True)
|
|
rot_matrix = rotation.as_matrix()
|
|
draw_frame(ax, origin, rot_matrix)
|
|
|
|
ax.set_xlim(-1, 3)
|
|
ax.set_ylim(-1, 3)
|
|
ax.set_zlim(-1, 3)
|
|
ax.set_xlabel("X")
|
|
ax.set_ylabel("Y")
|
|
ax.set_zlabel("Z")
|
|
plt.title("Multiple 3D Coordinate Frames (Euler Angles)")
|
|
plt.show()
|