Answer the question
In order to leave comments, you need to log in
How to write a system of equations correctly?
Compiled a function that will need to be integrated:
from scipy.integrate import odeint, quad
from math import sin, cos, tan
import numpy as np
import matplotlib.pyplot as mpl
def oman(x, t):
J = np.array([1400, 830, 730])
My = np.array([0, 0, 0])
Mv = np.array([0, 0, 0])
M = My + Mv
x = np.zeros((6))
x[0] = (M[0] - (J[2]-J[1])*x[1]*x[2])/J[0]
x[1] = (M[1] - (J[0]-J[2])*x[0]*x[2])/J[1]
x[2] = (M[2] - (J[1]-J[0])*x[0]*x[1])/J[2]
x[3] = x[0] - tan(x[4])*sin(x[3])*x[1] + tan(x[4])*cos(x[3])*x[2]
x[4] = x[1] * cos(x[3]) + x[2]*sin(x[3])
x[5] = x[1] * (-sin(x[3])/cos(x[4])) + x[2]*(cos(x[3])/cos(x[4]))
# Константы
pan = np.array([1, 1, 1])
pom = np.array([0, 0, 0])
PSig = np.array([1, 1, 1])
K = np.array([3, 3, 3])
Ki = np.array([1, 1, 1])
Ks = np.array([0, 0, 0])
T = 0.6
L = 1
pp = np.zeros((4))
pp[0] = cos(pan[2]/2) * cos(pan[1]/2) * cos(pan[0]/2) +\
sin(pan[2]/2) * sin(pan[1]/2) * sin(pan[0]/2)
pp[1] = cos(pan[2]/2) * cos(pan[1]/2) * sin(pan[0]/2) -\
sin(pan[2]/2) * sin(pan[1]/2) * cos(pan[0]/2)
pp[2] = cos(pan[2]/2) * sin(pan[1]/2) * cos(pan[0]/2) +\
sin(pan[2]/2) * cos(pan[1]/2) * sin(pan[0]/2)
pp[3] = - cos(pan[2]/2) * sin(pan[1]/2) * sin(pan[0]/2) +\
sin(pan[2]/2) * cos(pan[1]/2) * cos(pan[0]/2)
dphi = x[0:3] * 0.1
p = np.zeros((4))
dp = np.zeros((4))
dp[0] = (-dphi[0] * p[1] - dphi[1] * p[2] - dphi[2] * p[3])/2
dp[1] = ( dphi[0] * p[0] + dphi[2] * p[2] - dphi[1] * p[3])/2
dp[2] = ( dphi[1] * p[0] - dphi[2] * p[1] + dphi[0] * p[3])/2
dp[3] = ( dphi[2] * p[0] + dphi[1] * p[1] - dphi[0] * p[1])/2
p = p + dp
pc = np.zeros((4))
if p[0] > 0:
pc[1] = p[0]
pc[1] = -p[1]
pc[2] = -p[2]
pc[3] = -p[3]
else:
pc[0] = -p[0]
pc[1] = p[1]
pc[2] = p[2]
pc[3] = p[3]
R = np.zeros((4))
R[0] = pc[0] * pp[0] - pc[1] * pp[1] - pc[2] * pp[2] - pc[3] * pp[3]
R[1] = pc[0] * pp[1] - pc[1] * pp[0] - pc[2] * pp[3] - pc[3] * pp[2]
R[2] = pc[0] * pp[2] - pc[2] * pp[0] - pc[1] * pp[3] - pc[3] * pp[1]
R[3] = pc[0] * pp[3] - pc[3] * pp[0] - pc[1] * pp[2] - pc[2] * pp[1]
ddphi = dphi * t
Sig = np.zeros((3))
Sig[0] = K[0]*(-2*R[0]*R[1]) + Ki[0]*(x[0]*pom[0]) + Ks[0]*(ddphi[0])
Sig[1] = K[1]*(-2*R[0]*R[2]) + Ki[1]*(x[1]*pom[1]) + Ks[1]*(ddphi[1])
Sig[2] = K[2]*(-2*R[0]*R[3]) + Ki[2]*(x[2]*pom[2]) + Ks[2]*(ddphi[2])
if abs(Sig[0]) >= PSig[0]:
My[0] = 1.2 * sign(Sig[0])
else:
My[0] = 0
if abs(Sig[1]) >= PSig[1]:
My[1] = 1.2 * sign(Sig[1])
else:
My[1] = 0
if abs(Sig[2]) >= PSig[2]:
My[2] = 1.2 * sign(Sig[2])
else:
My[2] = 0
return x
x0 = np.array([1, 1, 1,
0, 0, 0])
t = np.linspace(0, 10, 1000)
y = odeint(oman, x0, t)
Answer the question
In order to leave comments, you need to log in
Didn't find what you were looking for?
Ask your questionAsk a Question
731 491 924 answers to any question