Four-bar Linkage

Welcome to this tutorial on using SymPy to derive the equations of motion (EoMs) of a four-bar linkage!

This notebook provides a hands-on introduction to using SymPy in a workflow, which is similar to the internals of BRiM. which also follows the four-bar linkage example in SymPy. In doing so it aims to satisfy the following learning objectives:

• Learn some of the basic syntax of SymPy for creating symbols and expressions.

• Learn how to create bodies and joints in sympy.physics.mechanics.

• Learn how to define a system in a systematic approach such that the EoMs can automatically be formed using Kane’s method.

• Learn about the underlying workflow utilized in BRiM.

• Learn to perform a simple simulation using solve_ivp from SciPy and visualize the results using SymMePlot.

Learn Multibody Dynamics by Moore is a great resource for learning about multibody dynamics while using the more lower-level API of the mechanics module. It should be noted that this book is part of a university course, so it does not use a lot of the high-level API of sympy.physics.mechanics. The following chapters are recommendable for learning more about the basics:

• SymPy gives a brief introduction to the basics of SymPy.

• Orientation of Reference Frames gives a brief introduction to the basics of reference frames.

• Vectors gives a brief introduction to the basics of vectors.

• Angular Kinematics is a more in-depth introduction to angular kinematics.

• Translational Kinematics introduces the usage of points and discusses various theorems for computing velocities and accelerations.

• Holonomic Constraints and Nonholonomic Constraints introduce the usage of constraints and how to formulate them.

• Mass Distribution gives an in-depth introduction to defining mass and inertia.

• Force, Moment and Torque gives an in-depth introduction to defining forces, moments, and torques without utilizing the high-level API of sympy.physics.mechanics.

• Other chapters cover things like how vector differentiation is defined and how to derive the EoMs using your own manually written Kane’s method and Lagrange’s method.

Overview of the Workflow

When forming the EoMs of a system in SymPy, one typically first describes the model after which an algorithmic implementation of Kane’s method or Lagrange’s method is used to derive the EoMs. In this tutorial, we will use Kane’s method. BRiM breaks the model description into four parts:

1. Define objects: Create the objects, such as symbols reference frames, without defining any relationships between them.

2. Define kinematics: Establish relationships between the objects’ orientations/positions, velocities, and accelerations.

3. Define loads: Specifies the forces and torques acting upon the system.

4. Define constraints: Computes the holonomic and nonholonomic constraints to which the system is subject.

The image below shows a schematic visualization of these steps for a rolling disc.

After describing the model and forming the EoMs, this tutorial will show how to do a simple simulation using solve_ivp from SciPy and visualize the results using SymMePlot.

Description Four-bar Linkage

A four-bar linkage is a frequently modeled system. It consists of four rigid bodies modeled as thin rods with uniform density, where the first body is fixed to the ground. Each of these bodies is attached to the previous via a pin joint. The last pin joint is modeled as holonomic constraints when using generalized coordinates, also called a minimal coordinate representation. In the image below the four-bar linkage is shown. Additionally, we will also model the gravity force acting on the system, introduce a spring-damper element between the first and second body, and have a time-dependent force acting horizontally at the third pin joint.

Define Objects

The first set of objects to define are the symbols, which represent various quantities in the system. In SymPy mechanics we distinguish two kinds of symbols: symbols and dynamic symbols. Symbols denote constants, like mass, dynamic symbols denote time-varying functions, like generalized coordinates. The code snippet below defines the symbols and dynamic symbols for the four-bar linkage.

Exercise: Add the missing symbols and dynamic symbols for the four-bar linkage: l1, l2, l3, l4, k, c and q3, u1, u2, u3.

import warnings
# Import sympy under the alias sm
# Import sympy.physics.mechanics under the alias me
import sympy as sm
import sympy.physics.mechanics as me
me.init_vprinting()  # Some pretty printing setting

# Define the symbols
rho, g = sm.symbols("rho g")
q1, q2 = me.dynamicsymbols("q1:3")  # You can also use ranges.
F = sm.Function("F")(sm.Symbol("t"))  # Is exactly the same as a dynamicsymbol.
### BEGIN SOLUTION
l1, l2, l3, l4, k, c = sm.symbols("l1:5 k c")
q3, u1, u2, u3 = me.dynamicsymbols("q3 u1:4")
### END SOLUTION
l1, l2, l3, l4, g, k, c, q1, q2, q3, u1, u2, u3

