四杆连杆机构

四杆连杆是力学中常用的例子，可以用两个完整约束来描述。这个例子将使用 sympy.physics.mechanics 提供的关节功能。简而言之，我们将使用刚体和关节来定义开环系统。接下来，我们定义配置约束以闭合环路。System 将用于对整个系统进行“记录”，而 KanesMethod 作为后端。

Link3 Link2 Link4 Degree of Freedom (DoF) Active constraint Constrained DoF Moving body Fixed body link1 q1, u1 q3, u3 q2, u2

首先，我们需要创建描述系统所需的 dynamicsymbols() ，如上图所示。在这种情况下，广义坐标 \(q_1\)、\(q_2\) 和 \(q_3\) 表示连杆之间的角度。同样，广义速度 \(u_1\)、\(u_2\) 和 \(u_3\) 表示连杆之间的角速度。我们还创建了一些 symbols() 来表示连杆的长度和密度。

>>> from sympy import Matrix, linear_eq_to_matrix, pi, simplify, symbols
>>> from sympy.physics.mechanics import *
>>> mechanics_printing(pretty_print=False)
>>> q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1:4, u1:4')
>>> l1, l2, l3, l4, rho = symbols('l1:5, rho')

定义了所有符号后，我们现在可以定义主体并初始化我们的 System 实例。:

>>> N = ReferenceFrame('N')
>>> mass_centers = [Point(f'mc{i}') for i in range(1, 5)]
>>> inertias = [Inertia.from_inertia_scalars(P, N, 0, 0, rho*l**3/12)
...             for P, l in zip(mass_centers, (l1, l2, l3, l4))]
>>> link1 = RigidBody('Link1', frame=N, mass=rho*l1,
...                   masscenter=mass_centers[0], inertia=inertias[0])
>>> link2 = RigidBody('Link2', mass=rho*l2, masscenter=mass_centers[1],
...                   inertia=inertias[1])
>>> link3 = RigidBody('Link3', mass=rho*l3, masscenter=mass_centers[2],
...                   inertia=inertias[2])
>>> link4 = RigidBody('Link4', mass=rho*l4, masscenter=mass_centers[3],
...                   inertia=inertias[3])
>>> system = System.from_newtonian(link1)

接下来，我们还定义了前三个关节，它们构成了开环摆，并将它们添加到系统中。:

>>> joint1 = PinJoint('J1', link1, link2, coordinates=q1, speeds=u1,
...                   parent_point=l1/2*link1.x,
...                   child_point=-l2/2*link2.x, joint_axis=link1.z)
>>> joint2 = PinJoint('J2', link2, link3, coordinates=q2, speeds=u2,
...                   parent_point=l2/2*link2.x,
...                   child_point=-l3/2*link3.x, joint_axis=link2.z)
>>> joint3 = PinJoint('J3', link3, link4, coordinates=q3, speeds=u3,
...                   parent_point=l3/2*link3.x,
...                   child_point=-l4/2*link4.x, joint_axis=link3.z)
>>> system.add_joints(joint1, joint2, joint3)

现在我们可以制定一个将闭合运动学环的完整约束。:

>>> start_point = link1.masscenter.locatenew('start_point', -l1/2*link1.x)
>>> end_point = link4.masscenter.locatenew('end_point', l4/2*link4.x)
>>> loop = end_point.pos_from(start_point)
>>> system.add_holonomic_constraints(loop.dot(link1.x), loop.dot(link1.y))

在生成运动方程之前，我们需要指定哪些广义坐标和速度是独立的，哪些是依赖的。之后，我们可以运行 validate_system() 来进行一些基本的连贯性检查。:

>>> system.q_ind = [q1]
>>> system.u_ind = [u1]
>>> system.q_dep = [q2, q3]
>>> system.u_dep = [u2, u3]
>>> system.validate_system()

由于我们已经准备好了整个系统，我们现在可以使用 KanesMethod 作为后端来形成运动方程。:

