はじめに

ロボティクスにおいて、幾何学処理、

特に三次元の回転は基本的な処理です。

myenigma.hatenablog.com

上記の記事で、ROSのtfを使った場合の三次元回転処理や、

C++のEigenを使った場合の三次元回転処理を紹介しましたが、

Pythonで三次元回転を扱う場合には、

scipy.spatial.transform.Rotationを使うのが便利です。

docs.scipy.org

今回の記事では、

このscipy.spatial.transform.Rotationの使い方について

簡単に紹介したいと思います。

scipy.spatial.transform.Rotationにおける三次元回転の表現方法

scipy.spatial.transform.Rotationでは、

下記の4つの三次元回転の表現方法をサポートしており、

それぞれの表現方法から、別の表現方法に相互変換できます。

クォータニオン
回転ベクトル
回転行列
オイラー角

それぞれの表現方法の特徴に関しては、下記の記事が非常にわかりやすいです。

kamino.hatenablog.com

下記のように、いずれかの表現方法で作成したインスタンスを、

様々な表現方法に変換できます。

from scipy.spatial.transform import Rotation as Rot

rot1 = Rot.from_euler("xyz", [0.0, 0.0, 90.0], degrees=True)
print(f"{rot1.as_quat()=}")
# rot1.as_quat()=array([0.        , 0.        , 0.70710678, 0.70710678])

print(f"{rot1.as_matrix()=}")
# rot1.as_matrix()=array([[ 2.22044605e-16, -1.00000000e+00,  0.00000000e+00],
#        [ 1.00000000e+00,  2.22044605e-16,  0.00000000e+00],
#        [ 0.00000000e+00,  0.00000000e+00,  1.00000000e+00]])

print(f"{rot1.as_euler('zyx', degrees=True)=}")
# rot1.as_euler('zyx', degrees=True)=array([90.,  0.,  0.])

print(f"{rot1.as_rotvec()=}")
# rot1.as_rotvec()=array([0.        , 0.        , 1.57079633])

オイラー角の場合、角度の指定順番を指定できるのと、

角度の単位はデフォルトでradianです。

三次元点の回転

作成した回転情報に基づく、

三次元点の回転は、apply関数を使って実現できます。

p1 = [1.0, 0.0, 0.0]
rot1 = Rot.from_euler("xyz", [0.0, 0.0, 90.0], degrees=True)
p2 = rot1.apply(p1)
print(f"{p2=}")
# p2=array([2.22044605e-16, 1.00000000e+00, 0.00000000e+00])

N x 3のarrayを作れば、

複数の三次元点を一気に回転させることもできます。

points1 = [[1.0, 0.0, 0.0],
          [0.0, 1.0, 0.0],
          [0.0, 0.0, 1.0]]
rot1 = Rot.from_euler("xyz", [0.0, 0.0, 90.0], degrees=True)
points2 = rot1.apply(points1)
print(f"{points2=}")
# points2=array([[ 2.22044605e-16,  1.00000000e+00,  0.00000000e+00],
#        [-1.00000000e+00,  2.22044605e-16,  0.00000000e+00],
#        [ 0.00000000e+00,  0.00000000e+00,  1.00000000e+00]])

x-yの点のリストからは、下記のように回転できます。

x = [1.0, 0.0, 1.0, 1.0]
y = [1.0, 1.0, 1.0, 1.0]
points1 = np.stack([x, y, np.zeros(np.array(x).shape)]).T
points = Rot.from_euler("z", 90.0, degrees=True).apply(points1)
print(f"{points=}")
# points=array([[-1.00000000e+00,  1.00000000e+00,  0.00000000e+00],
#        [-1.00000000e+00,  2.22044605e-16,  0.00000000e+00],
#        [-1.00000000e+00,  1.00000000e+00,  0.00000000e+00],
#        [-1.00000000e+00,  1.00000000e+00,  0.00000000e+00]])

回転の掛け合わせ

複数の回転を組み合わせて、

一つの回転を作りたい場合は、

掛け合わせることで実現できます。

rot1 = Rot.from_euler("xyz", [0.0, 0.0, 90.0], degrees=True)
rot2 = Rot.from_euler("xyz", [0.0, 10.0, 0.0], degrees=True)
rot3 = rot1 * rot2
print(f"{rot3.as_euler('xyz', degrees=True)=}")
# rot3.as_euler('xyz', degrees=True)=array([ 0., 10., 90.])

逆回転

ある回転の逆回転はinv関数で計算できます。

p = Rot.from_euler('z', 45, degrees=True)
q = p.inv()
print(f"{q.as_euler('zyx', degrees=True)=}")
# q.as_euler('zyx', degrees=True)=array([-45.,   0.,   0.])