$\displaystyle \left( l_{1}, \ l_{2}, \ l_{3}, \ l_{4}, \ g, \ k, \ c, \ q_{1}, \ q_{2}, \ q_{3}, \ u_{1}, \ u_{2}, \ u_{3}\right)$

Other objects we need to create are reference frames, points, rigid bodies, and particles. In general one can state that reference frames and points store the kinematic relationships, where reference frames store the rotational relations and points the translational. Rigid bodies and particles can mostly be seen as dataclasses, which basically store the inertial properties and the related point and frame if applicable. For the inertia it should be noted that we use dyadics, but as the code shows these can also be initialized in a tensor kind of format.

The code snippet below defines the first link by fully specifying all initialize properties (all arguments are made keyword arguments to show what each is). We also create a point P41 to represent the location of the closing joint w.r.t link1.

N = me.ReferenceFrame("N")
mc1 = me.Point("mc1")
I1 = me.Inertia.from_inertia_scalars(point=mc1, frame=N, ixx=0, iyy=0, izz=rho * l1 ** 3 / 12)
link1 = me.RigidBody(
    name="link1",
    masscenter=mc1,
    frame=N,
    mass=rho * l1,
    inertia=I1,
)
P41 = me.Point("P41")
link1

RigidBody('link1', masscenter=mc1, frame=N, mass=l1*rho, inertia=Inertia(dyadic=l1**3*rho/12*(N.z|N.z), point=mc1))

To define link2 we can also make use of SymPy automatic creation of properties, where we can also overwrite them afterward.

link2 = me.RigidBody("link2", mass=rho * l2)
display(link2.central_inertia.to_matrix(link2.frame))  # Display central inertia as matrix.
link2.central_inertia = me.inertia(link2.frame, 0, 0, rho * l2 ** 3 / 12)  # Overwrite central inertia.
link2.central_inertia

$\displaystyle \left[\begin{matrix}link_{2 ixx} & link_{2 ixy} & link_{2 izx}\\link_{2 ixy} & link_{2 iyy} & link_{2 iyz}\\link_{2 izx} & link_{2 iyz} & link_{2 izz}\end{matrix}\right]$

$\displaystyle \frac{l_{2}^{3} \rho}{12}\mathbf{\hat{link2_frame}_z}\otimes \mathbf{\hat{link2_frame}_z}$

To keep track of all objects in the system, such that we can in the end also easily form the EoMs we can make use of System, which is not available yet in the public API. It is of course also possible to add things to the system later on.

from sympy.physics.mechanics.system import System
system = System(N, link1.masscenter)
system.add_bodies(link1, link2)

Exercise: Create the remaining links link3 and link4 and the point P44 to represent the location of the closing joint w.r.t link4.

### BEGIN SOLUTION
link3 = me.RigidBody("link3", mass=rho * l3)
link3.central_inertia = me.inertia(link3.frame, 0, 0, rho * l3 ** 3 / 12)
link4 = me.RigidBody("link4", mass=rho * l4)
link4.central_inertia = me.inertia(link4.frame, 0, 0, rho * l4 ** 3 / 12)
P44 = me.Point("P44")
### END SOLUTION
system.bodies

(RigidBody('link1', masscenter=mc1, frame=N, mass=l1*rho, inertia=Inertia(dyadic=l1**3*rho/12*(N.z|N.z), point=mc1)),
 RigidBody('link2', masscenter=link2_masscenter, frame=link2_frame, mass=l2*rho, inertia=Inertia(dyadic=l2**3*rho/12*(link2_frame.z|link2_frame.z), point=link2_masscenter)))

Define Kinematics

The kinematic relations in SymPy mechanics are stored in graphs. Here the reference frames and points form the nodes and the edges are the kinematic relations. To orient frames w.r.t. each other it is advisable to use either:

• ReferenceFrame.orient_axis, which orients a frame w.r.t. another frame by rotating around an axis.

• ReferenceFrame.orient_body_fixed, which rotates a frame using three successive body fixed simple axis rotations.

• ReferenceFrame.orient_explicit, which allows you to specify the DCM explicitly.

The location of points w.r.t. one another is set using Point.set_pos and retrieved using Point.pos_from. As for their velocity those are set w.r.t. to the reference frames.

