Python 数值求解投石机刚性（奇异）运动微分方程组

所以我一直在研究投石机的数学，并试图模拟一个，以优化投石机的性能。我知道以前也做过，但我想自己尝试一下。因此，在阅读了Donald B.Siano（）的文章“投石机力学”之后，我坐下来进行了wrt计算。Siano简单地让Fortran为他做的Euler-Lagrange方程。我已将我的答案与文章中的答案进行了核对，事实上它们是一致的

现在问题来了。每当我尝试用python数值求解这些方程时，我都会得到能量不是常数的无意义解。这在能量图和我的模拟中都很明显，你们会注意到运动中的“跳跃”和运动变慢的事实。我得出的结论是，系统相当僵硬，因为每当φ和psi接近π时，我除以的因子eff I就会变得非常小。但因为我的梁惯性不等于0，这不是奇点，也不会引起问题

你如何才能比我更好地解决这些问题？甚至有可能用mb来解它们，因此Ib=0，这将使phi=psi=pi成为一个奇点，就像上面提到的文章中所做的那样？我在下面包含了我的代码

import numpy as np
import scipy as sc
from matplotlib import pyplot as plt
from numpy import sin, cos
from scipy import integrate
import matplotlib.animation as animation

# Differential equations

def trebuchet_model(t, r0, *param):
    dtheta, theta, dphi, phi, dpsi, psi = r0
    l1, l2, l3, l4, l5, m1, m2, mb, d, Ib, g = param

    def gen_t(m, d1, d2, ang, dang):
        return m * d1 * sin(ang) * (g * cos(theta + ang) - d2 * (dtheta + dang) ** 2 + d1 * dtheta ** 2 * cos(ang))

    eff_I = Ib + m1 * (l1 * sin(phi)) ** 2 + m2 * (l2 * sin(psi)) ** 2
    eff_t = grav_t + gen_t(m1, l1, l4, phi, dphi) + gen_t(m2, -l2, -l3, psi, dpsi)
    grav_t = mb * g * d * sin(theta)

    ddtheta = eff_t / eff_I

    def ddang(d1, d2, ang):
        return (d1 * dtheta ** 2 * sin(ang) + (d1 * cos(ang) - d2) * ddtheta + g * sin(theta + ang)) / d2

    ddphi = ddang(l1, l4, phi)
    ddpsi = ddang(-l2, -l3, psi)

    return [ddtheta, dtheta, ddphi, dphi, ddpsi, dpsi]


# Solve the DEs using Scipy integrate module

l1, l2, l3, l4, l5, m1, m2, mb, g = 1, 3.75, 3.75, 1, 0, 133, 1, 3, 10
d, Ib = (l2 - l1) / 2, mb * (l1 ** 2 + l2 ** 2 - l1 * l2) / 3,
param = [l1, l2, l3, l4, l5, m1, m2, mb, d, Ib, g]
init_state = [0, 3 * np.pi / 4, 0, np.pi / 4, 0, -np.pi / 4]

times = np.linspace(0, 5, 500)
solution = sc.integrate.solve_ivp(lambda t, r0: trebuchet_model(t, r0, *param), (0, 5), init_state, t_eval=times, method="Radau")

# Compute Cartesian coordinates and system energy

dtheta, theta, dphi, phi, dpsi, psi = solution.y
ddtheta, ddphi, ddpsi = np.array(trebuchet_model(0, solution.y, *param))[np.array([0, 2, 4]), :]

def coords(d1, d2, theta, ang):
    x = d1 * sin(theta) - d2 * sin(theta + ang)
    y = - d1 * cos(theta) + d2 * cos(theta + ang)
    return (x, y)

x1, y1 = coords(l1, l4, theta, phi)
x1p, y1p = l1 * sin(theta), - l1 * cos(theta)
x2, y2 = coords(-l2, -l3, theta, psi)
x2p, y2p = - l2 * sin(theta), l2 * cos(theta)

def speed_sq(d1, d2, dtheta, dang, ang):
    return (d1 * dtheta) ** 2 + (d2 * (dtheta + dang)) ** 2 - 2 * d1 * d2 * dtheta * (dtheta + dang) * cos(ang)

v1sq = speed_sq(l1, l4, dtheta, dphi, phi)
v2sq = speed_sq(-l2, -l3, dtheta, dpsi, psi)
T = (m1 * v1sq + m2 * v2sq + Ib * dtheta ** 2) / 2
U = g * (m1 * y1 + m2 * y2 + mb * d * cos(theta))
H = T + U

# Plot energy and animate trebuchet

energy_fig = plt.figure("energy")
plt.plot(times, H, 'k-')

treb_fig = plt.figure("trebuchet")
ax = plt.axes(xlim=(-8, 8), ylim=(-6, 8))

ax.plot([0], [0], 'ko', markersize=2)
ax.plot([0, 0], [0, -5], 'k-')
swingarm, = ax.plot([], [], 'k-')
pivot_points, = ax.plot([], [], 'ko', markersize=2)
cw_arm, = ax.plot([], [], 'k-')
cw, = ax.plot([], 'ko', markersize=20)
sling, = ax.plot([], [], 'k-')
projectile, = ax.plot([], [], 'ko', markersize=8)
projectile_path, = ax.plot([], [], '--', color="gray")

def update(i):
    projectile_path.set_data([x2[:i]], [y2[:i]])
    swingarm.set_data([x1p[i], x2p[i]], [y1p[i], y2p[i]])
    pivot_points.set_data([x1p[i], x2p[i]], [y1p[i], y2p[i]])
    cw_arm.set_data([x1p[i], x1[i]], [y1p[i], y1[i]])
    cw.set_data([x1[i]], [y1[i]])
    sling.set_data([x2p[i], x2[i]], [y2p[i], y2[i]])
    projectile.set_data([x2[i]], [y2[i]])

anim = animation.FuncAnimation(treb_fig, update, 500, interval=10)
plt.show()

---

错误：赋值前引用的局部变量'grav_t'

（在

eff_t=grav_t+gen_t（m1，l1，

）中，也没有在别处定义。

错误：赋值前引用的局部变量'grav_t'（在
eff_t=grav_t+gen_t（m1，l1，
）中，也没有在别处定义。