平面运动副和空间运动副复合运动（Python）

假设有一个机械系统，其中包含一个平面四杆机构（具有转动副和移动副等平面运动副），同时该平面四杆机构的某一个构件又连接到一个空间的球铰（空间运动副），并通过这个球铰连接到一个空间的滑块。已知平面四杆机构的各杆长度、初始位置以及空间部分的相关参数。要求计算整个系统在给定输入条件下（如平面四杆机构某一杆件的转动角度），各构件的位置和速度，并考虑运动副的约束关系。
具体参数如下：
平面四杆机构：
杆 1 长度 \(l_1 = 0.5\) m
杆 2 长度 \(l_2 = 1.0\) m
杆 3 长度 \(l_3 = 0.8\) m
杆 4 长度 \(l_4 = 1.2\) m
初始时杆 1 与水平方向夹角 \(\theta_1 = 0\) 弧度
输入：杆 1 的转动角速度 \(\omega_1 = 2\) rad/s
空间部分：
球铰连接在杆 3 的末端，球铰到空间滑块的距离 \(d = 0.5\) m
空间滑块只能在沿 z 轴方向移动。

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from matplotlib.widgets import Button

# 设置支持中文的字体
plt.rcParams['font.family'] = 'SimHei'
plt.rcParams['axes.unic