コマの運動方程式とシミュレーション：歳差・章動

概要

運動シミュレーションをpythonで実行しましたので、投稿します
コマの姿勢はクォータニオンでやりました。
オイラー角（z-y-z:重力y方向）でもやってみましたので一応最後にあります。
3次元倒立振子の制御を行う前にシミュレーションをしたく行っています。
3次元の制御で積分を導入するのがすごいむずいです

歳差運動とは

歳差運動とは、コマが回転するときごますりをするような運動です

https://ja.wikipedia.org/wiki/%E6%AD%B3%E5%B7%AE!　
より

章動運動とは

章動運動は剛体回転体で有るから発生する現象です。歳差運動に首振りに追加して微小な回転が追加されます。
コマの重心を上から見てプロットすると次のようになります。

大きな円は歳差の回転で、微小にぎざぎざしているのが章動です。
発生原因としては慣性マトリクス$J$の主回転方向以外の要素の存在に由来します。
以降重力は$-y$方向に働くとして考えます。コマの角速度ベクトルの主回転方向は$y$方向になります。
この時、$Jxx,Jzz$の成分が0でないと発生する運動です。歳差運動を行う以上、$\omega_x,\omega_y$の成分が発生します。結果的に歳差運動をすると、$x,z$方向の回転エネルギーが発生します。
この際にいろいろあるのでしょう（ぶん投げwww）、まぁそんなのもあるんだなぁぐらいで
テニスラケットの原理とかも不安定になるのはわかるが、実際どういう風に運動するかはシミュレーションでわかる感じだと思うので

シミュレーション結果

緑の点がコマの重心位置で、赤のベクトルが角速度ベクトルです。gifアニメなので別タブで開くと動きます
ベクトルの方を消せないので諦めました。
まぁ、歳差運動は確認できると思います。
運動方程式は

J\dot{\Omega_{oaa} }+\tilde{\Omega_{oaa}}J\Omega_{oaa} = C_{oa}^T \tilde{mgD_z} C_{oaa}R_{oaa} 

です。3次元剛体の運動方程式はややこしく、定義からしっかりしないと、説明がわかりにくくなりがちなので、本で勉強した方が良いと思います。
マルチボディダイナミクスの基礎　：　田島　洋
がおすすめです。分厚いですが、章ごとに分かれているのと前半は数学なのでさくっと読めると思います。

シミュレーションコード

import numpy as np		#	Numpyライブラリ
import copy
import math

import matplotlib
# matplotlib.use('Agg') # -----(1)
import matplotlib.pyplot as plt

import matplotlib.animation as animation
from matplotlib.animation import PillowWriter
from mpl_toolkits.mplot3d import axes3d

import Quatanion as Q

