Python NumPy サンプルコード: 3 次元回転行列の座標変換

個人的に科学技術計算でよく使っている Python のコードを備忘録を兼ねて紹介していこうと思います。
今回は機械屋さんには剛体の運動を扱うときに座標変換でお馴染の、 3 次元の回転行列 のコードを紹介します。

座標変換

回転行列、座標変換の説明は Wikipedia など詳しいページが沢山あるのでそちらを参照ください。
回転行列でややこしいのは、回転軸をどの座標系の座標軸にし、回転順序をどう定めるかによって形が異なるところです。
また、回転行列 \(\mathbf{R}\) と座標変換行列 \(\mathbf{A}=\mathbf{R}^{-1}=\mathbf{R}^{T}\) を混同しやすいので注意が必要です。

下図に示す 3 次元空間を考え、

150525.jpg

空間座業系 \(O-\boldsymbol{e}_1\boldsymbol{e}_2\boldsymbol{e}_3\) に対する物体座標系 \(B-\tilde{\boldsymbol{e}}_1\tilde{\boldsymbol{e}}_2\tilde{\boldsymbol{e}}_3\) のブライヤント角(広義のオイラー角)を \(\boldsymbol{\theta}=[{\theta}_1,{\theta}_2,{\theta}_3]^T\) とします。

ベクトル \(\boldsymbol{r}^B, \boldsymbol{r}^P, \boldsymbol{s}^P\) は空間座標系から見たベクトルとし、\(\boldsymbol{r}^P\) は

15052502.png

と表されます。
ここで、\(\mathbf{A}^{OB}\) は物体座標系から見たベクトルを空間座標系から見たベクトルに変換する座標変換行列です。
また、\(\boldsymbol{s}_B^{P}\) は物体座標系から見た \(\boldsymbol{s}^{P}\) です。

物体座標系の軸 1 → 2 → 3 の順番で回転させるとすると、回転行列 \(\mathbf{R}\) は

15052501.png

となります。
ここで、回転行列と座標変換行列の関係は \(\mathbf{A}^{OB}=\mathbf{R}^T\) です。

これを Python で書くと以下のようになります。

# coding: utf-8
from mpl_toolkits.mplot3d import axes3d
import matplotlib.pyplot as plt
import numpy as np


def arrow(v, sp, c):
    # 空間座標基準で矢印をプロットする
    # v:ベクトル、sp:始点、c:色
    ax.quiver(v[0]+sp[0], v[1]+sp[1], v[2]+sp[2],
	      v[0], v[1], v[2],
	      length=np.linalg.norm(v),
	      color=c, linewidth=3)


def rotM(p):
    # 回転行列を計算する
    px = p[0]
    py = p[1]
    pz = p[2]

    # 物体座標系の 1->2->3 軸で回転させる
    Rx = np.array([[1, 0, 0],
		   [0, np.cos(px), np.sin(px)],
		   [0, -np.sin(px), np.cos(px)]])
    Ry = np.array([[np.cos(py), 0, -np.sin(py)],
		   [0, 1, 0],
		   [np.sin(py), 0, np.cos(py)]])
    Rz = np.array([[np.cos(pz), np.sin(pz), 0],
		   [-np.sin(pz), np.cos(pz), 0],
		   [0, 0, 1]])
    R = Rz.dot(Ry).dot(Rx)

    # 物体座標系の 3->2->1 軸で回転させる
    # Rx = np.array([[1, 0, 0],
    #                [0, np.cos(px), np.sin(px)],
    #                [0, -np.sin(px), np.cos(px)]])
    # Ry = np.array([[np.cos(py), 0, -np.sin(py)],
    #                [0, 1, 0],
    #                [np.sin(py), 0, np.cos(py)]])
    # Rz = np.array([[np.cos(pz), np.sin(pz), 0],
    #                [-np.sin(pz), np.cos(pz), 0],
    #                [0, 0, 1]])
    # R = Rx.dot(Ry).dot(Rz)       

    # 空間座標系の 1->2->3 軸で回転させる
    # Rx = np.array([[1, 0, 0],
    #                [0, np.cos(px), -np.sin(px)],
    #                [0, np.sin(px), np.cos(px)]])
    # Ry = np.array([[np.cos(py), 0, -np.sin(py)],
    #                [0, 1, 0],
    #                [np.sin(py), 0, np.cos(py)]])
    # Rz = np.array([[np.cos(pz), np.sin(pz), 0],
    #                [-np.sin(pz), np.cos(pz), 0],
    #                [0, 0, 1]])
    # R = Rx.dot(Ry).dot(Rz)

    # 空間座標系の 3->2->1 軸で回転させる
    # Rx = np.array([[1, 0, 0],
    #                [0, np.cos(px), -np.sin(px)],
    #                [0, np.sin(px), np.cos(px)]])
    # Ry = np.array([[np.cos(py), 0, -np.sin(py)],
    #                [0, 1, 0],
    #                [np.sin(py), 0, np.cos(py)]])
    # Rz = np.array([[np.cos(pz), np.sin(pz), 0],
    #                [-np.sin(pz), np.cos(pz), 0],
    #                [0, 0, 1]])
    # R = Rz.dot(Ry).dot(Rx)
    return R

# 例
p = np.array([np.pi, np.pi/2, np.pi/3])
R = rotM(p)
A = R.T

O = np.array([0, 0, 0])
e1_O = np.array([1, 0, 0])
e2_O = np.array([0, 1, 0])
e3_O = np.array([0, 0, 1])

et1_B = np.array([1, 0, 0])
et2_B = np.array([0, 1, 0])
et3_B = np.array([0, 0, 1])

B = np.array([-2, -2, 1])
et1_O = np.dot(A, et1_B)
et2_O = np.dot(A, et2_B)
et3_O = np.dot(A, et3_B)

sP_B = np.array([1, 2, 3])
sP_O = np.dot(A, sP_B)
rB_O = B - O
rP_O = rB_O + sP_O

# プロット
fig = plt.figure()
ax = fig.gca(projection='3d')
arrow(e1_O, O, "k")
arrow(e2_O, O, "k")
arrow(e3_O, O, "k")
arrow(et1_O, B, "k")
arrow(et2_O, B, "k")
arrow(et3_O, B, "k")
arrow(rB_O, O, "r")
arrow(sP_O, B, "c")
arrow(rP_O, O, "b")
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
plt.savefig("../files/150525rotM01.svg")
# plt.show()

Sorry, your browser does not support SVG.

コメント

Comments powered by Disqus
書籍更新情報
2016-10-21
Pythonによる科学技術計算 基礎編
PDF版の販売を開始しました。
販売ページはこちら

2016-09-09
Pythonによる科学技術計算 基礎編
1.2版への更新が可能になりました。
サポートページはこちら
電子書籍
Pythonによる科学技術計算 基礎編
Kindle ストア、Leanpubで販売中です
Pythonによる科学技術計算 基礎編
PDF版の販売はこちら
同人誌
技術書典(2016/6/25)
Emacs/org-modeのPDF作成術
電子版をBOOTHで販売中です
Emacs/org-modeのPDF作成術
Share