# Multi Degree of Freedom Holonomic System¶

In this example we demonstrate the use of the functionality provided in sympy.physics.mechanics for deriving the equations of motion (EOM) of a holonomic system that includes both particles and rigid bodies with contributing forces and torques, some of which are specified forces and torques. The system is shown below:

The system will be modeled using System. First we need to create the dynamicsymbols() needed to describe the system as shown in the above diagram. In this case, the generalized coordinates $$q_1$$ represent lateral distance of block from wall, $$q_2$$ represents angle of the compound pendulum from vertical, $$q_3$$ represents angle of the simple pendulum from the compound pendulum. The generalized speeds $$u_1$$ represents lateral speed of block, $$u_2$$ represents lateral speed of compound pendulum and $$u_3$$ represents angular speed of C relative to B.

We also create some symbols() to represent the length and mass of the pendulum, as well as gravity and others.

>>> from sympy import zeros, symbols
>>> from sympy.physics.mechanics import *
>>> q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1, q2, q3, u1, u2, u3')
>>> F, T = dynamicsymbols('F, T')
>>> l, k, c, g, kT = symbols('l, k, c, g, kT')
>>> ma, mb, mc, IBzz= symbols('ma, mb, mc, IBzz')


With all symbols defined, we can now define the bodies and initialize our instance of System.

>>> wall = RigidBody('N')
>>> block = Particle('A', mass=ma)
>>> compound_pend = RigidBody('B', mass=mb)
>>> compound_pend.central_inertia = inertia(compound_pend.frame, 0, 0, IBzz)
>>> simple_pend = Particle('C', mass=mc)
>>> system = System.from_newtonian(wall)
>>> system.add_bodies(block, compound_pend, simple_pend)


Next, we connect the bodies using joints to establish the kinematics. Note that we specify the intermediate frames for both particles, as particles do not have an associated frame.

>>> block_frame = ReferenceFrame('A')
>>> block.masscenter.set_vel(block_frame, 0)
>>> slider = PrismaticJoint('J1', wall, block, coordinates=q1, speeds=u1,
...                         child_interframe=block_frame)
>>> rev1 = PinJoint('J2', block, compound_pend, coordinates=q2, speeds=u2,
...                 joint_axis=wall.z, child_point=l*2/3*compound_pend.y,
...                 parent_interframe=block_frame)
>>> simple_pend_frame = ReferenceFrame('C')
>>> simple_pend.masscenter.set_vel(simple_pend_frame, 0)
>>> rev2 = PinJoint('J3', compound_pend, simple_pend, coordinates=q3,
...                 speeds=u3, joint_axis=compound_pend.z,
...                 parent_point=-l/3*compound_pend.y,
...                 child_point=l*simple_pend_frame.y,
...                 child_interframe=simple_pend_frame)

>>> system.add_joints(slider, rev1, rev2)


Now we can apply loads (forces and torques) to the bodies, gravity acts on all bodies, a linear spring and damper act on block and wall, a rotational linear spring acts on C relative to B specified torque T acts on compound_pend and block, specified force F acts on block.

>>> system.apply_uniform_gravity(-g * wall.y)
>>> spring_damper_path = LinearPathway(wall.masscenter, block.masscenter)
...     LinearSpring(k, spring_damper_path),
...     LinearDamper(c, spring_damper_path),
...     TorqueActuator(T, wall.z, compound_pend, wall),
...     TorqueActuator(kT * q3, wall.z, compound_pend, simple_pend_frame),
... )


With the system setup, we can now form the equations of motion with KanesMethod in the backend.

