非最小坐标摆¶
在这个例子中，我们演示了在 sympy.physics.mechanics 中提供的功能，用于推导具有非最小坐标集的摆的运动方程（EOM）。由于摆是一个具有一个自由度的系统，它可以用一个坐标和一个速度（分别是摆角和角速度）来描述。选择用质量的 \(x\) 和 \(y\) 坐标来描述系统，则需要引入约束。系统如下所示：
系统将使用Kane和Lagrange的方法进行建模，并线性化得到的运动方程。虽然这是一个简单的问题，但它应该说明了在存在约束的情况下使用线性化方法的情况。
Kane 方法¶
首先，我们需要创建描述系统所需的 dynamicsymbols ，如上图所示。在这种情况下，广义坐标 \(q_1\) 和 \(q_2\) 表示质量在惯性 \(N\) 框架中的 \(x\) 和 \(y\) 坐标。同样，广义速度 \(u_1\) 和 \(u_2\) 表示在这些方向上的速度。我们还创建了一些 symbols 来表示摆的长度、质量，以及重力和时间。
>>> from sympy.physics.mechanics import *
>>> from sympy import symbols, atan, Matrix, solve
>>> # Create generalized coordinates and speeds for this non-minimal realization
>>> # q1, q2 = N.x and N.y coordinates of pendulum
>>> # u1, u2 = N.x and N.y velocities of pendulum
>>> q1, q2 = dynamicsymbols('q1:3')
>>> q1d, q2d = dynamicsymbols('q1:3', level=1)
>>> u1, u2 = dynamicsymbols('u1:3')
>>> u1d, u2d = dynamicsymbols('u1:3', level=1)
>>> L, m, g, t = symbols('L, m, g, t')
接下来，我们创建一个世界坐标系 \(N\)，以及它的原点 \(N^*\)。原点的速度设置为0。第二个坐标系 \(A\) 的方向是使其x轴沿着摆锤（如上图所示）。:
>>> # Compose world frame
>>> N = ReferenceFrame('N')
>>> pN = Point('N*')
>>> pN.set_vel(N, 0)
>>> # A.x is along the pendulum
>>> theta1 = atan(q2/q1)
>>> A = N.orientnew('A', 'axis', [theta1, N.z])
定位摆锤质量就像在世界坐标系中用其x和y坐标指定其位置一样简单。然后创建一个 Particle 对象来表示该位置的质量。:
>>> # Locate the pendulum mass
>>> P = pN.locatenew('P1', q1*N.x + q2*N.y)
>>> pP = Particle('pP', P, m)
运动学微分方程（KDEs）将广义坐标的导数与广义速度联系起来。在这种情况下，速度是导数，所以这些是简单的。还创建了一个字典来映射 \(\dot{q}\) 到 \(u\)：
>>> # Calculate the kinematic differential equations
>>> kde = Matrix([q1d - u1,
... q2d - u2])
>>> dq_dict = solve(kde, [q1d, q2d])
然后，质量的速度是位置相对于原点 \(N^*\) 的时间导数：
>>> # Set velocity of point P
>>> P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))
由于该系统具有比自由度更多的坐标，因此需要约束。配置约束将坐标相互关联。在这种情况下，约束是质量到原点的距离始终为长度 \(L`（摆的长度不变）。同样，速度约束是质量在 ``A.x`\) 方向上的速度始终为 0（无径向速度）。
>>> f_c = Matrix([P.pos_from(pN).magnitude() - L])
>>> f_v = Matrix([P.vel(N).express(A).dot(A.x)])
>>> f_v.simplify()
系统上的力仅仅是重力，在点 P 处。:
>>> # Input the force resultant at P
>>> R = m*g*N.x
在问题设定中，可以使用 KanesMethod 类生成运动方程。由于存在约束，需要向该类提供依赖和独立的坐标。在这种情况下，我们将使用 \(q_2\) 和 \(u_2\) 作为独立坐标和速度：
>>> # Derive the equations of motion using the KanesMethod class.
>>> KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1],
... u_dependent=[u1], configuration_constraints=f_c,
... velocity_constraints=f_v, kd_eqs=kde)
>>> (fr, frstar) = KM.kanes_equations([pP],[(P, R)])
对于线性化，操作点可以在调用时指定，或者之后进行替换。在这种情况下，我们将在调用时以列表形式提供它们。A_and_B=True 关键字参数指示求解 \(M\) 矩阵的逆，并仅求解显式线性化的 \(A\) 和 \(B\) 矩阵。simplify=True 关键字参数指示在线性化调用内部进行简化，并返回预简化的矩阵。对于简单系统，这样做成本很小，但对于较大的系统，这可能是一个代价高昂的操作，应避免。:
>>> # Set the operating point to be straight down, and non-moving
>>> q_op = {q1: L, q2: 0}
>>> u_op = {u1: 0, u2: 0}
>>> ud_op = {u1d: 0, u2d: 0}
>>> # Perform the linearization
>>> A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True,
... new_method=True, simplify=True)
>>> A
Matrix([
[ 0, 1],
[-g/L, 0]])
>>> B
Matrix(0, 0, [])
生成的 \(A\) 矩阵的维度为 2 x 2，而总状态数为 len(q) + len(u) = 2 + 2 = 4。这是因为对于受约束的系统，生成的 A_and_B 形式的状态向量仅包含独立坐标和速度。用数学方式表示，关于此点的线性化系统可以写成：
拉格朗日方法¶
使用拉格朗日方法的推导与上述使用凯恩方法的推导非常相似。与之前一样，我们首先创建描述系统所需的``dynamicsymbols``。在这种情况下，广义坐标 \(q_1\) 和 \(q_2\) 表示质量在惯性 \(N\) 框架中的 \(x\) 和 \(y\) 坐标。这导致时间导数 \(\dot{q_1}\) 和 \(\dot{q_2}\) 表示在这些方向上的速度。我们还创建了一些``symbols``来表示摆的长度、质量、重力以及时间。:
>>> from sympy.physics.mechanics import *
>>> from sympy import symbols, atan, Matrix
>>> q1, q2 = dynamicsymbols('q1:3')
>>> q1d, q2d = dynamicsymbols('q1:3', level=1)
>>> L, m, g, t = symbols('L, m, g, t')
接下来，我们创建一个世界坐标系 \(N\)，以及它的原点 \(N^*\)。原点的速度设置为0。第二个坐标系 \(A\) 的方向是使其x轴沿着摆锤（如上图所示）。:
>>> # Compose World Frame
>>> N = ReferenceFrame('N')
>>> pN = Point('N*')
>>> pN.set_vel(N, 0)
>>> # A.x is along the pendulum
>>> theta1 = atan(q2/q1)
>>> A = N.orientnew('A', 'axis', [theta1, N.z])
定位摆锤质量就像在世界坐标系中用其x和y坐标指定其位置一样简单。然后创建一个 Particle 对象来表示该位置的质量。:
>>> # Create point P, the pendulum mass
>>> P = pN.locatenew('P1', q1*N.x + q2*N.y)
>>> P.set_vel(N, P.pos_from(pN).dt(N))
>>> pP = Particle('pP', P, m)
由于该系统具有比自由度更多的坐标，因此需要约束。在这种情况下，只需要一个完整约束：从原点到质量的距离始终是长度 `L`（摆的长度不变）。
>>> # Holonomic Constraint Equations
>>> f_c = Matrix([q1**2 + q2**2 - L**2])
系统上的力仅仅是重力，在点 P 处。:
>>> # Input the force resultant at P
>>> R = m*g*N.x
在问题设定下，可以计算拉格朗日量，并形成运动方程。注意，对 LagrangesMethod 的调用包括拉格朗日量、广义坐标、约束（由 hol_coneqs 或 nonhol_coneqs 指定）、（体，力）对的列表以及惯性参考系。与 KanesMethod 初始化器相比，独立和依赖坐标在 LagrangesMethod 对象内部没有被划分。这种划分在之后提供。
>>> # Calculate the lagrangian, and form the equations of motion
>>> Lag = Lagrangian(N, pP)
>>> LM = LagrangesMethod(Lag, [q1, q2], hol_coneqs=f_c, forcelist=[(P, R)], frame=N)
>>> lag_eqs = LM.form_lagranges_equations()
接下来，我们编写操作点字典，设置在静止悬挂位置：
>>> # Compose operating point
>>> op_point = {q1: L, q2: 0, q1d: 0, q2d: 0, q1d.diff(t): 0, q2d.diff(t): 0}
由于公式中存在约束条件，因此会有相应的拉格朗日乘数。这些乘数也可能出现在线性化形式中，因此也应包含在操作点字典中。幸运的是，LagrangesMethod 类提供了一种使用 solve_multipliers 方法在给定操作点处轻松求解乘数的简便方法。:
>>> # Solve for multiplier operating point
>>> lam_op = LM.solve_multipliers(op_point=op_point)
通过此解决方案，线性化可以完成。请注意，与 KanesMethod 方法相比，LagrangesMethod.linearize 方法还需要将广义坐标及其时间导数划分为独立向量和依赖向量。这与上面传递给 KanesMethod 构造函数的内容相同：
>>> op_point.update(lam_op)
>>> # Perform the Linearization
>>> A, B, inp_vec = LM.linearize([q2], [q2d], [q1], [q1d],
... op_point=op_point, A_and_B=True)
>>> A
Matrix([
[ 0, 1],
[-g/L, 0]])
>>> B
Matrix(0, 0, [])
生成的 \(A\) 矩阵的维度为 2 x 2，而总状态数为 2*len(q) = 4。这是因为对于受约束的系统，生成的 A_and_B 形式的状态向量仅包含独立坐标及其导数。用数学方式写出，关于此点的线性化系统可以写成：