Double pendulum simulation

What is a double pendulum?

https://www.youtube.com/watch?v=25feOUNQB2Y This is a famous experiment as a chaos phenomenon. The chaos phenomenon is a phenomenon in which the initial state has a great influence on the subsequent state. The story of the butterfly effect, in which the wings of a butterfly generate a typhoon due to airflow, is also mentioned in the explanation of the same chaos phenomenon. (May not be correct)

The equation of motion is derived and simulated using the Euler-Lagrange method created by here.

Variable definition

define.jpg

Set the problem in the mass system / two-dimensional.

The speed of m1 and the speed of m2

\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}\\


Kinetic energy T

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

Potential energy U

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

When you calculate Lagrangian

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

When deriving the equation of motion

(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

The code I used is hard to see, so I'll attach it at the end.

Simulation (Euler method)

When simulated by the Euler method using the obtained formula Setting

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

initial value:

\theta_1 = 180deg\\
\theta_2 = 180deg\\
\dot{\theta_1 }= 2rad/s\\
\dot{\theta_2} = 2rad/s\\

output2.gif

initial value:

\theta_1 = 180deg\\
\theta_2 = 180deg\\
\dot{\theta_1 }= 0.1rad/s\\
\dot{\theta_2} = 0.1rad/s

output2.gif This one is more chaotic (middle 2 average feeling)

\theta_1 = 45deg\\
\theta_2 = 45deg\\
\dot{\theta_1 }= 0.1rad/s\\
\dot{\theta_2} = 0.1rad/s

output2.gif You can't be chaotic without speed

Equation of motion derivation code

From here on, it will be the program you created. It should work with copy and paste. I'm running on anaconda python2. Click here to animate gif https://qiita.com/yubais/items/c95ba9ff1b23dd33fde2 I referred to.

Analysis class

Formula2


import numpy as np		#Numpy library
import copy
import math
'''
Computer algebra program
Column: Polynomial
Row: θ θ ・ θ ・ ・ sin θ cos θ coefficient
6 x number of variables


Corresponds to character expressions
However,

'''
class Formula2 :
    def __init__(self,L,num,coeff):
        self.L = L
        self.n = num                      #Number of variable types
        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):               #Summarize the coefficients of the same term
        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:  #Addition of all match coefficients
                        self.L[i][5] += self.L[j][5]
                        self.coeff[i] = self.coeff[i] +" + "+ self.coeff[j]
                        for k in range(vari):  #j Line deleted
                            self.L[j][k] = 0
                        self.coeff[j] = "0"
                         
    def erase(self):    #Erase the term with a coefficient of 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):  #Partial derivative of terms with 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 to coefficient[3+moji*6]multiply
                    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 to coefficient[3+moji*6]multiply
                    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
            '''
As it is
Decrease the order by one, do nothing if 0
Multiply the original order by the coefficient
            sin,It's okay because the cos processing part is behind and not in the processing range
            '''
            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 if not included
                    self.coeff[i] = "0"
            
        if kind == 1:   
            '''
As it is
Decrease the order by one, do nothing if 0
Multiply the original order by the coefficient
            '''
            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 if not included
                    self.coeff[i] = "0"
                    
        
        self.together()
        self.erase()
        
        
    def diff(self):  #Time derivative
        '''
After partial differentiation with 4 patterns
Increase the order of θ · by 1.
        '''
        L1=copy.deepcopy(self.L)
        L1_coeff = copy.deepcopy(self.coeff)
        
        for i in range(self.n):
            self.L =copy.deepcopy( L1)   #L1:Before differentiation L2:For saving results
            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):  #Polynomial display
        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):              #Think of it as two variables and divide it into three, 1 and 2b.
        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):              #Think of it as 3 variables and divide it into 4 of 1 2 3 b
        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()
        '''

Execution code

import numpy as np		#Numpy library
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,Let's consider the case where there are only two expressions of L2 and there are two variables.

As the formula ≒ 0
Extract only the item of θ ... and instantiate it
The removed one is designated as b[L1_b,L2_b]Make a vector
The term of θ ...
[L1_1   L1_2
 L2_1   L2_2 ]Consider the instance matrix A

 L1_2 =Item of θ2 in the formula of L1
θ ...= A^-Calculate 1 b
 
The positive solution of the Euler method also produces θ and θ.
 
Let's do it so far
 
 L1_b,L1_1,L1_Think about how to generate 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:Lagrangian
Lt_1 ,L1 :Equation of motion for θ1
Ls_2 ,L2 :Equation of motion for θ2
'''

Simulation code

Analysis class

Formula.py


import numpy as np		#Numpy library
import copy
import math
'''
Computer algebra program
Column: Polynomial
Row: θ θ ・ θ ・ ・ sin θ cos θ coefficient
6 x number of variables


'''
class Formula :
    def __init__(self,L,num):
        self.L = L
        self.n = num                      #Number of variable types
        
    
    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):               #Summarize the coefficients of the same term
        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):    #Erase the term with a coefficient of 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):  #Partial derivative of terms with 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
            '''
As it is
Decrease the order by one, do nothing if 0
Multiply the original order by the coefficient
            '''
            #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 not included
            
        if kind == 1:   
            '''
As it is
Decrease the order by one, do nothing if 0
Multiply the original order by the coefficient
            '''
            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 if not included
                    
        self.together()
        self.erase()
        
    def diff(self):  #Time derivative
        '''
After partial differentiation with 4 patterns
Increase the order of θ · by 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):  #Polynomial display
        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):  #Polynomial display
        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):            #Substitution x,Put in the order of x
        vari =np.array(x).shape
        if vari[0] != self.n*2:
            print "cannot subst"
        else:
            '''
Generate row vector
Do L with dot and add all the elements of the resulting vector
            '''
            
            expr,vari = self.L.shape
            sum1 = 0
            hoge=0
            for j in range(expr):
                hoge=self.L[j][5]           #coefficient
                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):              #Think of it as two variables and divide it into three, 1 and 2b.
        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):              #Think of it as 3 variables and divide it into 4 of 1 2 3 b
        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"

Execution program

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

@author: kisim
"""
import numpy as np		#Numpy library
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,Let's consider the case where there are only two expressions of L2 and there are two variables.

As the formula ≒ 0
Extract only the item of θ ... and instantiate it
The removed one is designated as b[L1_b,L2_b]Make a vector
The term of θ ...
[L1_1   L1_2
 L2_1   L2_2 ]Consider the instance matrix A

 L1_2 =Item of θ2 in the formula of L1
θ ...= A^-Calculate 1 b
 
The positive solution of the Euler method also produces θ and θ.
 
Let's do it so far
 
 L1_b,L1_1,L1_Think about how to generate 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  :Equation of motion for θ1
Ls_2  :Equation of motion for θ2
'''

'''
Perform numerical solution by Euler method
Initial value setting
solution
display
Need to implement 3 processes
'''


DT = 0.001              #Times of Day
x=[math.pi,2,math.pi,2]            #initial value
saveX = x
T=10
N=int(T/DT)                 #Calculation time/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)

'''
#Point history
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')
'''

'''
#Graphing
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   #Number of frames to skip

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')             #Graph random numbers
        ims.append(im)                  #Add graph to array ims

#Display 10 plots every 100ms
ani = animation.ArtistAnimation(fig, ims, interval=DT*Step)
ani.save("output2.gif", writer='pillow')

Recommended Posts

Double pendulum simulation
Escalator simulation
Double pendulum equation of motion in python