>>> system.form_eoms(explicit_kinematics=True)
Matrix([
[                                -c*u1(t) - k*q1(t) + 2*l*mb*u2(t)**2*sin(q2(t))/3 - l*mc*(-sin(q2(t))*sin(q3(t)) + cos(q2(t))*cos(q3(t)))*Derivative(u3(t), t) - l*mc*(-sin(q2(t))*cos(q3(t)) - sin(q3(t))*cos(q2(t)))*(u2(t) + u3(t))**2 + l*mc*u2(t)**2*sin(q2(t)) - (2*l*mb*cos(q2(t))/3 + mc*(l*(-sin(q2(t))*sin(q3(t)) + cos(q2(t))*cos(q3(t))) + l*cos(q2(t))))*Derivative(u2(t), t) - (ma + mb + mc)*Derivative(u1(t), t) + F(t)],
[-2*g*l*mb*sin(q2(t))/3 - g*l*mc*(sin(q2(t))*cos(q3(t)) + sin(q3(t))*cos(q2(t))) - g*l*mc*sin(q2(t)) + l**2*mc*(u2(t) + u3(t))**2*sin(q3(t)) - l**2*mc*u2(t)**2*sin(q3(t)) - mc*(l**2*cos(q3(t)) + l**2)*Derivative(u3(t), t) - (2*l*mb*cos(q2(t))/3 + mc*(l*(-sin(q2(t))*sin(q3(t)) + cos(q2(t))*cos(q3(t))) + l*cos(q2(t))))*Derivative(u1(t), t) - (IBzz + 4*l**2*mb/9 + mc*(2*l**2*cos(q3(t)) + 2*l**2))*Derivative(u2(t), t) + T(t)],
[                                                                                                                                                                        -g*l*mc*(sin(q2(t))*cos(q3(t)) + sin(q3(t))*cos(q2(t))) - kT*q3(t) - l**2*mc*u2(t)**2*sin(q3(t)) - l**2*mc*Derivative(u3(t), t) - l*mc*(-sin(q2(t))*sin(q3(t)) + cos(q2(t))*cos(q3(t)))*Derivative(u1(t), t) - mc*(l**2*cos(q3(t)) + l**2)*Derivative(u2(t), t)]])

>>> system.mass_matrix_full
Matrix([
[1, 0, 0,                                                                                            0,                                                                                            0,                                                     0],
[0, 1, 0,                                                                                            0,                                                                                            0,                                                     0],
[0, 0, 1,                                                                                            0,                                                                                            0,                                                     0],
[0, 0, 0,                                                                                 ma + mb + mc, 2*l*mb*cos(q2(t))/3 + mc*(l*(-sin(q2(t))*sin(q3(t)) + cos(q2(t))*cos(q3(t))) + l*cos(q2(t))), l*mc*(-sin(q2(t))*sin(q3(t)) + cos(q2(t))*cos(q3(t)))],
[0, 0, 0, 2*l*mb*cos(q2(t))/3 + mc*(l*(-sin(q2(t))*sin(q3(t)) + cos(q2(t))*cos(q3(t))) + l*cos(q2(t))),                                         IBzz + 4*l**2*mb/9 + mc*(2*l**2*cos(q3(t)) + 2*l**2),                           mc*(l**2*cos(q3(t)) + l**2)],
[0, 0, 0,                                        l*mc*(-sin(q2(t))*sin(q3(t)) + cos(q2(t))*cos(q3(t))),                                                                  mc*(l**2*cos(q3(t)) + l**2),                                               l**2*mc]])

>>> system.forcing_full
Matrix([
[                                                                                                                                                                           u1(t)],
[                                                                                                                                                                           u2(t)],
[                                                                                                                                                                           u3(t)],
[                  -c*u1(t) - k*q1(t) + 2*l*mb*u2(t)**2*sin(q2(t))/3 - l*mc*(-sin(q2(t))*cos(q3(t)) - sin(q3(t))*cos(q2(t)))*(u2(t) + u3(t))**2 + l*mc*u2(t)**2*sin(q2(t)) + F(t)],
[-2*g*l*mb*sin(q2(t))/3 - g*l*mc*(sin(q2(t))*cos(q3(t)) + sin(q3(t))*cos(q2(t))) - g*l*mc*sin(q2(t)) + l**2*mc*(u2(t) + u3(t))**2*sin(q3(t)) - l**2*mc*u2(t)**2*sin(q3(t)) + T(t)],
[                                                                                -g*l*mc*(sin(q2(t))*cos(q3(t)) + sin(q3(t))*cos(q2(t))) - kT*q3(t) - l**2*mc*u2(t)**2*sin(q3(t))]])