The code below specifies the kinematic relations of the first pin joint using this approach.

link2.frame.orient_axis(link1.frame, link1.z, q1)
link2.frame.set_ang_vel(link1.frame, u1 * link1.z)  # Specify the angular velocity using a generalized speed.
P1 = link1.masscenter.locatenew("P1", l1 / 2 * link1.x)  # Location of the first pin joint.
link2.masscenter.set_pos(P1, l2 / 2 * link2.x)  # Set the position of the mass center of link2 w.r.t. P1.
link1.masscenter.set_vel(link1.frame, 0)  # Fixate the mass center of link1 in its frame.

# Add the generalized coordinate, speed, and kinematic differential equation to the system.
system.add_coordinates(q1)
system.add_speeds(u1)
system.add_kdes(u1 - q1.diff())

link2.masscenter.vel(link1.frame)  # Get the velocity of the mass center of link2 in the frame of link1.

$\displaystyle \frac{l_{2} u_{1}}{2}\mathbf{\hat{link2_frame}_y}$

The orientation between frames is stored as a Direction Cosine Matrix (DCM). A DCM between two frames can be computed using the dcm method. The following code computes the DCM, which maps vectors expressed in link1.frame to vectors expressed in link2.frame.

link2.frame.dcm(link1.frame)

$\displaystyle \left[\begin{matrix}\cos{\left(q_{1} \right)} & \sin{\left(q_{1} \right)} & 0\\- \sin{\left(q_{1} \right)} & \cos{\left(q_{1} \right)} & 0\\0 & 0 & 1\end{matrix}\right]$

While the above works fine, it is also possible to make use the PinJoint. For more information on the interface of the joint classes, refer to its documenation and API reference. The other joints in sympy.physics.mechanics are:

• PrismaticJoint

• CylindricalJoint

• PlanarJoint

• SphericalJoint

• WeldJoint

joint2 = me.PinJoint(
    name="joint2",
    parent=link2,
    child=link3,
    coordinates=q2,
    speeds=u2,
    parent_point=link2.masscenter.locatenew("P2", l2 / 2 * link2.x),  # Position of the second pin joint w.r.t. to link2 as point.
    child_point=-l3 / 2 * link3.x,  # Position of the second pin joint w.r.t. to link3 as vector.
    joint_axis=link2.z,  # Axis of rotation.
)
system.add_joints(joint2)
display(link3.masscenter.vel(N))  # Get the velocity of the mass center of link3 in the Newtonian frame.
system.kdes  # The kinematic differential equation has been retrieved from the joint when adding it to the system.

$\displaystyle \frac{l_{3} \left(u_{1} + u_{2}\right)}{2}\mathbf{\hat{link3_frame}_y} + l_{2} u_{1}\mathbf{\hat{link2_frame}_y}$

$\displaystyle \left[\begin{matrix}u_{1} - \dot{q}_{1}\\u_{2} - \dot{q}_{2}\end{matrix}\right]$

Exercise: Establish the kinematic relations for the third pin joint.

### BEGIN SOLUTION
joint3 = me.PinJoint("joint3", link3, link4, q3, u3, l3 / 2 * link3.x, -l4 / 2 * link4.x, joint_axis=link3.z)
system.add_joints(joint3)
### END SOLUTION
# A few sanity checks.
assert len(system.q) == 3
assert len(system.u) == 3
assert len(system.kdes) == 3
link4.masscenter.vel(N)  # Get the velocity of the mass center of link4 in the Newtonian frame.

$\displaystyle \frac{l_{4} \left(u_{1} + u_{2} + u_{3}\right)}{2}\mathbf{\hat{link4_frame}_y} + l_{3} \left(u_{1} + u_{2}\right)\mathbf{\hat{link3_frame}_y} + l_{2} u_{1}\mathbf{\hat{link2_frame}_y}$

Exercise: Set the position of P41 w.r.t. link1 and P44 w.r.t. link4.

### BEGIN SOLUTION
P41.set_pos(link1.masscenter, -l1 / 2 * link1.x)
P44.set_pos(link4.masscenter, l4 / 2 * link4.x)
### END SOLUTION
assert P44.pos_from(P41) == l1 * link1.x + l2 * link2.x + l3 * link3.x + l4 * link4.x
P44.pos_from(P41)  # Get the position of P44 w.r.t. P41.

