3次元剛体の運動方程式の立て方4_テニスラケットの原理

#概要
[wikipedia](https://ja.wikipedia.org/wiki/%E3%83%86%E3%83%8B%E3%82%B9%E3%83%A9%E3%82%B1%E3%83%83%E3%83%88%E3%81%AE%E5%AE%9A%E7%90%86)
テニスラケットの原理とは
> 剛体の第1，第3の慣性主軸のまわりの回転は安定しているが、第2の慣性主軸（中間軸）のまわりの回転は不安定である。』

一般に，回転する物体が外部トルクを与えなければ，回転軸は動かず，ずっと同じ角速度で回転すると考えられてきました．
![wx2.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/29c5c026-8b38-2f68-f9b7-55431719c46a.gif)
このgifは上方向（z方向）にずっと回転しつづけていますが，右方向（x方向）の角速度をわずかに初期値として与えると
![wx2.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/38f13def-949e-b709-f570-7dea7c64e137.gif)
このgifのように，z軸方向からずれて，z-軸方向（x-y方向ではありません）に変わっています．

[実際に宇宙で見られた映像](https://www.youtube.com/watch?v=1n-HMSCDYtM&feature=emb_logo)も面白いです．
[ロングバージョン](https://www.youtube.com/watch?v=cpp5DqxRjVI)も公開しました．

#シミュレーション方法

```math
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\\
```
に従います．

```math
F_A^O = 0
```
として，並進方向の動きはないとします．よって，角加速度・角速度・姿勢（クォータニオン）の3つ（それぞれ3方向あるので合計9つ）を追えばよいことになります．

外部トルクもないとします．

```math
N_A^A = 0
```

###流れ
1. 初期姿勢（クォータニオン）・角速度を与える
2. 角加速度ベクトルを計算
3. 角速度ベクトルを更新（オイラー法）
4. クォータニオンを更新（オイラー法）
5. 2に戻り，繰り返す

以上の流れです．

###クォータニオンと座標変換行列の関係
座標系Aの座標系Oに対するクォータニオン$q$を次のように置くと

```math
q=
\left(
    \begin{array}{ccc}
   \epsilon_0\\
\epsilon_1\\
\epsilon_2\\
\epsilon_3\\
    \end{array}
  \right)
```
座標変換行列は

```math
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}}$とします

```math

\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)
```
とすると，クォータニオンの微分は

```math
\dot{ \vec{q} }  =\frac{1}{2} \vec{q} \times \vec{\omega_{v}}

```

$\times $はクォータニオンの積です．


#表示の方法
matplotを使用．ラケットの形状にするのはあきらめた


```python
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次元グラフが表示できる．

さらに，[こちらのサイト](https://shizenkarasuzon.hatenablog.com/entry/2019/01/27/170550)を参考に，gifにした．


#シミュレーションコード

```python
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')
```

クォータニオン演算用サブルーチン

```python: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
'''
```