Let Python calculate Euler-Lagrange's equation of motion
Purpose and overview
Since the derivation of the equation of motion by the Euler-Lagrange equation uses energy, it is characterized by small calculation errors. It is a convenient method because it is easy to apply even to the complicated equation of motion of three dimensions. However, there is the trouble of calculating the partial differential after deriving the Lagrangian and solving the simultaneous equations. The content to be posted this time is that after the introduction of Lagrangian, partial differentiation and time differentiation are performed, and simulation is performed using the Euler method. It does not yet support character expressions. Also, it only supports polynomials of sin, cos, d / dt ^ 2 / dt ^ 2.
For those who can use matlab and scilab, browser back is recommended because it is completely unrelated.
3D Y-pendulum
https://www.youtube.com/watch?v=eeizdZscRjU
https://www.youtube.com/watch?v=llBik9FsEDo
The result of simulating
I am quite satisfied with the curve that matches the experiment.
If you want to experiment instead of computational simulation, you can use the powder used for washing.
However, when it comes to 3D, it becomes a peculiar state in terms of Euler angles, and it is difficult for the calculated value to increase explosively or to stop at nan and not work. For the time being, I'm processing it as Moore's general inverse matrix, but I don't know what to do because I haven't studied enough. Someone tell me For now, we use the Euler method for simulation, but it may be better to use the Rungelkutta method. However, I feel that the numerical explosion is different from the problem of calculation accuracy.
2D double pendulum
https://www.youtube.com/watch?v=25feOUNQB2Y
The result of simulating (Jif animation, so click to see)
For the time being, it looks like that.
Euler-Lagrange equation
Lagrangian $ L $
L=T-U\\
T:Physical energy\\
U:Potential energy
Against
\frac {\partial L}{\partial x} - \frac{d}{dt} \frac{\partial L}{\partial \dot{x}} = 0
Is the equation of motion. x is the generalized position, such as position coordinates and rotation angle.
Plug ram
I am running with python2 in anaconda environment. 1 As mentioned above, the difficulty of the Euler-Lagrange equation is
- Since there are many partial derivatives, calculation errors are likely to occur.
- It is troublesome to put together the same terms after partial differentiation
- When it comes to multivariable general coordinates, the general coordinates are intertwined and it is difficult to divide them into the form of $ d ^ 2 x_1 / dt ^ 2 = $ and $ d ^ 2 x_2 / dt ^ 2 = $ (for simulation). Can be solved without dividing into
When you actually solve the problem, you're probably tired of it.
2 Those who can use matlab and scilab will not be in trouble. Also, when solving the differential equation after deriving the equation of motion, there is always a solution such as ode45, so I think that it should be done. However, I couldn't find a way to convert it to $ d ^ 2 x_1 / dt ^ 2 = $ and $ d ^ 2 x_2 / dt ^ 2 = $, so I decided to calculate it by force.
3 code
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):
self.L = np.append(self.L,-1*other.L,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:
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 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
self.L =Polynomial information
self.n =Number of variables
self.L = [ [1, 2 ,3 , 4, 5, 6]
[7, 8, 9, 10,11,12]]
in the case of
L = x^1*(\frac{d}{dt} x) ^2*(\frac{d^2}{dt^2} x) ^3 sin(x)^4 cos(x)^5 *6\\
+\\
x^7*(\frac{d}{dt} x) ^8*(\frac{d^2}{dt^2} x) ^9 sin(x)^{10} cos(x)^{11} *12\\
Represents.
the function is
L1 + L2: L1 = L1 + L2 It's a little confusing L1-L2 : L1 = L1-L2 *: Not implemented
L1.partial (i, 0): Partial derivative with $ x_i $ L1.partial (i, 1): Partial derivative with $ \ frac {d} {dt} x_i $
L1.diff (): Time derivative
L1.disp (): Display polynomial L1.disp2 (): Easy-to-understand display
Is implemented.
Y-pendulum equation of motion
Next, I will explain how to simulate a Y-shaped pendulum.
The Y-shaped pendulum does not move the V-shaped angle. In other words, it is a pin join that allows only one-way rotation (it can move only in the Z-X plane).
A Y-shaped pendulum model can be created by attaching a weight to the ceiling with a pin joint and a ball joint (allowing three rotations).
In this way, set the rotation angle $ \ theta 123 $.
\vec{R_{m1}} =
\left(
\begin{array}{ccc}
R_1*cos( \theta 1)\\
0\\
-R_1*sin(\theta2)
\end{array}
\right)
,
\vec{R_{m2}} = \vec{R_{m1}} +
\left(
\begin{array}{ccc}
R_3*sin(\theta3) * cos(\theta2)\\
R_3*sin(\theta3) * sin(\theta2)\\
-R_3*cos(\theta3)
\end{array}
\right)
It will be. This is time-differentiated, the velocity vector is calculated, and the kinetic energy is calculated from the magnitude of the velocity vector.
T=\frac{1}{2} m_1 (R_1\dot{\theta})^2+\frac{1}{2} m_2 (R_1^2 \dot{\theta}^2 + R_3^2 sin(\theta 3)^2 \dot{\theta 2}^2 + R_3^2 \dot{\theta 3}^2
\\
-2*cos(\theta 1) sin(\theta 2) R_1 R_3 sin(\theta 3) \dot{\theta 1} \dot{\theta 2}\\
+2*(cos(\theta 1) cos(\theta 2) cos(\theta 3) + sin(\theta 1) sin(\theta 2) )R_1 R_3 \dot{\theta 1} \dot{\theta 3}
Potential energy
U=m_1 g R_1 (1- cos(\theta 1)) + m_2 g (R_1 (1- cos(\theta 1)) + R_3(1-cos(\theta 3))
Will be.
If you use this to calculate the Lagrangian
L= m_1*r_1^2 /2 + m_2*r_1^2/2 \dot{ x_1}^{2.0} +
m_2 /2 *r_3^2 \dot{ x_2}^{2.0} sin( x_3)^{2.0} +
m_2 /2 *r_3^2 \dot{ x_3}^{2.0} +
m_2 /2 *(-2*r_1*r_3) \dot{ x_1}^{1.0} cos( x_1)^{1.0} \dot{ x_2}^{1.0} sin( x_2)^{1.0} sin( x_3)^{1.0} +
m_2 /2 *(2*r_1*r_3) \dot{ x_1}^{1.0} cos( x_1)^{1.0} cos( x_2)^{1.0} \dot{ x_3}^{1.0} cos( x_3)^{1.0} +
m_2 /2 *(2*r_1*r_3) \dot{ x_1}^{1.0} sin( x_1)^{1.0} \dot{ x_3}^{1.0} sin( x_3)^{1.0} +
-1*(g*(m_1*r1+m_2*(r_1+r_3))) +
-1*(-1*g*(m_1*r1+m_2*r_1)) cos( x_1)^{1.0} +
-1*(-1*g*m_2*r_3) cos( x_3)^{1.0}
It will be. θ and x are interchanged and displayed
x_1 = \theta 1
x_2 = \theta 2
x_3 = \theta 3
If you derive the equation of motion
(m_2 /2 *(2*r_1*r_3) *1.0) + -1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *1.0) ) \dot{ x_1}^{1.0} cos( x_1)^{1.0} \dot{ x_3}^{1.0} sin( x_3)^{1.0} +
(m_2 /2 *(-2*r_1*r_3)*(-1)*1.0) + -1*( ( (m_2 /2 *(-2*r_1*r_3)*1.0) *(-1)*1.0) ) \dot{ x_1}^{1.0} sin( x_1)^{1.0} \dot{ x_2}^{1.0} sin( x_2)^{1.0} sin( x_3)^{1.0} +
(m_2 /2 *(2*r_1*r_3) *(-1)*1.0) + -1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *(-1)*1.0) ) \dot{ x_1}^{1.0} sin( x_1)^{1.0} cos( x_2)^{1.0} \dot{ x_3}^{1.0} cos( x_3)^{1.0} +
(-1*(-1*g*(m_1*r1+m_2*r_1))*(-1)*1.0) sin( x_1)^{1.0} +
0 + 0 + 0 +
0 + 0 +
0 +
-1*( ( (m_1*r_1^2 /2 + m_2*r_1^2/2*2.0) *1.0) ) \ddot { x_1}^{1.0} +
-1*( ( (m_2 /2 *(-2*r_1*r_3)*1.0) *1.0) ) cos( x_1)^{1.0} \dot{ x_2}^{2.0} cos( x_2)^{1.0} sin( x_3)^{1.0} +
-1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *(-1)*1.0) + ( (m_2 /2 *(-2*r_1*r_3)*1.0) *1.0) ) cos( x_1)^{1.0} \dot{ x_2}^{1.0} sin( x_2)^{1.0} \dot{ x_3}^{1.0} cos( x_3)^{1.0} +
-1*( ( (m_2 /2 *(-2*r_1*r_3)*1.0) *1.0) ) cos( x_1)^{1.0} \ddot { x_2}^{1.0} sin( x_2)^{1.0} sin( x_3)^{1.0} +
-1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *1.0) ) sin( x_1)^{1.0} \dot{ x_3}^{2.0} cos( x_3)^{1.0} +
-1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *(-1)*1.0) ) cos( x_1)^{1.0} cos( x_2)^{1.0} \dot{ x_3}^{2.0} sin( x_3)^{1.0} +
-1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *1.0) ) cos( x_1)^{1.0} cos( x_2)^{1.0} \ddot { x_3}^{1.0} cos( x_3)^{1.0} +
-1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *1.0) ) sin( x_1)^{1.0} \ddot { x_3}^{1.0} sin( x_3)^{1.0} =0\\
(m_2 /2 *(-2*r_1*r_3)*1.0) + -1*( ( (m_2 /2 *(-2*r_1*r_3)*1.0) *1.0) ) \dot{ x_1}^{1.0} cos( x_1)^{1.0} \dot{ x_2}^{1.0} cos( x_2)^{1.0} sin( x_3)^{1.0} +
(m_2 /2 *(2*r_1*r_3) *(-1)*1.0) + -1*( ( (m_2 /2 *(-2*r_1*r_3)*1.0) *1.0) ) \dot{ x_1}^{1.0} cos( x_1)^{1.0} sin( x_2)^{1.0} \dot{ x_3}^{1.0} cos( x_3)^{1.0} +
-1*( ( (m_2 /2 *(-2*r_1*r_3)*1.0) *(-1)*1.0) ) \dot{ x_1}^{2.0} sin( x_1)^{1.0} sin( x_2)^{1.0} sin( x_3)^{1.0} +
-1*( ( (m_2 /2 *(-2*r_1*r_3)*1.0) *1.0) ) \ddot { x_1}^{1.0} cos( x_1)^{1.0} sin( x_2)^{1.0} sin( x_3)^{1.0} +
0 + 0 +
-1*( ( (m_2 /2 *r_3^2*2.0) *1.0) ) \ddot { x_2}^{1.0} sin( x_3)^{2.0} +
-1*( ( (m_2 /2 *r_3^2*2.0) *2.0) ) \dot{ x_2}^{1.0} \dot{ x_3}^{1.0} sin( x_3)^{1.0} cos( x_3)^{1.0} =0\\
(m_2 /2 *r_3^2*2.0) \dot{ x_2}^{2.0} sin( x_3)^{1.0} cos( x_3)^{1.0} +
(m_2 /2 *(-2*r_1*r_3)*1.0) + -1*( ( (m_2 /2 *(2*r_1*r_3) *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} cos( x_3)^{1.0} +
(m_2 /2 *(2*r_1*r_3) *1.0) + -1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *1.0) ) \dot{ x_1}^{1.0} sin( x_1)^{1.0} \dot{ x_3}^{1.0} cos( x_3)^{1.0} +
(m_2 /2 *(2*r_1*r_3) *(-1)*1.0) + -1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *(-1)*1.0) ) \dot{ x_1}^{1.0} cos( x_1)^{1.0} cos( x_2)^{1.0} \dot{ x_3}^{1.0} sin( x_3)^{1.0} +
(-1*(-1*g*m_2*r_3)*(-1)*1.0) sin( x_3)^{1.0} +
-1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *1.0) ) \dot{ x_1}^{2.0} cos( x_1)^{1.0} sin( x_3)^{1.0} +
-1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *(-1)*1.0) ) \dot{ x_1}^{2.0} sin( x_1)^{1.0} cos( x_2)^{1.0} cos( x_3)^{1.0} +
-1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *1.0) ) \ddot { x_1}^{1.0} cos( x_1)^{1.0} cos( x_2)^{1.0} cos( x_3)^{1.0} +
-1*( ( (m_2 /2 *(2*r_1*r_3) *1.0) *1.0) ) \ddot { x_1}^{1.0} sin( x_1)^{1.0} sin( x_3)^{1.0} +
0 + 0 + 0 +
0 + 0 +
0 +
-1*( ( (m_2 /2 *r_3^2*2.0) *1.0) ) \ddot { x_3}^{1.0} =0
There are three equations of motion.
m1=1.0
m2=10
r1=1.0
r3=1.0
g=9.81
Then
5.5 \dot{ x_1}^{2.0} +
5.0 \dot{ x_2}^{2.0} sin( x_3)^{2.0} +
5.0 \dot{ x_3}^{2.0} +
-10.0 \dot{ x_1}^{1.0} cos( x_1)^{1.0} \dot{ x_2}^{1.0} sin( x_2)^{1.0} sin( x_3)^{1.0} +
10.0 \dot{ x_1}^{1.0} cos( x_1)^{1.0} cos( x_2)^{1.0} \dot{ x_3}^{1.0} cos( x_3)^{1.0} +
10.0 \dot{ x_1}^{1.0} sin( x_1)^{1.0} \dot{ x_3}^{1.0} sin( x_3)^{1.0} +
206.01000000000002 +
-107.91000000000001 cos( x_1)^{1.0} +
-98.10000000000001 cos( x_3)^{1.0}
If you derive the equation of motion
-107.91000000000001 sin( x_1)^{1.0} +
-11.0 \ddot { x_1}^{1.0} +
10.0 cos( x_1)^{1.0} \dot{ x_2}^{2.0} cos( x_2)^{1.0} sin( x_3)^{1.0} +
20.0 cos( x_1)^{1.0} \dot{ x_2}^{1.0} sin( x_2)^{1.0} \dot{ x_3}^{1.0} cos( x_3)^{1.0} +
10.0 cos( x_1)^{1.0} \ddot { x_2}^{1.0} sin( x_2)^{1.0} sin( x_3)^{1.0} +
-10.0 sin( x_1)^{1.0} \dot{ x_3}^{2.0} cos( x_3)^{1.0} +
10.0 cos( x_1)^{1.0} cos( x_2)^{1.0} \dot{ x_3}^{2.0} sin( x_3)^{1.0} +
-10.0 cos( x_1)^{1.0} cos( x_2)^{1.0} \ddot { x_3}^{1.0} cos( x_3)^{1.0} +
-10.0 sin( x_1)^{1.0} \ddot { x_3}^{1.0} sin( x_3)^{1.0} =0\\
-10.0 \dot{ x_1}^{2.0} sin( x_1)^{1.0} sin( x_2)^{1.0} sin( x_3)^{1.0} +
10.0 \ddot { x_1}^{1.0} cos( x_1)^{1.0} sin( x_2)^{1.0} sin( x_3)^{1.0} +
-10.0 \ddot { x_2}^{1.0} sin( x_3)^{2.0} +
-20.0 \dot{ x_2}^{1.0} \dot{ x_3}^{1.0} sin( x_3)^{1.0} cos( x_3)^{1.0} =0\\
10.0 \dot{ x_2}^{2.0} sin( x_3)^{1.0} cos( x_3)^{1.0} +
-98.10000000000001 sin( x_3)^{1.0} +
-10.0 \dot{ x_1}^{2.0} cos( x_1)^{1.0} sin( x_3)^{1.0} +
10.0 \dot{ x_1}^{2.0} sin( x_1)^{1.0} cos( x_2)^{1.0} cos( x_3)^{1.0} +
-10.0 \ddot { x_1}^{1.0} cos( x_1)^{1.0} cos( x_2)^{1.0} cos( x_3)^{1.0} +
-10.0 \ddot { x_1}^{1.0} sin( x_1)^{1.0} sin( x_3)^{1.0} +
-10.0 \ddot { x_3}^{1.0} = 0
There are three equations of motion. Let's simulate using this equation.
Simulation code (Euler method)
Position from speed $ r_ {n + 1} = r_ {n} + v {n} * DT $ From acceleration to velocity $ v_ {n + 1} = v_ {n} + a {n} * DT $ Simulate by updating.
# -*- 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
'''
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 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 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 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(a1,(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
#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=10
r1=1.0
r3=1.0
g=9.81
T1=np.array([[0,2,0,0,0,m1*r1**2 /2 + m2*r1**2/2,
0,0,0,0,0,1,
0,0,0,0,0,1]])
T2=np.array([[0,0,0,0,0,m2 /2 *r3**2 ,
0,2,0,0,0,1,
0,0,0,2,0,1]])
T3=np.array([[0,0,0,0,0,m2 /2 *r3**2 ,
0,0,0,0,0,1,
0,2,0,0,0,1]])
T4=np.array([[0,1,0,0,1,m2 /2 *(-2*r1*r3),
0,1,0,1,0,1,
0,0,0,1,0,1]])
T5=np.array([[0,1,0,0,1,m2 /2 *(2*r1*r3) ,
0,0,0,0,1,1,
0,1,0,0,1,1]])
T6=np.array([[0,1,0,1,0,m2 /2 *(2*r1*r3) ,
0,0,0,0,0,1,
0,1,0,1,0,1]])
print "T1"
T=Formula(T1,3)
T.disp2()
print "T2"
R=Formula(T2,3)
R.disp2()
T+R
print "T3"
R=Formula(T3,3)
R.disp2()
T+R
print "T4"
R=Formula(T4,3)
R.disp2()
T+R
print "T5"
R=Formula(T5,3)
R.disp2()
T+R
print "T6"
R=Formula(T6,3)
R.disp2()
T+R
print "T"
T.disp2()
U1=np.array([[0,0,0,0,0,g*(m1*r1+m2*(r1+r3)),
0,0,0,0,0,1,
0,0,0,0,0,1]])
U2=np.array([[0,0,0,0,1,-1*g*(m1*r1+m2*r1),
0,0,0,0,0,1,
0,0,0,0,0,1]])
U3=np.array([[0,0,0,0,0,-1*g*m2*r3,
0,0,0,0,0,1,
0,0,0,0,1,1]])
U = Formula(U1,3)
hoge = Formula(U2,3)
U+hoge
hoge = Formula(U3,3)
U+hoge
print "U"
U.disp2()
T1=copy.deepcopy(T)
T1+U
T-U
L=copy.deepcopy(T)
#Construction of Lagrange equation
Lt_1=copy.deepcopy(T)
Lt_2=copy.deepcopy(T)
Lt_1.partial(0,0)
Lt_2.partial(0,1)
Lt_2.diff()
Lt_1-Lt_2
Ls_1=copy.deepcopy(T)
Ls_2=copy.deepcopy(T)
Ls_1.partial(1,0)
Ls_2.partial(1,1)
Ls_1.disp2()
print "Ls_2"
Ls_2.disp2()
Ls_2.diff()
Ls_2.disp2()
Ls_1-Ls_2
Lr_1=copy.deepcopy(T)
Lr_2=copy.deepcopy(T)
Lr_1.partial(2,0)
Lr_2.partial(2,1)
Lr_2.diff()
Lr_1-Lr_2
L1t=copy.deepcopy(Lt_1)
L1t_1,L1t_2,L1t_3 = L1t.separete3()
L2t=copy.deepcopy(Ls_1)
L2t_1,L2t_2,L2t_3 = L2t.separete3()
L3t=copy.deepcopy(Lr_1)
L3t_1,L3t_2,L3t_3 = L3t.separete3()
'''
Perform numerical solution by Euler method
Initial value setting
solution
display
Need to implement 3 processes
'''
DT = 0.0001 #Times of Day
x=np.array([0.1,0,0.1,0,0.1,0]) #initial value
saveX = x
T=10
N=int(T/DT) #Calculation time/DT
saveEnergy = np.asarray([T1.subst(x)])
for i in range(N):
b1 = L1t.subst(x) * -1
b2 = L2t.subst(x) * -1
b3 = L3t.subst(x) * -1
a11 = L1t_1.subst(x)
a12 = L1t_2.subst(x)
a13 = L1t_3.subst(x)
a21 = L2t_1.subst(x)
a22 = L2t_2.subst(x)
a23 = L2t_3.subst(x)
a31 = L3t_1.subst(x)
a32 = L3t_2.subst(x)
a33 = L3t_3.subst(x)
B=np.array([b1,b2,b3])
B=B.reshape([3,1])
A=np.array([a11,a12,a13,a21,a22,a23,a31,a32,a33])
A=A.reshape([3,3])
hoge = np.linalg.det(A)
if hoge == 0:
#N=i
#print "hoge = 0 break"
Aneg = np.linalg.pinv(A)
else:
Aneg = np.linalg.inv(A)
'''
if hoge < 0.001:
N=i
print "hoge > 10000 break"
break
'''
K = np.dot(Aneg,B)
x2 =np.array( x ) + np.array( [ x[1] ,K[0][0] ,x[3], K[1][0], x[5] ,K[2][0] ] ) * DT
x = x2
saveX = np.append(saveX,x, axis=0)
Energy = T1.subst(x)
saveEnergy = np.append(saveEnergy,[Energy], axis=0)
saveX = saveX.reshape([6,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]) +r3*np.sin(saveX[4])*np.cos(saveX[2])
position_y1=r3*np.sin(saveX[4])*np.sin(saveX[2])
plt.figure(figsize=(28, 20)) # (width, height)
plt.rcParams["font.size"] = 25
plt.plot(position_x1,position_y1,label ="1")
plt.legend(fontsize = 25)
plt.savefig('Position_test.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.xlim(0,T)
#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.plot(t,saveX[4],label="x2")
plt.plot(t,saveX[5],label="dt x2")
plt.legend(fontsize = 25)
plt.savefig('Pendulum_test.png')
plt.figure(figsize=(28, 20)) # (width, height)
plt.rcParams["font.size"] = 25
plt.xlim(0,T)
#plt.plot(t,z,label ="z")
plt.plot(t,saveEnergy,label="Energy")
plt.legend(fontsize = 25)
plt.savefig('Energy_test.png')
A Lissajous curve is applied like this.
Recommended Posts