python - 绕原点进行 3D 旋转

Question

我想围绕原点变换姿势。我的姿势包含一个位置 [x,y,z] 和一个方向 [x,y,z,w]，表示为四元数。我有一个 4x4 变换矩阵，用于围绕原点平移和旋转姿势的位置。

np.dot(transforma_mat, np.transpose(np.append(pose.position, 1)))

因此，随着原点移动和旋转，姿势的起点也会移动和旋转。但是，我不确定如何旋转姿势的方向。

我可以使用 this library 将四元数转换为欧拉角和旋转矩阵(反之亦然) 。

故事: 有一个物体会移动和旋转。在物体移动和旋转之前，机器人末端执行器具有一个位姿。我想更新末端执行器的姿势，使其随对象移动和旋转。

Answer 1

我不确定如何使用四元数，但是，您可以将方向定义为两个正交向量(遵守右手或左手规则)。

然后，您可以简单地将坐标系中的这些向量乘以变换矩阵即可获得变换后的方向。