$\displaystyle l_{4}\mathbf{\hat{link4_frame}_x} + l_{3}\mathbf{\hat{link3_frame}_x} + l_{2}\mathbf{\hat{link2_frame}_x} + l_{1}\mathbf{\hat{n}_x}$

Define Loads

With the kinematics defined, the next step is to define the loads. The term loads is used as a general term for both forces and torques. A load within SymPy is defined as a vector, which is associated with a location. For a force this location must be a point and for a torque the location must be a reference frame.

The code below defines the gravity force acting on the system using System.apply_uniform_gravity.

system.apply_uniform_gravity(-g * N.y)
system.loads  # Get the loads of the system.

(Force(point=mc1, force=- g*l1*rho*N.y),
 Force(point=link2_masscenter, force=- g*l2*rho*N.y),
 Force(point=link3_masscenter, force=- g*l3*rho*N.y),
 Force(point=link4_masscenter, force=- g*l4*rho*N.y))

Exercise: Apply the force acting horizontally at the third pin joint with a magnitude of F(t) using me.Force.

### BEGIN SOLUTION
system.add_loads(
    me.Force(joint3.parent_point, F * N.x)
)
### END SOLUTION
system.loads[-1]

Force(point=joint3_link3_joint, force=F(t)*N.x)

Exercise: Apply the spring-damper element between link1 and link2 using me.TorqueActuator (you could use the class method at_pin_joint for simplicity).

### BEGIN SOLUTION
system.add_actuators(
    me.TorqueActuator(
        torque=-k * q2 - c * u2,
        axis=joint2.joint_axis,
        target_frame=link2,
        reaction_frame=link1
    )
)
### END SOLUTION
system.actuators

(TorqueActuator(-c*u2(t) - k*q2(t), axis=link2_frame.z, target_frame=link2_frame, reaction_frame=N),)

Define Constraints

At this state we have defined a 3-link pendulum with a spring-damper element between the first and second link and a time-dependent force acting horizontally at the third pin joint. The next step is to define the constraints. The System class distinguishes between holonomic and nonholonomic constraints. Holonomic constraints are constraints that can be written as a function of the generalized coordinates and time. Nonholonomic constraints are constraints also dependent on the generalized speeds and are non-integrable.

The code below defines the holonomic constraints in the horizontal direction for the four-bar linkage.

system.add_holonomic_constraints(
    P44.pos_from(P41).dot(N.x)
)
system.holonomic_constraints

$\displaystyle \left[\begin{matrix}l_{1} + l_{2} \cos{\left(q_{1} \right)} + l_{3} \left(- \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) + l_{4} \left(\left(- \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) \cos{\left(q_{3} \right)} + \left(- \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} - \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)}\right) \sin{\left(q_{3} \right)}\right)\end{matrix}\right]$

Exercise: Define the holonomic constraints in the vertical direction for the four-bar linkage.

### BEGIN SOLUTION
system.add_holonomic_constraints(
    P44.pos_from(P41).dot(N.y)
)
### END SOLUTION
assert len(system.holonomic_constraints) == 2
system.holonomic_constraints

$\displaystyle \left[\begin{matrix}l_{1} + l_{2} \cos{\left(q_{1} \right)} + l_{3} \left(- \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) + l_{4} \left(\left(- \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) \cos{\left(q_{3} \right)} + \left(- \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} - \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)}\right) \sin{\left(q_{3} \right)}\right)\\l_{2} \sin{\left(q_{1} \right)} + l_{3} \left(\sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)}\right) + l_{4} \left(\left(- \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{3} \right)} + \left(\sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)}\right) \cos{\left(q_{3} \right)}\right)\end{matrix}\right]$

Form EoMs

Before forming the EoMs it is necessary to specify which generalized coordinates and generalized speeds are independent and which are dependent. A useful feature to check if you have forgotten these kinds of things is System.validate_system.

try:
    system.validate_system()
except ValueError as e:
    print(e)

The number of dependent generalized coordinates 0 should be equal to
the number of holonomic constraints 2.

The number of dependent generalized speeds 0 should be equal to the
number of velocity constraints 2.

To set the independent and dependent generalized coordinates and speeds we can specify System.q_ind, System.q_dep, System.u_ind, System.u_dep. The code below specifies the generalized coordinates.

