python 中复微分方程的 Runge-Kutta 4 方法

问：

我正在尝试使用 python 中的 RK4- 方法求解复杂系统的运动方程，但我对这种方法不是很熟悉，需要帮助来修复我的代码。微分方程为：

等式的第一部分：

方程的下一部分：

我写的代码是：

import numpy as np

p = 7.850
A = (0.01) ** 2
m1 = 1
m3 = 1
g = 9.81

r2 = 1

r3 = 1

r1 = 1

rcm = r1

r = 4 * r2

m2 = 1


def equations(t,y):
    theta, phi1, phi2, x1, w1, p1 = y

    f0 = x1
    f1 = w1
    f2 = p1  # Corrected the variable name to p1

    # Calculate f4 and f5 separately
    f4 = ((m1 * r2 * rcm * x1**2 * np.sin(theta + phi1)) - (m3 * r3 * (r-r2) * p1**2 * np.sin(phi2 - phi1)) - (g * m1 * r2 * np.sin(phi1)) + (g * m2 * ((r/2)-r2) * np.sin(phi1)) + (g * m3 * (r-r2) * np.sin(phi1)) - (m1 * r2 * rcm * np.cos(theta + phi1) * f3) + (m3 * r3 * (r-r2) * np.cos(phi2-phi1) * f5))/((m1 * r2**2))
    f5 = ((m3 * r3 * (r-r2) * np.sin(phi2 - phi1) * w1**2) + (g * m3 * r3 * np.sin(phi2)) + (m3 * r3 * (r-r2) * np.cos(phi2-phi1)))/(m3 * r3**2)

    # Calculate f3 using the previously calculated f4 and f5
    f3 = ((m1 * r2 * rcm * w1**2 * np.sin(theta + phi1)) + (g * m1 * rcm * np.sin(theta)) - (m1 * r2 * rcm * np.cos(theta + phi1) * f4))/(m1 * r2**2)
    theta1, phi1, phi2, x1, w1, p1 = y
    return np.array([f0, f1, f2, f3, f4, f5])


def RK4(t, y, h, f):
    theta, phi1, phi2, x1, w1, p1 = y
    # Runge Kutta standard calculations
    k1 = f(t, y)
    k2 = f(t + h/2, y + h/2 * k1)
    k3 = f(t + h/2, y + h/2 * k2)
    k4 = f(t + h, y + h * k3)

    return 1/6 * (k1 + 2 * k2 + 2 * k3 + k4)

def main():
    
     # Specify time interval and number of domain subintervals
    t0 = 0
    T = 100
    n = 10000
    # initial conditions
    y0 = [np.pi - np.arccos((2 - 1.5) / r2), np.arccos((2 - 1.5) / r2), np.arcsin(1.5/(r-r2)), 0, 0, 0]

    # Domain discretization
    t_n = np.linspace(t0, T, n)
    y_n = [np.array(y0)]

    # Step size
    h = (T - t0) / n

    while t_n[-1] < T:
        # Keep going until T is reached.
        # Store numerical solutions into y_n
        y_n.append(y_n[-1] + h * RK4(t_n[-1], y_n[-1], h, equations))
        t_n = np.append(t_n, t_n[-1] + h)

    print(y_n)

if __name__ == "__main__":
    main()

运行此代码，我收到输出：

[array([2.0943951 , 1.04719755, 0.52359878, 0.        , 0.        ,
       0.        ])]

这看起来像是返回初始条件，这意味着系统保持不变，这不是我想要的结果。任何帮助和/或替代方法将不胜感激，因为我也想模拟系统。先谢谢你。