# python将四元数变换为旋转矩阵的实例

```import numpy as np
from autolab_core import RigidTransform

# 写上用四元数表示的orientation和xyz表示的position
orientation = {"y": -0.6971278819736084, "x": -0.716556549511624, "z": -0.010016582945017661, "w": 0.02142651612120239}
position = {"y": -0.26022684372145516, "x": 0.6453529828252734, "z": 1.179122068068349}

rotation_quaternion = np.asarray([orientation["w"], orientation["x"], orientation["y"], orientation["z"]])
translation = np.asarray([position["x"], position["y"], position["z"]])
# 这里用的是UC Berkeley的autolab_core，比较方便吧，当然可以自己写一个fuction来计算，计算公式在https://www.cnblogs.com/flyinggod/p/8144100.html
T_qua2rota = RigidTransform(rotation_quaternion, translation)

print(T_qua2rota)

# 以下是打印的结果
Tra: [ 0.64535298 -0.26022684 1.17912207]
Rot: [[ 0.02782477 0.99949234 -0.01551915]
[ 0.99863386 -0.02710724 0.0446723 ]
[ 0.04422894 -0.01674094 -0.99888114]]
Qtn: [-0.02142652 0.71655655 0.69712788 0.01001658]
from unassigned to world
```

```def quaternion_to_rotation_matrix(quat):
q = quat.copy()
n = np.dot(q, q)
if n < np.finfo(q.dtype).eps:
return np.identity(4)
q = q * np.sqrt(2.0 / n)
q = np.outer(q, q)
rot_matrix = np.array(
[[1.0 - q[2, 2] - q[3, 3], q[1, 2] + q[3, 0], q[1, 3] - q[2, 0], 0.0],
[q[1, 2] - q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] + q[1, 0], 0.0],
[q[1, 3] + q[2, 0], q[2, 3] - q[1, 0], 1.0 - q[1, 1] - q[2, 2], 0.0],
[0.0, 0.0, 0.0, 1.0]],
dtype=q.dtype)
return rot_matrix```

https://doc.rc-visard.com/latest/de/pose_formats.html?highlight=format