system.q_ind = [q1]
system.q_dep = [q2, q3]
display(system.q_ind)
display(system.q_dep)
display(system.q)

$\displaystyle \left[\begin{matrix}q_{1}\end{matrix}\right]$

$\displaystyle \left[\begin{matrix}q_{2}\\q_{3}\end{matrix}\right]$

$\displaystyle \left[\begin{matrix}q_{1}\\q_{2}\\q_{3}\end{matrix}\right]$

Exercise: Specify the generalized speeds.

### BEGIN SOLUTION
system.u_ind = [u1]
system.u_dep = [u2, u3]
### END SOLUTION
display(system.u_ind)
display(system.u_dep)
display(system.u)
system.validate_system()

$\displaystyle \left[\begin{matrix}u_{1}\end{matrix}\right]$

$\displaystyle \left[\begin{matrix}u_{2}\\u_{3}\end{matrix}\right]$

$\displaystyle \left[\begin{matrix}u_{1}\\u_{2}\\u_{3}\end{matrix}\right]$

Now that we have described the model entirely we can form the equations using System.form_eoms. This method will automatically make use of KanesMethod to form the EoMs. The code below forms the EoMs.

with warnings.catch_warnings():
    warnings.simplefilter("ignore")
    eoms = system.form_eoms(constraint_solver="CRAMER")

Solving Initial Conditions

Now that we have the equations describing the system’s kinematics and dynamics, we need to convert them to fast numeric code. This step is called code generation. SymPy has an entirely separate module that allows you to generate code in a variety of languages. In this tutorial, we will only use the lambdify function, which generates a Python function that uses NumPy by default.

Before we can simulate the system we need to determine the initial conditions. These initial conditions should satisfy the holonomic and nonholonomic constraints. To compute the values the general workflow is to first create functions to quickly evaluate the constraints. After which we can use SciPy’s fsolve to find the values for the dependent generalized coordinates and speeds.

The cell below specifies the constants and the initial values of the independent generalized coordinates and speeds. Next, it creates a function to quickly evaluate the holonomic constraints. Lastly, it solves for the dependent generalized coordinates.

import numpy as np
from scipy.integrate import solve_ivp
from scipy.optimize import fsolve

q_ind_vals = [-0.5]
u_ind_vals = [-0.1]
constants = {
    rho: 100.0, l1: 0.8, l2: 0.5, l3: 1.0, l4: 0.7, k: 100, c: 10, g: 9.81
}
p, p_vals = zip(*constants.items())  # Get a tuple of symbols and values.

# Always enable Common Subexpression Elimination (CSE) because it is cheap and faster.
# [:] is a quick method to convert a SymPy matrix to a list.
eval_hc = sm.lambdify((system.q_dep, system.q_ind, p), system.holonomic_constraints[:], cse=True)

q_dep_vals = fsolve(eval_hc, [-1.5, -1.5], args=(q_ind_vals, p_vals))
q0 = [*q_ind_vals, *q_dep_vals]
q0

$\displaystyle \left[ -0.5, \ -2.24641553739307, \ -1.49782726919522\right]$

We have now computed the initial values for the dependent generalized coordinates. For the dependent generalized speeds we have to do the same. These dependent generalized speeds should satisfy the constraints in the velocity space, i.e. velocity constraints. The velocity constraints are simply the combination of the time-derivatives of the holonomic constraints and the nonholonomic constraints.

The cell below computes the velocity constraints, where it replaces the time-derivatives of the generalized coordinates with the correct generalized speeds.

assert me.dynamicsymbols._t == sm.Symbol("t")  # This is the same.
vel_constrs = system.holonomic_constraints.diff(me.dynamicsymbols._t).col_join(system.nonholonomic_constraints)
vel_constrs = vel_constrs.xreplace(system.eom_method.kindiffdict())
set().union(*(me.find_dynamicsymbols(vc) for vc in vel_constrs))

$\displaystyle \left\{q_{1}, q_{2}, q_{3}, u_{1}, u_{2}, u_{3}\right\}$

Exercise: Code generate a function to solve the velocity constraints, compute the dependent generalized speeds, and create a list u0 with the initial values of all generalized speeds.

