A four bar linkage#
The four bar linkage is a common example used in mechanics, which can be
formulated with only two holonomic constraints. This example will make use of
joints functionality provided in sympy.physics.mechanics
. In summary we
will use bodies and joints to define the open loop system. Next, we define the
configuration constraints to close the loop. The JointsMethod
will be used
to do the “book-keeping” of the open-loop system. From this we will get the
input used in combination with the constraints to manually setup the
KanesMethod
as the backend.
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\), \(q_2\) and \(q_3\) represent the angles between the links. Likewise, the
generalized speeds \(u_1\), \(u_2\) and \(u_3\) represent the angular velocities
between the links. We also create some symbols()
to represent the
lengths and density of the links.
>>> from sympy import symbols, Matrix, solve, simplify
>>> 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')
With all symbols defined, we can now define the bodies.
>>> N = ReferenceFrame('N')
>>> inertias = [inertia(N, 0, 0, rho * l ** 3 / 12) for l in (l1, l2, l3, l4)]
>>> link1 = Body('Link1', frame=N, mass=rho * l1, central_inertia=inertias[0])
>>> link2 = Body('Link2', mass=rho * l2, central_inertia=inertias[1])
>>> link3 = Body('Link3', mass=rho * l3, central_inertia=inertias[2])
>>> link4 = Body('Link4', mass=rho * l4, central_inertia=inertias[3])
Next, we also define the first three joints.
>>> joint1 = PinJoint('J1', link1, link2, coordinates=q1, speeds=u1,
... parent_axis=link1.z, parent_joint_pos=l1 / 2 * link1.x,
... child_axis=link2.z, child_joint_pos=-l2 / 2 * link2.x)
>>> joint2 = PinJoint('J2', link2, link3, coordinates=q2, speeds=u2,
... parent_axis=link2.z, parent_joint_pos=l2 / 2 * link2.x,
... child_axis=link3.z, child_joint_pos=-l3 / 2 * link3.x)
>>> joint3 = PinJoint('J3', link3, link4, coordinates=q3, speeds=u3,
... parent_axis=link3.z, parent_joint_pos=l3 / 2 * link3.x,
... child_axis=link4.z, child_joint_pos=-l4 / 2 * link4.x)
Now we can formulate the holonomic constraint that will close the kinematic loop.
>>> loop = link4.masscenter.pos_from(link1.masscenter) + l1 / 2 * link1.x + l4 / 2 * link4.x
>>> fh = Matrix([loop.dot(link1.x), loop.dot(link1.y)])
In order to generate the equations of motions, we will use the JointsMethod
as our fronted. Before setting up the KanesMethod
as its backend we
need to calculate the velocity constraints.
>>> method = JointsMethod(link1, joint1, joint2, joint3)
>>> t = dynamicsymbols._t
>>> qdots = solve(method.kdes, [q1.diff(t), q2.diff(t), q3.diff(t)])
>>> fhd = fh.diff(t).subs(qdots)
Now we can setup the KanesMethod
as the backend and compute the
equations of motion.
>>> method._method = KanesMethod(
... method.frame, q_ind=[q1], u_ind=[u1], q_dependent=[q2, q3],
... u_dependent=[u2, u3], kd_eqs=method.kdes,
... configuration_constraints=fh, velocity_constraints=fhd,
... forcelist=method.loads, bodies=method.bodies)
>>> simplify(method.method._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))]])