I
I
Igor2015-11-27 00:11:36
Python
Igor, 2015-11-27 00:11:36

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)

There was a problem with the output of the results - nothing changes over time.
What did you do wrong and how to fix it?

Answer the question

In order to leave comments, you need to log in

Didn't find what you were looking for?

Ask your question

Ask a Question

731 491 924 answers to any question