四元数（Quaternions）和旋转矩阵是在三维空间中表示旋转的两种常见数学工具。

### 四元数和旋转矩阵相互转换import jax.numpy as jnp
import numpy as np
import math### 1. 角度和弧度相互转化
## 角度转弧度
degrees = 45
radians = math.radians(degrees)
print(f"{degrees} degrees is equal to {radians} radians")## 弧度转角度
radians = math.pi / 4
degrees = math.degrees(radians)
print(f"{radians} radians is equal to {degrees} degrees")### 2. 绕 x，y，z轴旋转一定角度的旋转矩阵
degrees = 45
θ = math.radians(degrees)# 绕x轴45度旋转的旋转矩阵
Rx = jnp.stack([jnp.array([1, 0, 0]),jnp.array([0, math.cos(θ), -math.sin(θ)]),jnp.array([0, math.sin(θ), math.cos(θ)])])print(f"绕x轴{degrees}度旋转的旋转矩阵为:{Rx}") # 绕y轴45度旋转的旋转矩阵
Ry = jnp.stack([jnp.array([math.cos(θ), 0, math.sin(θ)]),jnp.array([0, 1, 0]),jnp.array([-math.sin(θ), 0, math.cos(θ)])])print(f"绕y轴{degrees}度旋转的旋转矩阵为:{Ry}")                   # 绕z轴45度旋转的旋转矩阵    
Rz = jnp.stack([jnp.array([math.cos(θ), -math.sin(θ), 0]),jnp.array([math.sin(θ), math.cos(θ), 0]),jnp.array([0, 0, 1])])      print(f"绕z轴{degrees}度旋转的旋转矩阵为:{Rz}") ##### chatGPT代码 #####
### 3. 四元数和旋转矩阵相互转换-代码1def quaternion_to_rotation_matrix(q):"""Convert a quaternion to a 3x3 rotation matrix."""a, b, c, d = qreturn np.array([[a**2 + b**2 - c**2 - d**2, 2 * (b*c - a*d), 2 * (b*d + a*c)],[2 * (b*c + a*d), a**2 - b**2 + c**2