我太懂你现在的挫败感了——想绕F₁的Z轴旋转F₂，结果要么让它绕原点O转了个寂寞，要么绕自己的Z轴原地打转，完全不是想要的效果。别慌，咱们先把坐标系变换的逻辑捋明白，一步步解决问题。

先揪出你之前的核心问题 #

你之前的操作踩了两个典型的坐标系变换坑：

1. 相对变换矩阵搞反了：
   你算的 T_rel = np.linalg.inv(T_2).dot(T_1)，这个矩阵表示的是F₁相对于F₂的位姿，而不是你需要的F₂相对于F₁的位姿，这直接导致后续的旋转操作完全偏离了目标。
2. 旋转的坐标系选错了：
   • 第一种方法里的T_rotation是定义在原点O坐标系下的，直接左乘自然会让F₂绕O旋转；
   • 第二种方法里直接修改T_rel的四元数，相当于在F₂自身坐标系下做旋转，结果当然是绕自己的Z轴转。

正确的操作流程（基于齐次变换） #

先明确几个关键矩阵的定义：

• T₁：F₁在原点O坐标系下的位姿（齐次变换矩阵，作用是把F₁中的点坐标转换到O坐标系）
• T₂：F₂在原点O坐标系下的位姿

要绕F₁的Z轴旋转F₂，本质是先把F₂的位姿转换到F₁坐标系下，在F₁的“地盘”里完成旋转，再转换回O坐标系。具体步骤如下：

1. 计算F₂相对于F₁的正确位姿矩阵 #

我们需要得到F₂在F₁坐标系下的位姿T₁₂，计算公式是：

import numpy as np

# 你的原始T1和T2
T_1 = np.array([
    [1,      0,       0, 0.3000],
    [0, 0.9397, -0.3420,      0],
    [0, 0.3420,  0.9397,      0],
    [0,      0,       0,      1]
])

T_2 = np.array([
    [      0,      0, 1,  0.3000],
    [ 0.6434, 0.7655, 0, -0.0572],
    [-0.7655, 0.6434, 0, -0.0474],
    [      0,      0, 0,       1]
])

# 计算F2在F1坐标系下的位姿
T_12 = np.linalg.inv(T_1) @ T_2

解释：np.linalg.inv(T₁)是从O坐标系转换到F₁坐标系的变换，乘上T₂（F₂在O中的位姿），就能得到F₂在F₁坐标系下的位置和姿态。

2. 定义F₁坐标系下的绕Z轴旋转矩阵 #

这个旋转矩阵是在F₁坐标系内定义的，平移部分为0（因为只是旋转，不改变在F₁坐标系下的位置）：

theta = np.deg2rad(30)
# 绕F1的Z轴旋转30度的齐次变换矩阵
T_rot = np.array([
    [np.cos(theta), -np.sin(theta), 0, 0],
    [np.sin(theta),  np.cos(theta), 0, 0],
    [0,              0,             1, 0],
    [0,              0,             0, 1]
])

3. 在F₁坐标系下完成旋转 #

把F₂在F₁中的位姿左乘旋转矩阵，得到旋转后的F₂在F₁中的位姿：

T_12_rot = T_rot @ T_12

这里要注意：左乘旋转矩阵表示在当前参考坐标系（F₁）下做旋转，这是实现“绕F₁的轴旋转”的关键。

4. 转换回原点O坐标系下的位姿 #

最后把旋转后的位姿转换回O坐标系，得到最终的T₂_new：

T2_new = T_1 @ T_12_rot

现在你可以把T2_new拿去可视化，这次F₂应该会乖乖绕F₁的Z轴旋转30度了。

四元数版本的正确做法 #

如果你想用四元数实现，核心逻辑完全一致：

1. 先把F₂的位姿（平移+四元数）转换到F₁坐标系下；
2. 在F₁坐标系下，用绕Z轴的旋转四元数左乘F₂的四元数（四元数左乘表示在参考坐标系下旋转，右乘才是自身坐标系下的旋转）；
3. 把旋转后的位姿（平移+新四元数）再转换回O坐标系下。

这样就不会出现绕自身轴旋转的问题了。

备注：内容来源于stack exchange，提问作者ravi