### BEGIN SOLUTION
eval_vc = sm.lambdify((system.u_dep, system.u_ind, system.q, p), vel_constrs[:], cse=True)
u_dep_vals = fsolve(eval_vc, [0.5, -0.5], args=(u_ind_vals, q0, p_vals))
u0 = [*u_ind_vals, *u_dep_vals]
### END SOLUTION
assert np.allclose(u0, [-0.1, 0.07, -0.03], atol=0.01)
u0

$\displaystyle \left[ -0.1, \ 0.0715829929380234, \ -0.0274688413154955\right]$

For our simulation, we of course also need to define our inputs, which is in this case just the time-dependent force acting horizontally at the third pin joint. The code below sets this force to be equal to \(\sin(t)\), but feel free to modify it.

r = [F]  # Inputs to the system

def r_eval(t):
    return [100 * np.sin(np.pi * t)]

Simulation

An important aspect of integrators is their suitability for solving either Ordinary Differential Equations (ODEs) or Differential Algebraic Equations (DAEs). ODEs are expressed as \(\dot{x}(t) = F(x(t), t)\), while DAEs take the form of \(F(\dot{x}(t), x(t), t) = 0\).

In this tutorial, we’ll focus on ODEs as they are easier to implement. However, it’s worth noting that ODEs may eventually lead to the violation of holonomic and nonholonomic constraints. DAE integrators can maintain these constraints at zero, but they involve a more complex setup. You can access a variety of ODE integrators using the Python function solve_ivp from SciPy, and for DAE integrators, consider using dae from ODES SciKits.

To use solve_ivp we need to create a function which evaluates \(F(x(t), t)\), where \(x(t)=\begin{bmatrix} q(t) \\ u(t)\end{bmatrix}\). In the code below we create this function by first lambdifying the full mass matrix and full forcing vector, which are then evaluated and solved for \(\dot{x}(t)\) in eval_rhs.

x = system.q.col_join(system.u)
x0 = np.concatenate((q0, u0))
eval_sys = sm.lambdify((x, p, r, me.dynamicsymbols._t), (system.mass_matrix_full, system.forcing_full), cse=True)

def eval_rhs(t, x):
    Mm, gm = eval_sys(x, p_vals, r_eval(t), t)
    return np.linalg.solve(Mm, gm).ravel()

eval_rhs(0.0, x0)

array([-0.1       ,  0.07158299, -0.02746884, -9.26918156,  6.63703663,
       -2.55110976])

With everything set and ready we can solve the system for the initial conditions.

sol = solve_ivp(eval_rhs, (0, 4), x0)

Visualization

To quickly visualize a system in matplotlib we can use SymMePlot, a tool to easily make 3D visualizations of mechanical systems from sympy.physics.mechanics in matplotlib.

from symmeplot.matplotlib import Scene3D
from scipy.interpolate import CubicSpline
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from IPython.display import HTML

# Create an interpolation function for animation purposes.
x_eval = CubicSpline(sol.t, sol.y.T)

# Get the location of the third pin joint.
joint3_point = joint3.parent_point

# Create a figure and scene.
fig, ax = plt.subplots(subplot_kw={"projection": "3d", "proj_type": "ortho"}, figsize=(10, 10))
scene = Scene3D(system.frame, system.fixed_point, ax=ax)
# Add the bodies to the scene.
for body in system.bodies:
    scene.add_body(body)
# Add the four-bar linkage as a line.
scene.add_line([P41, P1, joint2.parent_point, joint3_point, P44], name="four-bar linkage", color="k")
# Add the force vector.
F_max = max(r_eval(ti)[0] for ti in sol.t)
scene.add_vector(F * N.x / F_max, joint3_point, color="r")
# Setup the plotter for evaluation and plot the first frame.
scene.lambdify_system((x, r, p))
scene.evaluate_system(x_eval(0.0), r_eval(0.0), p_vals)
scene.plot()
# Some extra configurations.
ax.axis("off")
ax.view_init(90, -90, 0)

# Create the animation.
fps = 30
ani = scene.animate(
    lambda ti: (x_eval(ti), r_eval(ti), p_vals),
    frames=np.arange(0, sol.t[-1], 1 / fps),
    blit=False)
display(HTML(ani.to_jshtml(fps=fps)))
plt.close()

Once Loop Reflect

You have come to the end of this tutorial. Congratulations! As you may have noticed the animation above clearly shows that the holonomic constraints are violated during the simulation. In future tutorials we will be using a utility, which allows the easy usage of a DAE solver to prevent this problem.