概要
wikipedia
テニスラケットの原理とは
剛体の第1，第3の慣性主軸のまわりの回転は安定しているが、第2の慣性主軸（中間軸）のまわりの回転は不安定である。』
一般に，回転する物体が外部トルクを与えなければ，回転軸は動かず，ずっと同じ角速度で回転すると考えられてきました．
このgifは上方向（z方向）にずっと回転しつづけていますが，右方向（x方向）の角速度をわずかに初期値として与えると
このgifのように，z軸方向からずれて，z-軸方向（x-y方向ではありません）に変わっています．
実際に宇宙で見られた映像も面白いです．
ロングバージョンも公開しました．
シミュレーション方法
M_A a_{OA}^O = F_A^O\\
I_A^A \frac{d}{dt}\omega_{A}^A = N_A^A - cross(\omega_{A}^A) I_A^A \omega_{A}^A\\
に従います．
F_A^O = 0
として，並進方向の動きはないとします．よって，角加速度・角速度・姿勢（クォータニオン）の3つ（それぞれ3方向あるので合計9つ）を追えばよいことになります．
外部トルクもないとします．
N_A^A = 0
流れ
- 初期姿勢（クォータニオン）・角速度を与える
- 角加速度ベクトルを計算
- 角速度ベクトルを更新（オイラー法）
- クォータニオンを更新（オイラー法）
- 2に戻り，繰り返す
以上の流れです．
クォータニオンと座標変換行列の関係
座標系Aの座標系Oに対するクォータニオン$q$を次のように置くと
q=
\left(
\begin{array}{ccc}
\epsilon_0\\
\epsilon_1\\
\epsilon_2\\
\epsilon_3\\
\end{array}
\right)
座標変換行列は
C_{OA} =
\left(
\begin{array}{ccc}
\epsilon_1^2 - \epsilon_2^2 -\epsilon_3^2 + \epsilon_0^2 & 2(\epsilon_1 \epsilon_2-\epsilon_3\epsilon_0) & 2(\epsilon_1 \epsilon_3+\epsilon_2\epsilon_0) \\
2(\epsilon_1 \epsilon_2+\epsilon_3\epsilon_0) & \epsilon_2^2 - \epsilon_3^2 -\epsilon_1^2 + \epsilon_0^2 & 2(\epsilon_2 \epsilon_3-\epsilon_1\epsilon_0) \\
2(\epsilon_1 \epsilon_3-\epsilon_2\epsilon_0) & 2(\epsilon_2 \epsilon_3+\epsilon_1\epsilon_0) &\epsilon_3^2 - \epsilon_1^2 -\epsilon_2^2 + \epsilon_0^2\\
\end{array}
\right)
で計算できます．
角速度ベクトルとクォータニオンの時間微分の関係
$\omega_A^A $を、第一成分0で、次のようにクォータニオン形式にしたものを$\vec{\omega _{v}}$とします
\vec{\omega_{v}}= \left(
\begin{array}{ccc}
0\\
\omega_{x}\\
\omega_{y}\\
\omega_{z}\\
\end{array}
\right) =\left(
\begin{array}{ccc}
0\\
\lambda_{x} \omega\\
\lambda_{y} \omega\\
\lambda_{z} \omega\\
\end{array}
\right)
とすると，クォータニオンの微分は
\dot{ \vec{q} } =\frac{1}{2} \vec{q} \times \vec{\omega_{v}}
$\times $はクォータニオンの積です．
表示の方法
matplotを使用．ラケットの形状にするのはあきらめた
import numpy as np
import matplotlib.pyplot as plt
fig = plt.figure(figsize=(8,6))
ax = fig.add_subplot(111, projection='3d')
# x, y, z成分のデータの作成
# x = np.array([0,1,0,0,0,0,0,1])
# y = np.array([0,0,0,1,0,0,1,0])
# z = np.array([0,0,1,0,0,1,0,0])
# x = np.array([0,1,0])
# y = np.array([0,0,0])
# z = np.array([0,0,1])
x = np.array([0,1,0,0,0,0])
y = np.array([0,0,0,1,0,0])
z = np.array([0,0,1,0,0,1])
ax.plot(x, y, z, color='blue')
ax.scatter(x, y, z, color='blue')
min_range = -1
max_range = 1
ax.set_xlim(min_range, max_range)
ax.set_ylim(min_range, max_range)
ax.set_zlim(min_range, max_range)
これで下図のような，3次元グラフが表示できる．
さらに，こちらのサイトを参考に，gifにした．
シミュレーションコード
import Quatanion as Q
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
def cross(vec):
line,width = vec.shape
if width != 1:
print ("cannot convert")
return
if line !=3:
print ("toobig of small")
return
A=np.array([[0,-1*vec[2][0],vec[1][0]],
[vec[2][0],0,-1*vec[0][0]],
[-1*vec[1][0],vec[0][0],0] ])
return A
# 初期値
w_x = 0.01
w_y = 0
w_z = 10
q = Q.makeQuat(np.array([[1],[0],[0]]),0) #初期姿勢は絶対座標系Oと一致
vec_w =np.asarray([w_x,w_y,w_z])
vec_w =vec_w .reshape([3,1])
# パラメータ指定
Iaa=np.array([[1,0,0],
[0,4,0],
[0,0,2]])
inv_Iaa = np.linalg.inv(Iaa)
DT=0.001
T=10
N=int(T/DT)
# N=1
# 保存用変数
save_q = q
save_w = vec_w
for i in range(N):
#角加速度ベクトルの計算
cross_w = cross(vec_w)
vec_dw = -1*np.dot(inv_Iaa,np.dot(cross_w,np.dot(Iaa,vec_w)))
#角速度ベクトルの更新
vec_w = vec_w + vec_dw * DT
#norm_w = np.linalg.norm(vec_w)
#クォータニオンの時間微分の計算
vec_qw = np.array([[0],[vec_w[0][0]],[vec_w[1][0]],[vec_w[2][0]]])
vec_dq = 0.5* Q.crossQuat(q,vec_qw)
#クォータニオンの更新
q = q + vec_dq * DT
q = Q.normalize(q)
#保存
save_q = np.append(save_q,q,axis = 1)
save_w = np.append(save_w,vec_w,axis = 1)
# 表示
fig = plt.figure(figsize=(10,10))
ax = fig.add_subplot(111, projection='3d')
# x, y, z成分のデータの作成
# x = np.array([0,1,0,0,0,0,0,1])
# y = np.array([0,0,0,1,0,0,1,0])
# z = np.array([0,0,1,0,0,1,0,0])
# x = np.array([0,1,0,0,0,0])
# y = np.array([0,0,0,1,0,0])
# z = np.array([0,0,1,0,0,1])
x = np.array([0,0,1,0,0,0])
y = np.array([0,0,0,0,1,0.5])
z = np.array([0.5,0,0,1,0,0])
# ax.plot(x, y, z, color='blue')
# ax.scatter(x, y, z, color='blue')
min_range = -1
max_range = 1
ax.set_xlim(min_range, max_range)
ax.set_ylim(min_range, max_range)
ax.set_zlim(min_range, max_range)
# plt.show()
ims = []
for i in range(300):
"""
save_qに従い，座標変換行列CAOを作り，らせんの点を移動しながら，プロットする
"""
CAO = Q.makeRotateMatrixFram(save_q.T[i*10].reshape(4,1)).T
rotate_x = []
rotate_y = []
rotate_z = []
for j in range(6):
vec_xyz = np.array([x[j],y[j],z[j]])
vec_xyz =vec_xyz.reshape([3,1])
vec_xyz = np.dot(CAO,vec_xyz)
rotate_x = np.append(rotate_x,vec_xyz[0][0])
rotate_y = np.append(rotate_y,vec_xyz[1][0])
rotate_z = np.append(rotate_z,vec_xyz[2][0])
im = ax.plot(rotate_x, rotate_y, rotate_z, color='blue')
ims.append(im)
ani = animation.ArtistAnimation(fig, ims, interval=100)
plt.show()
ani.save("wx2_long.gif", writer='pillow')
クォータニオン演算用サブルーチン
Quatanion.py
import numpy as np
import copy
def CrossOpe(vec):
line,width = vec.shape
if width != 1:
print ("cannot convert")
return
if line !=3:
print ("toobig of small")
return
A=np.array([[0,-1*vec[2][0],vec[1][0]],
[vec[2][0],0,-1*vec[0][0]],
[-1*vec[1][0],vec[0][0],0] ])
return A
def normalize(v):
l2 = np.linalg.norm(v)
if l2==0:
hoge = 1/np.sqrt(3)
return np.array([[hoge],[hoge],[hoge]])
return v/l2
def makeQuat(hoge,theta):
vec = copy.deepcopy(hoge)
line,width = vec.shape
if width != 1:
print ("cannot convert")
return
if line !=3:
print( "toobig of small")
return
vec = normalize(vec)
A=np.array([[np.cos(theta/2)],
[vec[0][0]*np.sin(theta/2)],
[vec[1][0]*np.sin(theta/2)],
[vec[2][0]*np.sin(theta/2)]])
return A
def crossQuat(hoge1,hoge2):
q1 =copy.deepcopy(hoge1)
q2 =copy.deepcopy(hoge2)
line,width = q1.shape
if width != 1:
print ("cannot convert")
return
if line !=4:
print ("toobig of small")
return
line,width = q2.shape
if width != 1:
print ("cannot convert")
return
if line !=4:
print( "toobig of small")
return
q1 = q1.reshape(1,4)
q2 = q2.reshape(1,4)
q1 = q1[0]
q2 = q2[0]
A=np.array([[q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3] ],
[q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2] ],
[q1[0]*q2[2]-q1[1]*q2[3]+q1[2]*q2[0]+q1[3]*q2[1] ],
[q1[0]*q2[3]+q1[1]*q2[2]-q1[2]*q2[1]+q1[3]*q2[0] ]])
return A
def makeRotateMatrixVec(hoge): #これはベクトルを回転させる 回転行列 こっちは本筋ではない
q=copy.deepcopy(hoge)
line,width = q.shape
if width != 1:
print( "cannot convert")
return
if line !=4:
print ("toobig of small")
return
q = q.reshape(1,4)
q=q[0]
q0=q[0]
q1=q[1]
q2=q[2]
q3=q[3]
C=np.array([[q1**2-q2**2-q3**2+q0**2,2*(q1*q2-q3*q0) ,2*(q3*q1+q2*q0)],
[2*(q1*q2+q3*q0),q2**2-q3**2-q1**2+q0**2,2*(q2*q3-q1*q0)],
[2*(q3*q1-q2*q0),2*(q2*q3+q1*q0),q3**2-q1**2-q2**2+q0**2] ])
return C.T
def makeRotateMatrixFram(hoge): #これはフレームを回転させる 回転行列 追記：フレームが回転していると考える方式
'''
qが Oー＞A への座標変換とすると
Coa:A->Oへの座標変換行列を作る のが、これです
'''
q=copy.deepcopy(hoge)
line,width = q.shape
if width != 1:
print ("cannot convert")
return
if line !=4:
print ("toobig of small")
return
q = q.reshape(1,4)
q=q[0]
q0=q[0]
q1=q[1]
q2=q[2]
q3=q[3]
C=np.array([[q1**2-q2**2-q3**2+q0**2,2*(q1*q2-q3*q0) ,2*(q3*q1+q2*q0)],
[2*(q1*q2+q3*q0),q2**2-q3**2-q1**2+q0**2,2*(q2*q3-q1*q0)],
[2*(q3*q1-q2*q0),2*(q2*q3+q1*q0),q3**2-q1**2-q2**2+q0**2] ])
return C
def Conj(q):
hoge =copy.deepcopy(q)
hoge[1][0]=-1*hoge[1][0]
hoge[2][0]=-1*hoge[2][0]
hoge[3][0]=-1*hoge[3][0]
return hoge
'''
q = makeQuat(np.array([[1],[0],[0]]),np.pi/2)
C = makeRotateMatrixFram(q)
vec1=np.array([1,9,3])
vec1 =vec1.reshape(3,1) #二次元にしながら、転置もしている
print q
print C
vec2 = np.dot(C,vec1)
print vec1
print vec2
q2 = makeQuat(np.array([[0],[1],[0]]),np.pi/2)
C2 = makeRotateMatrixFram(q2)
q3 = makeQuat(np.array([[0],[0],[1]]),np.pi/2)
C3 = makeRotateMatrixFram(q3)
q4=crossQuat(crossQuat(q,q2),q3)
C4= np.dot(np.dot(C,C2),C3)
print q4
print C4
'''