二重振り子のシミュレーション

#二重振り子とは
https://www.youtube.com/watch?v=25feOUNQB2Y
カオス現象として有名な実験です。
カオス現象とは初期状態がその後の状態に大きく影響を与える現象です。
バタフライエフェクトという、蝶の羽が気流によって台風が発生するという話も同じカオス現象の説明で出てきます。（正しいくないかもしれません）

[こちら](https://qiita.com/hiRina/items/519d5168c2c2fdf012cd)で作ったオイラー・ラグランジュ法を用いて運動方程式の導出・シミュレーションをします。


#変数定義
![define.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/2fb7c89b-4528-19f2-4e51-4298a405271a.jpeg)

質点系・二次元で問題設定します。

m1の速度、m2の速度は

```math
\vec{R_{m_1}} =\left(
    \begin{array}{ccc}
      r_1 sin(\theta_1) \\
      -r_1 cos(\theta_1)
    \end{array}
  \right) ,
\vec{R_{m_2}}=\vec{R_{m_1}} +\left(
    \begin{array}{ccc}
      r_2 sin(\theta_1) \\
      -r_2 cos(\theta_1)
    \end{array}
  \right) \\



\vec{V_{m_1}} =\left(
    \begin{array}{ccc}
      r_1 cos(\theta_1) \\
      r_1 sin(\theta_1)
    \end{array}
  \right) \dot{\theta_1},
\vec{V_{m_2}}=\vec{V_{m_1}} +\left(
    \begin{array}{ccc}
      r_2 cos(\theta_1) \\
      r_2 sin(\theta_1)
    \end{array}
  \right) \dot{\theta_2}\\


```

運動エネルギーTは

```math
T=\frac{1}{2} m_1 |\vec{V_{m_1}}|^2+\frac{1}{2} m_2 |\vec{V_{m_2}}|^2
```

位置エネルギUは

```math
U=m_1gr_1(1-cos(\theta_1)) + m_2g(r_1(1-cos(\theta_1)) + r_2(1-cos(\theta_2))
```

ラグランジアンを計算すると

```math
L= m_1*r_1^2/2  + m2*r_2^2 /2 \dot{ x_1}^{2.0}          +  
  m_2*r_2^2/2      \dot{ x_2}^{2.0}     +  
  m_2*r_1*r_2 \dot{ x_1}^{1.0} sin( x_1)^{1.0}      \dot{ x_2}^{1.0} sin( x_2)^{1.0}     +  
  m_2*r_1*r_2 \dot{ x_1}^{1.0} cos( x_1)^{1.0}      \dot{ x_2}^{1.0} cos( x_2)^{1.0}     +  
  -1*(g*(m_1*r_1+m_2*(r_1+r_2)))          +  
  -1*(-1*g*(m_1*r_1+m_2*r_1)) cos( x_1)^{1.0}          +  
  -1*(-1*g*r_2*m_2)      cos( x_2)^{1.0}  
\\
\theta_1 = x_1\\
\theta_2 = x_2 とした

```

運動方程式を導出すると

```math
(m_2*r_1*r_2*1.0)  + -1*( ( (m_2*r_1*r_2*1.0) *1.0) ) \dot{ x_1}^{1.0} cos( x_1)^{1.0}      \dot{ x_2}^{1.0} sin( x_2)^{1.0}     +  
   (m_2*r_1*r_2*(-1)*1.0)  + -1*( ( (m_2*r_1*r_2*1.0) *(-1)*1.0) ) \dot{ x_1}^{1.0} sin( x_1)^{1.0}      \dot{ x_2}^{1.0} cos( x_2)^{1.0}     +  
   (-1*(-1*g*(m_1*r_1+m_2*r_1))*(-1)*1.0)  sin( x_1)^{1.0}          +  
  -1*( ( (m_1*r_1^2/2  + m2*r_2^2 /2*2.0) *1.0) ) \ddot { x_1}^{1.0}          +  
  -1*( ( (m_2*r_1*r_2*1.0) *1.0) ) sin( x_1)^{1.0}      \dot{ x_2}^{2.0} cos( x_2)^{1.0}     +  
  -1*( ( (m_2*r_1*r_2*1.0) *(-1)*1.0) ) cos( x_1)^{1.0}      \dot{ x_2}^{2.0} sin( x_2)^{1.0}     +  
  -1*( ( (m_2*r_1*r_2*1.0) *1.0) ) sin( x_1)^{1.0}      \ddot { x_2}^{1.0} sin( x_2)^{1.0}     +  
  -1*( ( (m_2*r_1*r_2*1.0) *1.0) ) cos( x_1)^{1.0}      \ddot { x_2}^{1.0} cos( x_2)^{1.0}    = 0  \\

  (m_2*r_1*r_2*1.0)  + -1*( ( (m_2*r_1*r_2*1.0) *1.0) ) \dot{ x_1}^{1.0} sin( x_1)^{1.0}      \dot{ x_2}^{1.0} cos( x_2)^{1.0}     +  
   (m_2*r_1*r_2*(-1)*1.0)  + -1*( ( (m_2*r_1*r_2*1.0) *(-1)*1.0) ) \dot{ x_1}^{1.0} cos( x_1)^{1.0}      \dot{ x_2}^{1.0} sin( x_2)^{1.0}     +  
   (-1*(-1*g*r_2*m_2)*(-1)*1.0)       sin( x_2)^{1.0}     +  
  -1*( ( (m_2*r_1*r_2*1.0) *1.0) ) \dot{ x_1}^{2.0} cos( x_1)^{1.0}      sin( x_2)^{1.0}     +  
  -1*( ( (m_2*r_1*r_2*1.0) *(-1)*1.0) ) \dot{ x_1}^{2.0} sin( x_1)^{1.0}      cos( x_2)^{1.0}     +  
  -1*( ( (m_2*r_1*r_2*1.0) *1.0) ) \ddot { x_1}^{1.0} sin( x_1)^{1.0}      sin( x_2)^{1.0}     +  
  -1*( ( (m_2*r_1*r_2*1.0) *1.0) ) \ddot { x_1}^{1.0} cos( x_1)^{1.0}      cos( x_2)^{1.0}     +  
  -1*( ( (m_2*r_2^2/2*2.0) *1.0) )      \ddot { x_2}^{1.0}  =0
```
使った、コードは見にくいので最後に添付します。

#シミュレーション（オイラー法）
得られた式を用いて、オイラー法でシミュレーションすると
設定

```math
m1=1.0\\
m2=1.0\\
r1=1.0\\
r2=1.0\\
g=9.81\\
```

初期値：

```math
\theta_1 = 180deg\\
\theta_2 = 180deg\\
\dot{\theta_1 }= 2rad/s\\
\dot{\theta_2} = 2rad/s\\
```
![output2.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/2dd61a10-9608-852e-e044-3aef419a1592.gif)


初期値：

```math
\theta_1 = 180deg\\
\theta_2 = 180deg\\
\dot{\theta_1 }= 0.1rad/s\\
\dot{\theta_2} = 0.1rad/s
```
![output2.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/52bb1826-8aac-62c2-5ba6-70b067ea4dba.gif)
こっちの方がカオスですね（中2並感）

```math
\theta_1 = 45deg\\
\theta_2 = 45deg\\
\dot{\theta_1 }= 0.1rad/s\\
\dot{\theta_2} = 0.1rad/s
```
![output2.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/8c868015-cbc8-9bbb-f579-3340e2762fc4.gif)
速度がないとカオス的にはならないですね

#運動方程式導出コード
ここから先は、作ったプログラムになります。
コピペで動くはずです。
anaconda python2で実行しています。
gifアニメ化するためにこちら
https://qiita.com/yubais/items/c95ba9ff1b23dd33fde2
を参考にしました。

解析用クラス

```python:Formula2
import numpy as np		#	Numpyライブラリ
import copy
import math
'''
数式処理プログラム
列　：　多項式
行　：　θ　θ・　θ・・　sinθ　cosθ　係数　 
の6個　×　変数数


文字式に対応させる
ただし、

'''
class Formula2 :
    def __init__(self,L,num,coeff):
        self.L = L
        self.n = num                      #変数の種類数
        self.coeff = coeff
        #print self.coeff.dtype
    
    def __add__(self,other):
        
        self.L = np.append(self.L,other.L,axis =0)
        #print self.coeff.dtype
        #print other.coeff.dtype
        
        expr,vari = self.L.shape
        self.coeff = np.append(self.coeff,other.coeff,axis =0)
        self.together()
        #self.erase()

    def __sub__(self,other):
        expr,vari = other.L.shape
        hoge = copy.deepcopy(other.L)
        for i in range(expr):
            hoge[i][5] = hoge[i][5] * -1
        self.L = np.append(self.L,hoge,axis =0)
        
        for i in range(expr):
            other.coeff[i] = "-1*(" + other.coeff[i] +")"
        self.coeff = np.append(self.coeff,other.coeff,axis =0)
        
        self.together()
        #self.erase()
            
        
    def together(self):               #同じ項の係数をまとめる
        expr,vari = self.L.shape
        if expr >=2:
            for i in range(expr-1):
                for j in range(i+1,expr):                    
                    m=0
                    while self.L[i][0+m*6:5+m*6].tolist() == self.L[j][0+m*6:5+m*6].tolist():
                        m += 1
                        if m == self.n:
                            break
                    if m== self.n:  #全部一致  係数の足し算
                        self.L[i][5] += self.L[j][5]
                        self.coeff[i] = self.coeff[i] +" + "+ self.coeff[j]
                        for k in range(vari):  #　j　行削除
                            self.L[j][k] = 0
                        self.coeff[j] = "0"
                         
    def erase(self):    #係数が0の項を消す
        expr,vari = self.L.shape
        for i in list(reversed(range(expr))):
            if self.coeff[i] == "0":
                self.L = np.delete(self.L,i,axis = 0)
                self.coeff =np.delete(self.coeff,i,axis = 0)
   
                
    def partial(self,moji,kind):  #kindの種類のもので項を偏微分する
        expr,vari = self.L.shape
        if kind == 0:   
            '''
            sin
            cos
            
            +moji*6
            '''
            #print self.L
            for i in range(expr):
                if self.L[i][3+moji*6] !=0:  #sin
                    hoge = copy.deepcopy(self.L[i])
                    hogestr = copy.deepcopy(self.coeff[i])
                    hoge[5] = hoge[5]*hoge[3+moji*6]
                    #係数に hoge[3+moji*6]を　かける
                    hogestr = " ("+hogestr+"*"+str(hoge[3+moji*6])+") " 
                    hoge[3+moji*6] = hoge[3+moji*6]-1
                    hoge[4+moji*6] = hoge[4+moji*6]+1
                    self.L = np.append(self.L,hoge[np.newaxis,:],axis =0)
                    self.coeff = np.append(self.coeff,[hogestr],axis =0)
                    
                    #print self.L
            for i in range(expr):
                if self.L[i][4+moji*6] !=0:  #cos
                    hoge = copy.deepcopy(self.L[i])
                    hogestr = copy.deepcopy(self.coeff[i])
                    hoge[5] = hoge[5]*hoge[4+moji*6]*-1
                    #係数に hoge[3+moji*6]を　かける
                    hogestr = " ("+hogestr+"*(-1)*"+str(hoge[4+moji*6])+") " 
                    hoge[4+moji*6]= hoge[4+moji*6]-1
                    hoge[3+moji*6] = hoge[3+moji*6]+1
                    self.L = np.append(self.L,hoge[np.newaxis,:],axis =0)
                    self.coeff = np.append(self.coeff,[hogestr],axis =0)
                    #print self.L
            '''
            そのままのもの
            次数を一つ下げる、0なら何もしない
            元の次数を係数に掛ける
            sin,cos処理部は後ろにあって処理範囲にないため大丈夫
            '''
            for i in range(expr):
                if self.L[i][kind] !=0:
                    #print kind,self.L[i][5]
                    self.L[i][5] = self.L[i][5] * self.L[i][kind+moji*6]                    
                    self.coeff[i] = " ("+self.coeff[i]+"*"+str(self.L[i][kind+moji*6])+") " 
                    #print self.L[i][5]
                    self.L[i][kind+moji*6] = self.L[i][kind+moji*6] -1
                    
                else:
                    self.L[i][5] = 0   #含まないなら　0
                    self.coeff[i] = "0"
            
        if kind == 1:   
            '''
            そのままのもの
            次数を一つ下げる、0なら何もしない
            元の次数を係数に掛ける
            '''
            for i in range(expr):
                if self.L[i][kind+moji*6] !=0:
                    self.L[i][5] = self.L[i][5] * self.L[i][kind+moji*6]
                    self.coeff[i] = " ("+self.coeff[i]+"*"+str(self.L[i][kind+moji*6])+") " 
                    self.L[i][kind+moji*6] = self.L[i][kind+moji*6] -1
                    
                else:
                    self.L[i][5] = 0   #含まないなら　0
                    self.coeff[i] = "0"
                    
        
        self.together()
        self.erase()
        
        
    def diff(self):  #時間微分
        '''
        4つのパターンで偏微分後
        θ・の次数を1上げる        
        '''
        L1=copy.deepcopy(self.L)
        L1_coeff = copy.deepcopy(self.coeff)
        
        for i in range(self.n):
            self.L =copy.deepcopy( L1)   #L1:微分前　　L2:結果保存用
            self.coeff = copy.deepcopy(L1_coeff)
            self.partial(i,0)
            expr,vari = self.L.shape
            #print expr
            #print self.L
            for j in range(expr):
                self.L[j][1+i*6] = self.L[j][1+i*6] + 1
            if i == 0:
                L2 = copy.deepcopy( self.L)
                L2_coeff = copy.deepcopy(self.coeff)
            else:
                L2 = np.append(L2,self.L,axis =0)
                L2_coeff = np.append(L2_coeff,self.coeff,axis =0)
            
            self.L =copy.deepcopy( L1)    
            self.coeff = copy.deepcopy(L1_coeff) 
            self.partial(i,1)
            expr,vari = self.L.shape
            for j in range(expr):
                self.L[j][2+i*6] += 1
            L2 = np.append(L2,self.L,axis =0)
            L2_coeff = np.append(L2_coeff,self.coeff,axis =0)
                    
        self.L = copy.deepcopy( L2)
        self.coeff = copy.deepcopy(L2_coeff)
        
        self.together()
        self.erase()
    
    def disp2(self):  #多項式表示
        print "-------------------Formula--------------"
        expr,vari = self.L.shape
        for j in range(expr):
            hoge =""
            for i in range(self.n):
                hoge += "["+str(i+1)+"  "
                if i == 0:
                    hoge += self.coeff[j]+"    "
                if self.L[j][0+i*6]!= 0:   
                    hoge += " θ^"
                    hoge += str(self.L[j][0+i*6])
                    
                if self.L[j][1+i*6]!= 0:
                    hoge += " θ・^"
                    hoge += str(self.L[j][1+i*6])
                    
                if self.L[j][2+i*6]!= 0:
                    hoge += " θ・・^"
                    hoge += str(self.L[j][2+i*6])
                    
                if self.L[j][3+i*6]!= 0:
                    hoge += " sin^"
                    hoge += str(self.L[j][3+i*6])
                    
                if self.L[j][4+i*6]!= 0:
                    hoge += " cos^"
                    hoge += str(self.L[j][4+i*6])
                     
                hoge += " ]  "
                
            hoge += "  +  "
            print hoge
            #print self.coeff
            
    def latex(self):
        variable = " x"
        k=1
        print "-------------------Formula_latex---------"
        expr,vari = self.L.shape
        for j in range(expr):
            hoge =""
            for i in range(self.n):
                hoge += "  "
                
                if i == 0:
                    hoge += self.coeff[j]
                if self.L[j][0+i*6]!= 0:   
                    hoge += variable+"_"+str(i+k)+"^{"
                    hoge += str(self.L[j][0+i*6])+"}"
                    
                if self.L[j][1+i*6]!= 0:
                    hoge += " \dot{"+variable+"_"+str(i+k)+"}^{"
                    hoge += str(self.L[j][1+i*6])+"}"
                    
                if self.L[j][2+i*6]!= 0:
                    hoge += " \ddot {"+variable+"_"+str(i+k)+"}^{"
                    hoge += str(self.L[j][2+i*6])+"}"
                    
                if self.L[j][3+i*6]!= 0:
                    hoge += " sin("+variable+"_"+str(i+k)+")^{"
                    hoge += str(self.L[j][3+i*6])+"}"
                    
                if self.L[j][4+i*6]!= 0:
                    hoge += " cos("+variable+"_"+str(i+k)+")^{"
                    hoge += str(self.L[j][4+i*6])+"}"
                     
                hoge += "   "
                
            hoge += "  +  "
            print hoge
            
    def separete(self):              #2変数と考えて、　1　２　ｂ　の3つに分割する
        a1=np.asarray([[]])
        a2=np.asarray([[]])
        coeff1 = np.asarray([],dtype = object)
        coeff2 = np.asarray([],dtype = object)
        expr,vari = self.L.shape
        for j in range(expr)[::-1]:
            if self.L[j][2] == 1:
                if len(a1[0]) == 0:
                    a1=(self.L[j]) [np.newaxis,:]
                    coeff1=np.asarray([self.coeff[j]],dtype = object)
                else:
                    a1 = np.append(a1,(self.L[j]) [np.newaxis,:],axis =0)
                    coeff1=np.append(coeff1,[self.coeff[j]],axis = 0)
                self.L = np.delete(self.L,j,0)
                self.coeff=np.delete(self.coeff,j,0)
                continue
                
            if self.L[j][8] == 1:
                if len(a2[0]) == 0:
                    a2=(self.L[j]) [np.newaxis,:]
                    coeff2=np.asarray([self.coeff[j]],dtype = object)
                else:
                    a2 = np.append(a2,(self.L[j]) [np.newaxis,:],axis =0)
                    coeff2=np.append(coeff2,[self.coeff[j]],axis = 0)
                self.L = np.delete(self.L,j,0)
                self.coeff=np.delete(self.coeff,j,0)
                continue
        
        if len(a1[0]) == 0:
            a1 =np.asarray([ [0,0,0,0,0,0  ,0,0,0,0,0,0]])
            coeff1 = np.asarray(["0"],dtype = object)
        if len(a2[0]) == 0:
            a2 = np.asarray([[0,0,0,0,0,0  ,0,0,0,0,0,0]])
            coeff2 = np.asarray(["0"],dtype = object)
            
        L1 = Formula2(a1,2,coeff1)
        L2 = Formula2(a2,2,coeff2)
        return L1,L2
        
    def separete3(self):              #3変数と考えて、　1　2 3　ｂ　の 4 つに分割する
        a1=np.asarray([[]])
        a2=np.asarray([[]])
        a3=np.asarray([[]])
        coeff1 = np.asarray([],dtype = object)
        coeff2 = np.asarray([],dtype = object)
        coeff3 = np.asarray([],dtype = object)        
        expr,vari = self.L.shape
        for j in range(expr)[::-1]:
            
            if self.L[j][2] == 1:
                if len(a1[0]) == 0:
                    a1=(self.L[j]) [np.newaxis,:]
                    coeff1=np.asarray([self.coeff[j]],dtype = object)
                else:
                    a1 = np.append(a1,(self.L[j]) [np.newaxis,:],axis =0)
                    coeff1=np.append(coeff1,[self.coeff[j]],axis = 0)
                self.L = np.delete(self.L,j,0)
                self.coeff=np.delete(self.coeff,j,0)
                continue
                
            if self.L[j][8] == 1:
                if len(a2[0]) == 0:
                    a2=(self.L[j]) [np.newaxis,:]
                    coeff2=np.asarray([self.coeff[j]],dtype = object)
                else:
                    a2 = np.append(a2,(self.L[j]) [np.newaxis,:],axis =0)
                    coeff2=np.append(coeff2,[self.coeff[j]],axis = 0)
                self.L = np.delete(self.L,j,0)
                self.coeff=np.delete(self.coeff,j,0)
                continue
            
            if self.L[j][14] == 1:
                if len(a3[0]) == 0:
                    a3=(self.L[j]) [np.newaxis,:]
                    coeff3=np.asarray([self.coeff[j]],dtype = object)
                else:
                     a3 = np.append(a3,(self.L[j]) [np.newaxis,:],axis =0)
                     coeff3=np.append(coeff3,[self.coeff[j]],axis = 0)
                self.L = np.delete(self.L,j,0)
                self.coeff=np.delete(self.coeff,j,0)
                continue
        
        if len(a1[0]) == 0:
            a1 =np.asarray([ [0,0,0,0,0,0  ,0,0,0,0,0,0  ,0,0,0,0,0,0]])
            coeff1 = np.asarray(["0"],dtype = object)
        if len(a2[0]) == 0:
            a2 = np.asarray([[0,0,0,0,0,0  ,0,0,0,0,0,0  ,0,0,0,0,0,0]])
            coeff2 = np.asarray(["0"],dtype = object)
        if len(a3[0]) == 0:
            a3 = np.asarray([[0,0,0,0,0,0  ,0,0,0,0,0,0  ,0,0,0,0,0,0]])
            coeff3 = np.asarray(["0"],dtype = object)
            
        L1 = Formula2(a1,3,coeff1)
        L2 = Formula2(a2,3,coeff2)
        L3 = Formula2(a3,3,coeff3)
        return L1,L2,L3
            
        
    if __name__ == '__main__':
        print "This is not for main  TEST RESULT"
        
        T=np.asarray([[1,2,0,4,5,1  ,1,2,0,4,5,1]])
        coeff = np.asarray(["m      "], dtype=object)
        print coeff.dtype
        
        
        sai =Formula2(T,2,coeff)
        
        
        sai.disp2()
        sai.diff()
        sai.disp2()
        #sai.latex()
        L1,L2= sai.separete()
        L1.disp2()
        
        '''
        L1+L2
        
        L1.disp2()
        '''
```
実行コード

```python
import numpy as np		#	Numpyライブラリ
import copy
import math
#import matplotlib.pyplot as plt

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

import matplotlib.animation as animation
from matplotlib.animation import PillowWriter

import Formula2
            

#L= np.asarray([[7,2,3,4,5,6,     7,2,3,4,5,6]])
#L= np.asarray([[2,2,0,5,4,5],[2,2,0,5,4,5]])

'''
L1,L2　の二つしか　式がなく、変数も2つの場合で考えよう

式　≒　０　　として、
θ・・の項のみ取り出し新しいインスタンス化
除かれた方をｂとして[L1_b,L2_b]ベクトルを作る
θ・・の項は
[L1_1   L1_2
 L2_1   L2_2 ]　というインスタンスの行列　A　を考える

 L1_2 = L1の式の中の　θ2・・の項
 θ・・ =　A^-1 b を計算
 
 オイラー法の陽解法でθ・　θ　も出てくる
 
 ここまでやってみよう
 
 L1_b,L1_1,L1_2　を生成する　方法を考える
'''

m1=1.0
m2=1.0

r1=1.0
r2=1.0
g=9.81


T=np.asarray([[0,2,0,0,0,m1*r1**2/2  + m2*r2**2 /2,
              0,0,0,0,0,1],
             [0,0,0,0,0,m2*r2**2/2 ,
                0,2,0,0,0,1],
              [0,1,0,1,0,m2*r1*r2,
                0,1,0,1,0,1],
               [0,1,0,0,1,m2*r1*r2,
                0,1,0,0,1,1]]
    )
T_n=np.asarray(["m_1*r_1^2/2  + m2*r_2^2 /2",
              "m_2*r_2^2/2" ,
              "m_2*r_1*r_2",
              "m_2*r_1*r_2"],dtype=object    )

U= np.asarray([[0,0,0,0,0,g*(m1*r1+m2*(r1+r2)),
                0,0,0,0,0,1],
               [0,0,0,0,1,-1*g*(m1*r1+m2*r1),
                0,0,0,0,0,1],
                [0,0,0,0,0,-1*g*r2*m2,
                0,0,0,0,1,1]
                ])  
U_n= np.asarray(["g*(m_1*r_1+m_2*(r_1+r_2))",
               "-1*g*(m_1*r_1+m_2*r_1)",
                "-1*g*r_2*m_2"],dtype=object)  

T1=Formula2.Formula2(T,2,T_n)
SaveT = copy.deepcopy(T1)
U1=Formula2.Formula2(U,2,U_n)
T1-U1
Lt_1=copy.deepcopy(T1)
Lt_2=copy.deepcopy(T1)
Lt_1.partial(0,0)
Lt_2.partial(0,1)
Lt_2.diff()
Lt_1-Lt_2

Ls_1=copy.deepcopy(T1)
Ls_2=copy.deepcopy(T1)
Ls_1.partial(1,0)
Ls_2.partial(1,1)
Ls_2.diff()
Ls_1-Ls_2

Lt_1.disp2()
Ls_1.disp2()
T1.disp2()

L1 = copy.deepcopy(Ls_1)
L2 = copy.deepcopy(Lt_1)

L1_1,L1_2 = L1.separete()
L2_1,L2_2 = L2.separete()

'''
T1:ラグランジアン
Lt_1 ,L1 :θ1に関する運動方程式
Ls_2 ,L2 :θ2に関する運動方程式
'''
```


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

解析用クラス

```python:Formula.py
import numpy as np		#	Numpyライブラリ
import copy
import math
'''
数式処理プログラム
列　：　多項式
行　：　θ　θ・　θ・・　sinθ　cosθ　係数　 
の6個　×　変数数


'''
class Formula :
    def __init__(self,L,num):
        self.L = L
        self.n = num                      #変数の種類数
        
    
    def __add__(self,other):
        self.L = np.append(self.L,other.L,axis =0)
        self.together()
        self.erase()

    def __sub__(self,other):
        expr,vari = other.L.shape
        hoge = copy.deepcopy(other.L)
        for i in range(expr):
            hoge[i][5] = hoge[i][5] * -1
        self.L = np.append(self.L,hoge,axis =0)
        self.together()
        self.erase()
    
    def latex(self):
        variable = " x"
        k=1
        print "-------------------Formula--------------"
        expr,vari = self.L.shape
        for j in range(expr):
            hoge =""
            for i in range(self.n):
                hoge += "  "
                if self.L[j][5]!= 0:
                    if i == 0:
                        hoge += str(self.L[j][5+i*6])
                    if self.L[j][0+i*6]!= 0:   
                        hoge += variable+"_"+str(i+k)+"^{"
                        hoge += str(self.L[j][0+i*6])+"}"
                        
                    if self.L[j][1+i*6]!= 0:
                        hoge += " \dot{"+variable+"_"+str(i+k)+"}^{"
                        hoge += str(self.L[j][1+i*6])+"}"
                        
                    if self.L[j][2+i*6]!= 0:
                        hoge += " \ddot {"+variable+"_"+str(i+k)+"}^{"
                        hoge += str(self.L[j][2+i*6])+"}"
                        
                    if self.L[j][3+i*6]!= 0:
                        hoge += " sin("+variable+"_"+str(i+k)+")^{"
                        hoge += str(self.L[j][3+i*6])+"}"
                        
                    if self.L[j][4+i*6]!= 0:
                        hoge += " cos("+variable+"_"+str(i+k)+")^{"
                        hoge += str(self.L[j][4+i*6])+"}"
                         
                    hoge += "   "
                
            hoge += "  +  "
            print hoge
            
        
    def together(self):               #同じ項の係数をまとめる
        expr,vari = self.L.shape
        if expr >=2:
            for i in range(expr-1):
                for j in range(i+1,expr):                    
                    m=0
                    while self.L[i][0+m*6:5+m*6].tolist() == self.L[j][0+m*6:5+m*6].tolist():
                        m += 1
                        if m == self.n:
                            break
                    if m== self.n:
                        self.L[i][5] += self.L[j][5]
                        for k in range(vari):
                            self.L[j][k] = 0
                            
    def erase(self):    #係数が0の項を消す
        expr,vari = self.L.shape
        for i in list(reversed(range(expr))):
            if self.L[i][5] == 0:
                self.L = np.delete(self.L,i,axis = 0)
                
    def partial(self,moji,kind):  #kindの種類のもので項を偏微分する
        expr,vari = self.L.shape
        if kind == 0:   
            '''
            sin
            cos
            
            +moji*6
            '''
            #print self.L
            for i in range(expr):
                if self.L[i][3+moji*6] !=0:  #sin
                    hoge = copy.deepcopy(self.L[i])
                    hoge[5] = hoge[5]*hoge[3+moji*6]
                    hoge[3+moji*6] = hoge[3+moji*6]-1
                    hoge[4+moji*6] = hoge[4+moji*6]+1
                    self.L = np.append(self.L,hoge[np.newaxis,:],axis =0)
                    #print self.L
            for i in range(expr):
                if self.L[i][4+moji*6] !=0:  #cos
                    hoge = copy.deepcopy(self.L[i])
                    hoge[5] = hoge[5]*hoge[4+moji*6]*-1
                    hoge[4+moji*6]= hoge[4+moji*6]-1
                    hoge[3+moji*6] = hoge[3+moji*6]+1
                    self.L = np.append(self.L,hoge[np.newaxis,:],axis =0)
                    #print self.L
            '''
            そのままのもの
            次数を一つ下げる、0なら何もしない
            元の次数を係数に掛ける
            '''
            #print self.L
            for i in range(expr):
                if self.L[i][kind] !=0:
                    #print kind,self.L[i][5]
                    self.L[i][5] = self.L[i][5] * self.L[i][kind+moji*6]
                    #print self.L[i][5]
                    self.L[i][kind+moji*6] = self.L[i][kind+moji*6] -1
                else:
                    self.L[i][5] = 0   #含まないなら　0
            
        if kind == 1:   
            '''
            そのままのもの
            次数を一つ下げる、0なら何もしない
            元の次数を係数に掛ける
            '''
            for i in range(expr):
                if self.L[i][kind+moji*6] !=0:
                    self.L[i][5] = self.L[i][5] * self.L[i][kind+moji*6]
                    self.L[i][kind+moji*6] = self.L[i][kind+moji*6] -1
                    
                else:
                    self.L[i][5] = 0   #含まないなら　0
                    
        self.together()
        self.erase()
        
    def diff(self):  #時間微分
        '''
        4つのパターンで偏微分後
        θ・の次数を1上げる        
        '''
        L1=copy.deepcopy(self.L)
        
        for i in range(self.n):
            self.L =copy.deepcopy( L1)
            self.partial(i,0)
            expr,vari = self.L.shape
            #print expr
            #print self.L
            for j in range(expr):
                self.L[j][1+i*6] = self.L[j][1+i*6] + 1
            if i == 0:
                L2 = copy.deepcopy( self.L)
            else:
                L2 = np.append(L2,self.L,axis =0)
            
            self.L =copy.deepcopy( L1)         
            self.partial(i,1)
            expr,vari = self.L.shape
            for j in range(expr):
                self.L[j][2+i*6] += 1
            L2 = np.append(L2,self.L,axis =0)
                    
        self.L = copy.deepcopy( L2)
        
        self.together()
        self.erase()
    
    def disp(self):  #多項式表示
        print "-------------------Formula--------------"
        expr,vari = self.L.shape
        for j in range(expr):
            hoge =""
            for i in range(self.n):
                hoge += str(self.L[j][5+i*6])
                hoge += " θ^"
                hoge += str(self.L[j][0+i*6])
                hoge += " θ・^"
                hoge += str(self.L[j][1+i*6])
                hoge += " θ・・^"
                hoge += str(self.L[j][2+i*6])
                hoge += " sin^"
                hoge += str(self.L[j][3+i*6])
                hoge += " cos^"
                hoge += str(self.L[j][4+i*6])
                hoge += "   "
                
            hoge += "  +  "
            print hoge
            
    def disp2(self):  #多項式表示
        print "-------------------Formula--------------"
        expr,vari = self.L.shape
        for j in range(expr):
            hoge =""
            for i in range(self.n):
                hoge += "["+str(i+1)+"  "
                if self.L[j][5]!= 0:
                    if i == 0:
                        hoge += str(self.L[j][5+i*6])
                    if self.L[j][0+i*6]!= 0:   
                        hoge += " θ^"
                        hoge += str(self.L[j][0+i*6])
                        
                    if self.L[j][1+i*6]!= 0:
                        hoge += " θ・^"
                        hoge += str(self.L[j][1+i*6])
                        
                    if self.L[j][2+i*6]!= 0:
                        hoge += " θ・・^"
                        hoge += str(self.L[j][2+i*6])
                        
                    if self.L[j][3+i*6]!= 0:
                        hoge += " sin^"
                        hoge += str(self.L[j][3+i*6])
                        
                    if self.L[j][4+i*6]!= 0:
                        hoge += " cos^"
                        hoge += str(self.L[j][4+i*6])
                         
                    hoge += " ]  "
                
            hoge += "  +  "
            print hoge
            
    def subst(self,x):            #代入 x ,x・の順に入れる
        vari =np.array(x).shape
        if vari[0] != self.n*2:
            print "cannot subst"
        else:
            '''
            行ベクトルを生成して
            dotでLと  をして、できたベクトルの全要素を足せばよい
            '''
            
            expr,vari = self.L.shape
            sum1 = 0
            hoge=0
            for j in range(expr):
                hoge=self.L[j][5]           #係数
                for i in range(self.n):
                    
                    hoge = hoge*x[0+i*2]**self.L[j][0+i*6]            #θ
                    hoge = hoge*x[1+i*2]**self.L[j][1+i*6]            #θ・
                    hoge = hoge*math.sin(x[0+i*2])**self.L[j][3+i*6]  #sin
                    hoge = hoge*math.cos(x[0+i*2])**self.L[j][4+i*6]  #cos
                    
                    
                sum1 += hoge
                
            return sum1
    def separete(self):              #2変数と考えて、　1　２　ｂ　の3つに分割する
        a1=np.asarray([[]])
        a2=np.asarray([[]])
        expr,vari = self.L.shape
        for j in range(expr)[::-1]:
            if self.L[j][2] == 1:
                if len(a1[0]) == 0:
                    a1=(self.L[j]) [np.newaxis,:]
                else:
                    a1 = np.append(a1,(self.L[j]) [np.newaxis,:],axis =0)
                self.L = np.delete(self.L,j,0)
                continue
                
            if self.L[j][8] == 1:
                if len(a2[0]) == 0:
                    a2=(self.L[j]) [np.newaxis,:]
                else:
                    a2 = np.append(a2,(self.L[j]) [np.newaxis,:],axis =0)
                self.L = np.delete(self.L,j,0)
                continue
        
        if len(a1[0]) == 0:
            a1 =np.asarray([ [0,0,0,0,0,0  ,0,0,0,0,0,0]])
        if len(a2[0]) == 0:
            a2 = np.asarray([[0,0,0,0,0,0  ,0,0,0,0,0,0]])
        L1 = Formula(a1,2)
        L2 = Formula(a2,2)
        return L1,L2
    
    def separete3(self):              #3変数と考えて、　1　2 3　ｂ　の 4 つに分割する
        a1=np.asarray([[]])
        a2=np.asarray([[]])
        a3=np.asarray([[]])
        expr,vari = self.L.shape
        for j in range(expr)[::-1]:
            
            print "a1"
            print a1.shape
            print "a3"
            print a3.shape
            print "a2"
            print a2.shape
            
            if self.L[j][2] == 1:
                if len(a1[0]) == 0:
                    a1=(self.L[j]) [np.newaxis,:]
                else:
                    a1 = np.append(a1,(self.L[j]) [np.newaxis,:],axis =0)
                self.L = np.delete(self.L,j,0)
                continue
                
            if self.L[j][8] == 1:
                if len(a2[0]) == 0:
                    a2=(self.L[j]) [np.newaxis,:]
                else:
                    a2 = np.append(a2,(self.L[j]) [np.newaxis,:],axis =0)
                self.L = np.delete(self.L,j,0)
                continue
            
            if self.L[j][14] == 1:
                if len(a3[0]) == 0:
                    a3=(self.L[j]) [np.newaxis,:]
                else:
                     a3 = np.append(a3,(self.L[j]) [np.newaxis,:],axis =0)
                self.L = np.delete(self.L,j,0)
                continue
        
        if len(a1[0]) == 0:
            a1 =np.asarray([ [0,0,0,0,0,0  ,0,0,0,0,0,0  ,0,0,0,0,0,0]])
        if len(a2[0]) == 0:
            a2 = np.asarray([[0,0,0,0,0,0  ,0,0,0,0,0,0  ,0,0,0,0,0,0]])
        if len(a3[0]) == 0:
            a3 = np.asarray([[0,0,0,0,0,0  ,0,0,0,0,0,0  ,0,0,0,0,0,0]])
            
        L1 = Formula(a1,3)
        L2 = Formula(a2,3)
        L3 = Formula(a3,3)
        return L1,L2,L3
        
    if __name__ == '__main__':
        print "This is not for main"
```

実行プログラム

```python
# -*- coding: utf-8 -*-
"""
Created on Sat Mar 21 14:11:03 2020

@author: kisim
"""
import numpy as np		#	Numpyライブラリ
import copy
import math
#import matplotlib.pyplot as plt

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

import matplotlib.animation as animation
from matplotlib.animation import PillowWriter

import Formula
            

#L= np.asarray([[7,2,3,4,5,6,     7,2,3,4,5,6]])
#L= np.asarray([[2,2,0,5,4,5],[2,2,0,5,4,5]])

'''
L1,L2　の二つしか　式がなく、変数も2つの場合で考えよう

式　≒　０　　として、
θ・・の項のみ取り出し新しいインスタンス化
除かれた方をｂとして[L1_b,L2_b]ベクトルを作る
θ・・の項は
[L1_1   L1_2
 L2_1   L2_2 ]　というインスタンスの行列　A　を考える

 L1_2 = L1の式の中の　θ2・・の項
 θ・・ =　A^-1 b を計算
 
 オイラー法の陽解法でθ・　θ　も出てくる
 
 ここまでやってみよう
 
 L1_b,L1_1,L1_2　を生成する　方法を考える
'''

m1=1.0
m2=1.0
l1=1.0
l2=1.0
r1=1.0
r2=1.0
R1=1.0
g=9.81

T=np.asarray([[0,2,0,0,0,m1*r1**2/2  + m2*r2**2 /2,
              0,0,0,0,0,1],
             [0,0,0,0,0,m2*r2**2/2 ,
                0,2,0,0,0,1],
              [0,1,0,1,0,m2*r1*r2,
                0,1,0,1,0,1],
               [0,1,0,0,1,m2*r1*r2,
                0,1,0,0,1,1]]
    )
T_n=np.asarray(["m_1*r_1^2/2  + m2*r_2^2 /2",
              "m_2*r_2^2/2" ,
              "m_2*r_1*r_2",
              "m_2*r_1*r_2"],dtype=object    )

U= np.asarray([[0,0,0,0,0,g*(m1*r1+m2*(r1+r2)),
                0,0,0,0,0,1],
               [0,0,0,0,1,-1*g*(m1*r1+m2*r1),
                0,0,0,0,0,1],
                [0,0,0,0,0,-1*g*r2*m2,
                0,0,0,0,1,1]
                ])  
U_n= np.asarray(["g*(m_1*r_1+m_2*(r_1+r_2))",
               "-1*g*(m_1*r_1+m_2*r_1)",
                "-1*g*r_2*m_2"],dtype=object)  

T1=Formula.Formula(T,2)
U1=Formula.Formula(U,2)
T1-U1
Lt_1=copy.deepcopy(T1)
Lt_2=copy.deepcopy(T1)
Lt_1.partial(0,0)
Lt_2.partial(0,1)
Lt_2.diff()
Lt_1-Lt_2

Ls_1=copy.deepcopy(T1)
Ls_2=copy.deepcopy(T1)
Ls_1.partial(1,0)
Ls_2.partial(1,1)
Ls_2.diff()
Ls_1-Ls_2

Lt_1.disp2()
Ls_1.disp2()
T1.disp2()

L1 = copy.deepcopy(Ls_1)
L2 = copy.deepcopy(Lt_1)

L1_1,L1_2 = L1.separete()
L2_1,L2_2 = L2.separete()

'''
Lt_1  :θ1に関する運動方程式
Ls_2  :θ2に関する運動方程式
'''

'''
オイラー法による数値解法を行う
初期値設定
解法
表示
の3プロセスを実装する必要がある
'''


DT = 0.001              #時刻
x=[math.pi,2,math.pi,2]            #初期値
saveX = x
T=10
N=int(T/DT)                 #計算時間/DT

for i in range(N):
    b1 = L1.subst(x) * -1
    b2 = L2.subst(x) * -1
    a11 = L1_1.subst(x)
    a12 = L1_2.subst(x)
    a21 = L2_1.subst(x)
    a22 = L2_2.subst(x)
    
    hoge = a11*a22-a12*a21
    if hoge == 0:
         print "hoge = 0 break"
         break
    alpha1 = (a22*b1-a12*b2)/hoge   #A^-1 b
    alpha2 = (a11*b2-a21*b1)/hoge
    
    x2 =np.array( x ) + np.array( [ x[1] ,alpha1 ,x[3],alpha2 ] ) * DT
    
    x = x2
    saveX = np.append(saveX,x, axis=0)


saveX = saveX.reshape([4,N+1],order ='F')

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

'''
#点履歴
position_x1=r1*np.sin(saveX[0]) 
position_y1=-1*r1*np.cos(saveX[0])
position_x2=R1*np.sin(saveX[0])+r2*np.sin(saveX[2])
position_y2=-1*R1*np.cos(saveX[0]) - r2*np.cos(saveX[2])
plt.figure(figsize=(28, 20))  # (width, height)
plt.rcParams["font.size"] = 25
plt.plot(position_x1,position_y1,label ="1")
plt.plot(position_x2,position_y2,label="2")
plt.legend(fontsize = 25)

plt.savefig('figure0.png')
'''

'''
#グラフ化
z=np.sin(saveX[0])*R1+np.sin(saveX[2])*r2
plt.figure(figsize=(28, 20))  # (width, height)
plt.rcParams["font.size"] = 25
plt.plot(t,z,label ="z")
plt.plot(t,saveX[0],label="x0")
plt.plot(t,saveX[1],label="dt x0")
plt.plot(t,saveX[2],label="x1")
plt.plot(t,saveX[3],label="dt x1")
plt.legend(fontsize = 25)

plt.savefig('figure0.png')

'''

#animation化

print "start animation"
ims = []
fig,ax = plt.subplots()
ax.set_xlim([-2, 2])
ax.set_ylim([-2, 2])


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

for i in range(int(N/Step)):
        position_x1=r1*np.sin(saveX[0][i*Step]) 
        position_y1=-1*r1*np.cos(saveX[0][i*Step])
        position_x2=R1*np.sin(saveX[0][i*Step])+r2*np.sin(saveX[2][i*Step])
        position_y2=-1*R1*np.cos(saveX[0][i*Step]) - r2*np.cos(saveX[2][i*Step])
        im = plt.plot([0,position_x1,position_x2],[0,position_y1,position_y2],color='blue')             # 乱数をグラフにする
        ims.append(im)                  # グラフを配列 ims に追加

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

```