def TILDE(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 diff_f(q,Omoa,Ma,pJoa,invJoa):    
    Coa = Q.makeRotateMatrixFram(q)
    
    OmoaQ = np.insert(Omoa,0,0,axis = 0)
    dq = 0.5*Q.crossQuat(q,OmoaQ)    
    
    Roa_y = 0.03
    Roaa=np.asarray([0,Roa_y,0])
    Roaa=Roaa.reshape([3,1])
    
    TILDERoaa = TILDE(Roaa)
    
    Dy=np.asarray([0,1,0])
    Dy=Dy.reshape([3,1])
    
    g=9.81
    Foa = Dy*Ma*g    
    TILDEOmoa = TILDE(Omoa)
    hoge =-1*np.dot(np.dot(TILDERoaa,Coa.T),Foa) - np.dot(  np.dot ( TILDEOmoa,pJoa),Omoa)
    DOmoa = np.dot(invJoa,hoge)
    
    return dq,DOmoa

vec1 =np.asarray([0,0,1])
vec1 =vec1.reshape([3,1])

q=Q.makeQuat(vec1,np.pi/6)

omega1 =0
omega2 = 10
omega3 = 0

Omoa =np.asarray([omega1,omega2,omega3])
Omoa =Omoa.reshape([3,1])


Ma = 100
Joa=np.array([[1,0,0],
            [0,4,0],
            [0,0,1]])

Roa_y = 0.03
Roaa=np.asarray([0,Roa_y,0])  #Oから AまでのベクトルをA座表で表示したもの
Roaa=Roaa.reshape([3,1])


g=9.81

TILDERoa = TILDE(Roaa)
pJoa = Joa + np.dot(TILDERoa.T*Ma,TILDERoa)
invJoa = np.linalg.inv(pJoa)

DT=0.001
T=10
N=int(T/DT)
# N=1

saveX = q.T[0]
Roa=np.asarray([0,0.03,0])
Roa=Roa.reshape([3,1])

Coa = Q.makeRotateMatrixFram(q)
saveRoa = np.dot(Coa, Roa).T[0]
saveOmoao = np.dot(Coa,Omoa).T[0]
saveEnergy =np.array([0])

for i in range(N):

    K1T,K1O = diff_f(q,Omoa,Ma,pJoa,invJoa)
    K2T,K2O = diff_f(q+DT/2*K1T,Omoa + DT/2 *K1O,Ma,pJoa,invJoa)
    K3T,K3O = diff_f(q+DT/2*K2T,Omoa + DT/2 *K2O,Ma,pJoa,invJoa)
    K4T,K4O = diff_f(q+DT*K3T,Omoa + DT *K3O,Ma,pJoa,invJoa)
    
    q = q + DT/6*(K1T+2*K2T+2*K3T+K4T)
    q = Q.normalize(q)
    Omoa = Omoa +DT/6*(K1O+2*K2O+2*K3O+K4O)
    
    saveX = np.append(saveX,q.T[0])
    
    Coa = Q.makeRotateMatrixFram(q)
    
    Roa = np.dot(Coa,Roaa)
    saveRoa = np.append(saveRoa,Roa.T[0])
    
    Omoao = np.dot(Coa,Omoa)
    saveOmoao = np.append(saveOmoao,Omoao.T[0])
    
    
    
    T=0.5*np.dot(np.dot(Omoa.T,pJoa),Omoa)
    U=Roa[1][0] *Ma*g
    saveEnergy = np.append(saveEnergy,[T+U])

saveX = saveX.reshape(N+1,4)
saveX=saveX.T
saveRoa = saveRoa.reshape(N+1,3)
saveRoa=saveRoa.T
saveOmoao = saveOmoao.reshape(N+1,3)
saveOmoao = saveOmoao.T


t=[float(i)*DT for i in range(N+1)]
t= np.asarray(t)

# 点履歴

plt.figure(figsize=(28, 20))  # (width, height)
plt.rcParams["font.size"] = 25
plt.plot(t,saveX[0],label ="w1")
plt.plot(t,saveX[1],label ="w2")
plt.plot(t,saveX[2],label ="w3")
plt.plot(t,saveX[3],label ="w4")
plt.legend(fontsize = 25)

plt.savefig('TapTheta_koma20_2.png')


# 点履歴

plt.figure(figsize=(28, 20))  # (width, height)
plt.rcParams["font.size"] = 25
plt.plot(saveRoa[0],saveRoa[2],label ="x-z")
plt.legend(fontsize = 25)

plt.savefig('x-z.png')

# 点履歴

plt.figure(figsize=(28, 20))  # (width, height)
plt.rcParams["font.size"] = 25
plt.plot(saveRoa[1],saveRoa[2],label ="y-z")
plt.legend(fontsize = 25)

plt.savefig('y-z.png')

# 点履歴

plt.figure(figsize=(28, 20))  # (width, height)
plt.rcParams["font.size"] = 25
plt.plot(saveRoa[0],saveRoa[1],label ="x-y")
plt.legend(fontsize = 25)

plt.savefig('x-y.png')

# 点履歴

plt.figure(figsize=(28, 20))  # (width, height)
plt.rcParams["font.size"] = 25
plt.plot(t,saveRoa[1],label ="t-y")
plt.legend(fontsize = 25)

plt.savefig('t-y.png')

# 点履歴

plt.figure(figsize=(28, 20))  # (width, height)
plt.rcParams["font.size"] = 25
plt.plot(t,saveEnergy,label ="Energy")
plt.legend(fontsize = 25)

plt.savefig('Energy.png')


# 3Dグラフ:アニメーション
plt.rcParams["font.size"] = 25
plt.style.use('ggplot')
plt.rcParams["axes.facecolor"] = 'white'
fig = plt.figure(figsize=(28, 20))
plt.rcParams["font.size"] = 25
ax = fig.gca(projection='3d')

print "start animation"
ims = []
maxD = 0.04
ax.set_xlim([-maxD, maxD])
ax.set_ylim([-maxD, maxD])
ax.set_zlim([-maxD,maxD] )

Step = 100   #飛ばすフレーム数

for i in range(int(N/Step)):
        # Make the grid
        x=[saveRoa[2][i*Step]]
        y=[saveRoa[0][i*Step]]
        z=[saveRoa[1][i*Step]]
        u=[saveOmoao[2][i*Step]*0.001]
        v=[saveOmoao[0][i*Step]*0.001]
        w=[saveOmoao[1][i*Step]*0.001]
        ax.set(xlabel='z',ylabel='x',zlabel='y')
        ax.quiver(x, y, z, u, v, w)
        im = ax.plot(x,y,z,'go')
        ims.append(im)                  # グラフを配列 ims に追加

# 10枚のプロットを 100ms ごとに表示
ani = animation.ArtistAnimation(fig, ims, interval=DT*Step)
ani.save("AnimaTap.gif", writer='pillow')

Quatanion

# -*- coding: utf-8 -*-
"""
Created on Sun Mar 29 13:16:22 2020

@author: kisim
"""
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

'''sample
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
'''