>>> simplify(system.form_eoms())
 Matrix([[l2*rho*(-2*l2**2*sin(q3)*u1' + 3*l2*l3*u1**2*sin(q2 + q3)*sin(q2) + 3*l2*l3*sin(q2)*cos(q2 + q3)*u1' - 3*l2*l3*sin(q3)*u1' + 3*l2*l4*u1**2*sin(q2 + q3)*sin(q2) + 3*l2*l4*sin(q2)*cos(q2 + q3)*u1' + 3*l3**2*u1**2*sin(q2)*sin(q3) + 6*l3**2*u1*u2*sin(q2)*sin(q3) + 3*l3**2*u2**2*sin(q2)*sin(q3) + 2*l3**2*sin(q2)*cos(q3)*u1' + 2*l3**2*sin(q2)*cos(q3)*u2' - l3**2*sin(q3)*cos(q2)*u1' - l3**2*sin(q3)*cos(q2)*u2' + 3*l3*l4*u1**2*sin(q2)*sin(q3) + 6*l3*l4*u1*u2*sin(q2)*sin(q3) + 3*l3*l4*u2**2*sin(q2)*sin(q3) + 3*l3*l4*sin(q2)*cos(q3)*u1' + 3*l3*l4*sin(q2)*cos(q3)*u2' + l4**2*sin(q2)*u1' + l4**2*sin(q2)*u2' + l4**2*sin(q2)*u3')/(6*sin(q3))]])

揭示非贡献力

为了揭示闭合关节处的非贡献力，我们必须在端点处引入x和y方向的辅助速度。

>>> uaux1, uaux2 = dynamicsymbols('uaux1:3')
>>> end_point_aux = end_point.locatenew('end_point_aux', 0)
>>> end_point_aux.set_vel(N, end_point.vel(N) + uaux1*N.x + uaux2*N.y)

为了确保速度包含在速度约束中，我们必须手动覆盖速度约束，因为这些约束默认指定为完整约束的时间导数。

>>> system.velocity_constraints = [
...    end_point_aux.vel(N).dot(N.x), end_point_aux.vel(N).dot(N.y)]

在添加非贡献力时，我们需要它们仅依赖于辅助速度，而不是由约束消除的速度。这可以通过对非辅助端点施加相等且相反的力来实现。

>>> faux1, faux2 = dynamicsymbols('faux1:3')
>>> noncontributing_forces = [
...   Force(end_point_aux, faux1*N.x + faux2*N.y),
...   Force(end_point, -(faux1*N.x + faux2*N.y)),
... ]

或者，我们可以指定一个新的点，该点已经减去了由约束消除的速度。

>>> end_point_forces = end_point.locatenew('end_point_forces', 0)
>>> end_point_forces.set_vel(N, uaux1*N.x + uaux2*N.y)
>>> noncontributing_forces = [Force(end_point_forces, faux1*N.x + faux2*N.y)]

接下来，我们可以将辅助速度和非贡献力添加到系统中。

>>> system.add_loads(*noncontributing_forces)
>>> system.u_aux = [uaux1, uaux2]

要包含重力，我们可以在验证系统并形成运动方程之前使用 apply_uniform_gravity()。

>>> g = symbols('g')
>>> system.apply_uniform_gravity(-g*N.y)
>>> system.validate_system()
>>> eoms = system.form_eoms()

有了运动方程，我们可以求解非贡献力的辅助方程，并计算其在简单配置下的值。

>>> auxiliary_eqs = system.eom_method.auxiliary_eqs
>>> forces_eqs = Matrix.LUsolve(
...   *linear_eq_to_matrix(auxiliary_eqs, [faux1, faux2]))
>>> subs = {
...   l1: 2, l2: 1, l3: 2, l4: 1,
...   rho: 5, g: 9.81,
...   q1: pi/2, q2: pi/2, q3: pi/2,
...   u1: 0, u2: 0, u3: 0, u1.diff(): 0, u2.diff(): 0, u3.diff(): 0,
... }
>>> forces_eqs.xreplace(subs)
Matrix([
[    0],
[-98.1]])