Below is an example of a dynamics simulation model generated in a Jupyter Notebook using Python and the sympy.physics.mechanics module. The text, equations, and plots below are pasted from a HTML export of the notebook, if you are interested in the raw source, it may be found on GitHub.

All Notebooks

fourbar

Four-bar Linkage Simulation¶

Python and the sympy, numpy, and scipy modules are used to derive equations of motion and simulate the motion of a four bar linkage mechanism. The derivation includes the definition of symbolic variables, frames to define angular orientation, points to define position, and positional constraints. Rigid bodies are defined with mass and inertia defined at the center of each link, and gravitational force is applied at the mass centers. Kane’s method is used to derive equations of motion, which are subsequently simulated, and the results plotted for generalized coordinates, speeds, and link positions as a function of time.

Introduction¶

Derivation of the four bar linkage model requires several mathematical constructs:

Symbolic variables representing constants and time varying states
Kinematic representations for orientation of reference frames and positions of points
Kinetics of rigid bodies, including mass and inertia
An algorithmic implementation of Kane’s Method to derive equations of motion
Symbolic libraries for derivatives, simplification, and solving systems of equations

In [1]:

# Import symbolic math libraries
from sympy import symbols, Matrix
from sympy.physics.mechanics import (dynamicsymbols, 
                                     ReferenceFrame, Point, 
                                     RigidBody, inertia, 
                                     KanesMethod)
from sympy import diff, simplify, lambdify, solve

We will also be performing various numerical and linear algebra methods, including:

Array/matrix math operations
Matrix inversion
Ordinary differential equation intengration
Determining function minima for nonlinear equations

In [2]:

# Import numerical libraries
from numpy import array, linspace, rad2deg, deg2rad, append, interp
from numpy.linalg import pinv, inv
from scipy.integrate import odeint, solve_ivp
from scipy.optimize import fmin

Throughout this derivation we will also include multiple methods to visualize the model, including plotting of simulation results, and display of typeset equations.

In [3]:

# Import visualization and printing libraries
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from IPython.display import display, Latex
from sympy.physics.mechanics import mechanics_printing, mlatex
mechanics_printing()

We will frequently use a symbolic equation form where the left hand side is the symbol for a given function, and the right hand side is its functional representation.

In [4]:

"""
This function will take a list or tuple of symbolic or text objects, 
and render them using IPython LaTeX display functions placing an 
equal sign between each individual item in the list.
"""
def dtex(*args):
    tex = ''
    for arg in args:
        if isinstance(arg, str):
            # Handle string arguments
            tex += f'${arg}$ $\, = \,$ '
        else:
            # Handle SymPy arguments
            tex += f'${mlatex(arg)}$ $\, = \,$ '
    
    # Trim off the last equals sign
    tex = tex[:-10]  
    display(Latex(tex))

Symbolic Variables¶

Symbolic variables are defined using a subscript convention representing the number of the angular coordinate or speed, and corresponding bar length, mass, or inertia. Angles are represented as generalized coordinates $\boldsymbol{q} = (\theta_0, \theta_1, \theta_2)$, angular rates as generalized speeds $\boldsymbol{s} = (\omega_0, \omega_1, \omega_2)$, where the time rate of change of the $k^{th}$ coordinate, $\dot{\theta}_k = \omega_k$.

In [5]:

"""
Time-varying symbols are created using the dynamicsymbols function, where the 
argument is a string containing one or more output variables, comma separated.
The kinematic equations are created as the variable named kd, in this case with 
each derivative of a generalized coordinate being subtracted by the 
corresponding generalized speed. Convention for the kinematic equations is that
each item in the list is assumed to be equal to zero, thus the sk - diff(qk) can
be assume to be analogous to sk == diff(qk) from the perspective of the equation 
of motion generation step to follow.
"""
q = θ0, θ1, θ2 = dynamicsymbols('θ_0, θ_1, θ_2')
s = ω0, ω1, ω2 = dynamicsymbols('ω_0, ω_1, ω_2')
kd = [sk - diff(qk) for qk, sk in zip(q, s)]

Constants include the bar lengths, represented by the symbols $(l_0, l_1, l_2, l_3)$, bar masses with the symbols $(m_0, m_1, m_2)$, and bar inertias with the symbols $(I_0, I_1, I_2)$. Gravitational acceleration and time are represented by $g$ and $t$, respectively.

In [6]:

"""
Non-time-varying symbols are created using the symbols function, with the same
argument and output formatting as dynamicsymbols. These, however, will be 
replaced with a constant, numeric value in the simulation step.
"""
lengths = l0, l1, l2, l3 = symbols('l_0, l_1, l_2, l_3')
masses = m0, m1, m2 = symbols('m_0, m_1, m_2')
inertias = I0, I1, I2 = symbols('I_0, I_1, I_2')
g = symbols('g')
t = symbols('t')

Kinematics¶

The relative positioning of the links is defined first using frames to define orientation, then points for each of the link intersections, and the link centers of gravity (CG’s).

Frames¶

The inertial frame is considered to be stationary and non-rotating, and is denoted $R$. Notably, the unit vector $\hat{\boldsymbol{r}}_x$ is aligned horizontally (tangent to the Earth’s surface), the unit vector $\hat{\boldsymbol{r}}_y$ is aligned upward (opposite of gravitational acceleration), and the unit vector $\hat{\boldsymbol{r}}_z$ is orthogonal to $\hat{\boldsymbol{r}}_x$ and $\hat{\boldsymbol{r}}_y$.

In [7]:

"""
Frames are represented by the ReferenceFrame object, which takes a string
formatted name as its first argument. If no additional arguments are provided, 
then it can be assumed to be stationary.
"""
inertial = ReferenceFrame('R')

Three additional frames are defined relative to the inertial frame, with the aforementioned angles previously defined with the generalized coordinate vector $\boldsymbol{q}$. In each case, the orientation is defined with a rotation about the $\hat{\boldsymbol{r}}_z$ axis. Angular velocity vectors for the $k^{th}$ frame are defined with the $k^{th}$ generalized speed, or $\omega_k \hat{\boldsymbol{r}}_z$. The set of orthogonal unit vectors attached to the $k^{th}$ rotating frame is denoted $(\hat{\boldsymbol{e}}_{kx}, \hat{\boldsymbol{e}}_{ky}, \hat{\boldsymbol{e}}_{kz})$.

In [8]:

"""
With the inertial frame defined, additional non-stationary frames can be
created using the .orientnew method, which takes a string formatted name as its 
first argument, followed by arguments defining its orientation. The second
argument is the orientation method, where 'Axis' specifies a rotation about a
single axis in a specified frame. The next argument is the amounts of rotation, 
which for an 'Axis' definition, is a tuple containing the symbol representating 
the rotation angle, and the axis that the rotation is defined about.
"""
frames = [inertial.orientnew(f'E_{k}', 'Axis', (q[k], inertial.z)) for k in range(3)]
for k in range(3):
  frames[k].set_ang_vel(inertial, s[k]*inertial.z)

  display(Latex(f'Rotation matrix for the {k}th frame'))
  dtex('\mathbf{R}_' + str(k), frames[k].dcm(inertial))

  display(Latex(f'Angular rate for the {k}th frame'))
  dtex('\mathbf{\omega}_' + str(k), frames[k].ang_vel_in(inertial))

Rotation matrix for the 0th frame

$\mathbf{R}_0$ $\, = \,$ $\left[\begin{matrix}\cos{\left(θ_{0} \right)} & \sin{\left(θ_{0} \right)} & 0\\- \sin{\left(θ_{0} \right)} & \cos{\left(θ_{0} \right)} & 0\\0 & 0 & 1\end{matrix}\right]$

Angular rate for the 0th frame

$\mathbf{\omega}_0$ $\, = \,$ $ω_{0}\mathbf{\hat{r}_z}$

Rotation matrix for the 1th frame

$\mathbf{R}_1$ $\, = \,$ $\left[\begin{matrix}\cos{\left(θ_{1} \right)} & \sin{\left(θ_{1} \right)} & 0\\- \sin{\left(θ_{1} \right)} & \cos{\left(θ_{1} \right)} & 0\\0 & 0 & 1\end{matrix}\right]$

Angular rate for the 1th frame

$\mathbf{\omega}_1$ $\, = \,$ $ω_{1}\mathbf{\hat{r}_z}$

Rotation matrix for the 2th frame

$\mathbf{R}_2$ $\, = \,$ $\left[\begin{matrix}\cos{\left(θ_{2} \right)} & \sin{\left(θ_{2} \right)} & 0\\- \sin{\left(θ_{2} \right)} & \cos{\left(θ_{2} \right)} & 0\\0 & 0 & 1\end{matrix}\right]$

Angular rate for the 2th frame

$\mathbf{\omega}_2$ $\, = \,$ $ω_{2}\mathbf{\hat{r}_z}$

Points¶

The position of each point is defined relative to the origin, which is stationary and denoted $O$. For indexing purposes, we will consider $O$ as the $0^{th}$ point. Supposing we denote the $k^{th}$ point as $p_k$, and its position vector as $\boldsymbol{r}_k$, then for each $k=0, 1 ,2$, the subsequent position vector

$\boldsymbol{r}_{k+1} = \boldsymbol{r}_k + l_k \boldsymbol{e}_{kx}$.

Points representing the CG of each link are denoted $g_k$, where for each $k=0, 1, 2$, the CG position is

$r_{gk} = r_{k} + \frac{1}{2} l_k \boldsymbol{e}_{kx}$.

Velocity vectors for the $0^{th}$ and $3^{rd}$ point are set to zero, while velocity of the $1^{st}$ point is

$\boldsymbol{v}_1 = l_0 \omega_0 \hat{\boldsymbol{e}}_{0y}$,

and of the $2^{nd}$ point is

$\boldsymbol{v}_2 = \boldsymbol{v}_1 + l_1 \omega_1 \hat{\boldsymbol{e}}_{1y}$.

Velocity vectors for the CG’s of each link are

$\boldsymbol{v}_{gk} = \boldsymbol{v}_{k}+\frac{1}{2}l_k \omega_k \hat{\boldsymbol{e}}_{ky}$

In [9]:

"""
The variables points, and cms are both created as Python lists of Point objects.
The origin (0th point), and end of the third link (3rd) point, are each 
specified to have zero velocity, using the .set_vel() method. The locations of 
each link intersection is specified using the .locatenew() method from the 
point that precedes it in the list, by indexing the lengths list and the frames.
Velocity for the moving points is specified using the .v2pt_theory() method, 
which calculates velocity using the transport equation.
"""
# Create the origin, and specify that it is stationary
points = [Point('O')]
points[0].set_vel(inertial, 0)

# Create the points corresponding to the intersection of links
for k in range(3):
  points += [points[k].locatenew(f'p_{k}', lengths[k] * frames[k].x)]
for k in (1, 2):
  points[k].v2pt_theory(points[k-1], inertial, frames[k-1])
points[3].set_vel(inertial, 0)

# Create the points corresponding to centers of mass
cms = []
cms = [points[k].locatenew(f'g_{k}', 0.5 * lengths[k] * frames[k].x) for k in range(3)]
for k in range(3):
  cms[k].v2pt_theory(points[k if k < 2 else k+1], inertial, frames[k])

for k in range(3):
  display(Latex(f'Position for the {k+1}th point'))
  dtex('\mathbf{r}_' + str(k+1), points[k+1].pos_from(points[0]))
  display(Latex(f'Velocity for the {k+1}th point'))
  dtex('\mathbf{v}_' + str(k+1), points[k+1].vel(inertial))
  display(Latex(f'Position for the CG of the {k}th link'))
  dtex('\mathbf{r}_{g' + str(k) + '}', cms[k].pos_from(points[0]))
  display(Latex(f'Velocity for the CG of the {k}th link'))
  dtex('\mathbf{v}_{g' + str(k) + '}', cms[k].vel(inertial))

Position for the 1th point

$\mathbf{r}_1$ $\, = \,$ $l_{0}\mathbf{\hat{e_0}_x}$

Velocity for the 1th point

$\mathbf{v}_1$ $\, = \,$ $l_{0} ω_{0}\mathbf{\hat{e_0}_y}$

Position for the CG of the 0th link

$\mathbf{r}_{g0}$ $\, = \,$ $0.5 l_{0}\mathbf{\hat{e_0}_x}$

Velocity for the CG of the 0th link

$\mathbf{v}_{g0}$ $\, = \,$ $0.5 l_{0} ω_{0}\mathbf{\hat{e_0}_y}$

Position for the 2th point

$\mathbf{r}_2$ $\, = \,$ $l_{1}\mathbf{\hat{e_1}_x} + l_{0}\mathbf{\hat{e_0}_x}$

Velocity for the 2th point

$\mathbf{v}_2$ $\, = \,$ $l_{0} ω_{0}\mathbf{\hat{e_0}_y} + l_{1} ω_{1}\mathbf{\hat{e_1}_y}$

Position for the CG of the 1th link

$\mathbf{r}_{g1}$ $\, = \,$ $0.5 l_{1}\mathbf{\hat{e_1}_x} + l_{0}\mathbf{\hat{e_0}_x}$

Velocity for the CG of the 1th link

$\mathbf{v}_{g1}$ $\, = \,$ $l_{0} ω_{0}\mathbf{\hat{e_0}_y} + 0.5 l_{1} ω_{1}\mathbf{\hat{e_1}_y}$

Position for the 3th point

$\mathbf{r}_3$ $\, = \,$ $l_{2}\mathbf{\hat{e_2}_x} + l_{1}\mathbf{\hat{e_1}_x} + l_{0}\mathbf{\hat{e_0}_x}$

Velocity for the 3th point

$\mathbf{v}_3$ $\, = \,$ $0$

Position for the CG of the 2th link

$\mathbf{r}_{g2}$ $\, = \,$ $0.5 l_{2}\mathbf{\hat{e_2}_x} + l_{1}\mathbf{\hat{e_1}_x} + l_{0}\mathbf{\hat{e_0}_x}$

Velocity for the CG of the 2th link

$\mathbf{v}_{g2}$ $\, = \,$ $- 0.5 l_{2} ω_{2}\mathbf{\hat{e_2}_y}$

Constraints¶

A constraint is enforced such that the location of the $2^{nd}$ point is fixed to a constant location $\boldsymbol{r}_2 = l_3 \hat{\boldsymbol{r}}_x$. Using the position defined above, this gives the constraint equation

$f_c(\boldsymbol{q}) = l_2 \hat{\boldsymbol{e}}_{2x} + l_1 \hat{\boldsymbol{e}}_{1x} + l_0 \hat{\boldsymbol{e}}_{0x} – l_3 \hat{\boldsymbol{r}}_x = 0$.

Similarly, a velocity constraint can exists, which is the derivative of $f_c(\boldsymbol{q})$ with respect to time, or

$f_v(\boldsymbol{q}, \boldsymbol{s}) = \frac{\partial f_c(\boldsymbol{q})}{\partial t} = \sum_{k=0}^2 l_k \omega_k \hat{\boldsymbol{e}}_{kx}$.

Implementation of these constraints involves transforming the vector form of the constraint equations into a vector form, evaluated in the inertial frame. Upon this transformation, the positional constraint equation in the $\hat{\boldsymbol{r}}_x$ direction is

$-l_3 + \sum_{k=0}^2 l_k \cos \theta_k = 0$,

and the positional constraint equation in the $\hat{\boldsymbol{r}}_y$ direction is

$\sum_{k=0}^2 l_k \sin \theta_k = 0$.

The corresponding velocity constraint equation in the $\hat{\boldsymbol{r}}_x$ direction is

$\sum_{k=0}^2 -l_k \omega_k \sin \theta_0 = 0$,

and the velocity constraint equation in the $\hat{\boldsymbol{r}}_y$ direction is

$\sum_{k=0}^2 l_k \omega_k \cos \theta_0 = 0$.

In [10]:

"""
Positional constraints are determined by obtaining the position of the 3rd point
relative to the origin (0th point), using the .pos_from() method included in the
Point class, and subtracting the specified location of the constraint. The
velocity constraint equation is determined through calling the .dt() method to
calculate the time derivative of the position equation. Lastly, the to_matrix() 
method converts the vector form of the equations to a sympy Matrix object, with 
the first two elements of the matrix corresponding to the equations evaluated in
the rx and ry directions of the inertial frame, the third element is zero
because the motion is planar.
"""
# Constraint equations
fc = points[3].pos_from(points[0]) - lengths[3] * inertial.x
fv = fc.dt(inertial)

# Constraint matrix equations in inertial frame
position_constraint = fc.to_matrix(inertial)
velocity_constraint = fv.to_matrix(inertial)

display(Latex('Position constraint equation'))
dtex(fc, '\mathbf{0}')

display(Latex('Position contraint equations in inertial frame'))
dtex(position_constraint, Matrix([0, 0, 0]))

display(Latex('Velocity constraint equation'))
dtex(fv, '\mathbf{0}')

display(Latex('Velocity constraint equations in inertial frame'))
dtex(velocity_constraint, Matrix([0, 0, 0]))

# Define a function that will be used to solve for initial conditions
def solve_ics(theta_0, subs_dict):
  err = position_constraint.subs(q[0], theta_0).subs(subs_dict)
  err2 = lambdify(q[1:], err[0]**2 + err[1]**2)
  def loss(y):
    return err2(y[0], y[1])
  min_x = fmin(func=loss, x0=[0.0, 0.0]);
  return [theta_0, min_x[0], min_x[1], 0.0, 0.0, 0.0]

Position constraint equation

$l_{2}\mathbf{\hat{e_2}_x} + l_{1}\mathbf{\hat{e_1}_x} + l_{0}\mathbf{\hat{e_0}_x} – l_{3}\mathbf{\hat{r}_x}$ $\, = \,$ $\mathbf{0}$

Position contraint equations in inertial frame

$\left[\begin{matrix}l_{0} \cos{\left(θ_{0} \right)} + l_{1} \cos{\left(θ_{1} \right)} + l_{2} \cos{\left(θ_{2} \right)} – l_{3}\\l_{0} \sin{\left(θ_{0} \right)} + l_{1} \sin{\left(θ_{1} \right)} + l_{2} \sin{\left(θ_{2} \right)}\\0\end{matrix}\right]$ $\, = \,$ $\left[\begin{matrix}0\\0\\0\end{matrix}\right]$

Velocity constraint equation

$l_{2} ω_{2}\mathbf{\hat{e_2}_y} + l_{1} ω_{1}\mathbf{\hat{e_1}_y} + l_{0} ω_{0}\mathbf{\hat{e_0}_y}$ $\, = \,$ $\mathbf{0}$

Velocity constraint equations in inertial frame

$\left[\begin{matrix}- l_{0} ω_{0} \sin{\left(θ_{0} \right)} – l_{1} ω_{1} \sin{\left(θ_{1} \right)} – l_{2} ω_{2} \sin{\left(θ_{2} \right)}\\l_{0} ω_{0} \cos{\left(θ_{0} \right)} + l_{1} ω_{1} \cos{\left(θ_{1} \right)} + l_{2} ω_{2} \cos{\left(θ_{2} \right)}\\0\end{matrix}\right]$ $\, = \,$ $\left[\begin{matrix}0\\0\\0\end{matrix}\right]$

Dynamics¶

Rigid Bodies¶

Each of the three links are defined with a mass at its CG in the center of the link, and a moment of inertia $I_k$ relative to the CG and about the $\hat{\boldsymbol{e}}_{kz} = \hat{\boldsymbol{r}}_z$ axis. Linear momentum for each body in the inertial frame is

$\mathbf{P}_k = m_k \mathbf{v}_{gk}$

and angular momentum for each body, relative to the origin and in the inertial frame, is

$\mathbf{H}_k = I_k \omega_k \hat{\boldsymbol{e}}_{kz} + \mathbf{r}_{gk} \times m_k \mathbf{v}_{gk}$

In [11]:

"""
Bodies are instantiated using the RigidBody class, which takes as arguments a 
string-formatted name, vector corresponding to the CG position, frame in which
its inertia tensor components are defined, mass of the body, and an inertia
tuple. The inertia tuple includes an inertia dyadic, and a CG position. For 
convenience, the inertia dyadic is created using the inertia function, which
takes as arguments the frame in which the inertia tensor components are defined,
three required inertia terms (ixx, iyy, izz), and optional product of inertia
terms (ixy, izx, iyz) as keyword arguments.
"""
bodies = [RigidBody(f'Bar {k}', cms[k], frames[k], masses[k], 
                    (inertia(frames[k], 0, 0, inertias[k]), cms[k])) 
          for k in range(3)]

for k in range(3):
  display(Latex(f'Linear momentum for the {k}th link in the inertial frame'))
  dtex('\mathbf{P}_' + str(k), bodies[k].linear_momentum(inertial))
  display(Latex(f'Angular momentum for the {k}th link relative to the origin, in the inertial frame'))
  dtex('\mathbf{H}_' + str(k), bodies[k].angular_momentum(points[0], inertial))

Linear momentum for the 0th link in the inertial frame

$\mathbf{P}_0$ $\, = \,$ $0.5 l_{0} m_{0} ω_{0}\mathbf{\hat{e_0}_y}$

Angular momentum for the 0th link relative to the origin, in the inertial frame

$\mathbf{H}_0$ $\, = \,$ $(I_{0} ω_{0} + 0.25 l_{0}^{2} m_{0} ω_{0})\mathbf{\hat{e_0}_z}$

Linear momentum for the 1th link in the inertial frame

$\mathbf{P}_1$ $\, = \,$ $l_{0} m_{1} ω_{0}\mathbf{\hat{e_0}_y} + 0.5 l_{1} m_{1} ω_{1}\mathbf{\hat{e_1}_y}$

Angular momentum for the 1th link relative to the origin, in the inertial frame

$\mathbf{H}_1$ $\, = \,$ $(I_{1} ω_{1} + 0.5 l_{1} m_{1} \left(l_{0} \left(\sin{\left(θ_{0} \right)} \sin{\left(θ_{1} \right)} + \cos{\left(θ_{0} \right)} \cos{\left(θ_{1} \right)}\right) + 0.5 l_{1}\right) ω_{1})\mathbf{\hat{e_1}_z} + l_{0} m_{1} \left(l_{0} + 0.5 l_{1} \left(\sin{\left(θ_{0} \right)} \sin{\left(θ_{1} \right)} + \cos{\left(θ_{0} \right)} \cos{\left(θ_{1} \right)}\right)\right) ω_{0}\mathbf{\hat{e_0}_z}$

Linear momentum for the 2th link in the inertial frame

$\mathbf{P}_2$ $\, = \,$ $- 0.5 l_{2} m_{2} ω_{2}\mathbf{\hat{e_2}_y}$

Angular momentum for the 2th link relative to the origin, in the inertial frame

$\mathbf{H}_2$ $\, = \,$ $(I_{2} ω_{2} – 0.5 l_{2} m_{2} \left(l_{0} \left(\sin{\left(θ_{0} \right)} \sin{\left(θ_{2} \right)} + \cos{\left(θ_{0} \right)} \cos{\left(θ_{2} \right)}\right) + l_{1} \left(\sin{\left(θ_{1} \right)} \sin{\left(θ_{2} \right)} + \cos{\left(θ_{1} \right)} \cos{\left(θ_{2} \right)}\right) + 0.5 l_{2}\right) ω_{2})\mathbf{\hat{e_2}_z}$

Loads¶

The external forces on the three links are from gravitational acceleration, which is assumed to act in the $-\hat{\boldsymbol{r}}_z$ direction, such that the force at the $k^{th}$ link CG is

$\mathbf{F}_k = -g m_k \hat{\boldsymbol{r}}_z$

In [12]:

"""
Loads are created as a list of tuples, where the first element of the tuple is 
the point at which the force is applied, and the second element is the force 
vector.
"""
loads = [(cms[k], -masses[k] * g * inertial.y) for k in range(3)]
for load in loads:
  display(Latex(f'Force applied at the point ${load[0]}$'))
  dtex('\mathbf{F}_' + str(k), load[1])

Force applied at the point $g_0$

$\mathbf{F}_2$ $\, = \,$ $- g m_{0}\mathbf{\hat{r}_y}$

Force applied at the point $g_1$

$\mathbf{F}_2$ $\, = \,$ $- g m_{1}\mathbf{\hat{r}_y}$

Force applied at the point $g_2$

$\mathbf{F}_2$ $\, = \,$ $- g m_{2}\mathbf{\hat{r}_y}$

Equations of motion¶

Kane’s method is used to derive equations of motion given an inertial reference frame, the set of independent and dependent generalized coordinates and speeds, the set of configuration and velocity constraints, and the kinematic differential equations defining the relationship between generalized coordinates and speeds. The form of the equations is

$\mathbf{F}_r + \mathbf{F}_r^* = \mathbf{0}$

where if we assume the only independent generalized speed is $\omega_0$, the applied, or active force terms

$ \mathbf{F}_r = \sum_{k=0}^2 \frac{\partial \mathbf{v}_{gk}}{\partial \omega_0} \cdot \mathbf{F}_k $

and the body, or inertial force terms

$ \mathbf{F}_r^* = \sum_{k=0}^2 \left( \frac{\partial \mathbf{v}_{gk}}{\partial \omega_0} \cdot \dot{\mathbf{P}}_k + \frac{\partial \mathbf{\omega}_{k}}{\partial \omega_0} \cdot \dot{\mathbf{H}}_k \right) $

These equations are expanded to a mass matrix and forcing vector form, where

$\mathbf{M}(\boldsymbol{q}) \dot{\mathbf{x}} = \boldsymbol{F}\,(\boldsymbol{q}, \boldsymbol{s})$

where $\boldsymbol{x}=\left(\boldsymbol{q}, \boldsymbol{s}\right)$ is the combined state vector containing generalized coordinates and speeds.

In [13]:

"""
The KanesMethod object takes the inertial frame as its sole required argument. 
Additionally, the keyword arguments q_ind, and u_ind define the independent
generalized coordinates and speeds, in this case the first entry in the q and s
lists, which are surrounded by brackets so that the argument is in the required
list form. the q_dependent and u_dependent keyword arguments are the remaining 
entries in q and s. The configuration_constraints and velocity_constraints 
keyword arguments are taken from the matrix form of the contraint equations, in
the first two, non-zero directions. The kd_eqs keyword argument are kinematic 
equations defining the relation between generalized coordinates and speeds. The
.kanes_equations() method is called with the lists of bodies and loads as
arguments. After this is called, the equations can be accessed in various forms,
Notably in the mass matrix and forcing vector form that will be integrated here.
"""
kane = KanesMethod(inertial, q_ind=[q[0]], u_ind=[s[0]], 
                   q_dependent=q[1:], u_dependent=s[1:], 
                   configuration_constraints=position_constraint[:2], 
                   velocity_constraints=velocity_constraint[:2], 
                   kd_eqs=kd)
fr, frstar = kane.kanes_equations(bodies, loads)
display(Latex('Applied Forces'))
dtex('\mathbf{F}_r', simplify(fr))
display(Latex('Internal Forces'))
dtex('\mathbf{F}_r^*', simplify(frstar))
display(Latex('Mass Matrix'))
M = simplify(kane.mass_matrix_full)
dtex('\mathbf{M}', M)
display(Latex('Forcing'))
F = simplify(kane.forcing_full).subs(kane.kindiffdict())
dtex('\mathbf{F}', F)

Applied Forces

$\mathbf{F}_r$ $\, = \,$ $\left[\begin{matrix}- \frac{1.0 g l_{0} \cdot \left(0.5 m_{0} \sin{\left(θ_{1} – θ_{2} \right)} \cos{\left(θ_{0} \right)} – 0.5 m_{1} \sin{\left(θ_{0} + θ_{2} \right)} \cos{\left(θ_{1} \right)} + 1.0 m_{1} \sin{\left(θ_{1} \right)} \cos{\left(θ_{0} \right)} \cos{\left(θ_{2} \right)} – 0.5 m_{2} \sin{\left(θ_{0} – θ_{1} \right)} \cos{\left(θ_{2} \right)}\right)}{\sin{\left(θ_{1} – θ_{2} \right)}}\end{matrix}\right]$

Internal Forces

$\mathbf{F}_r^*$ $\, = \,$ $\left[\begin{matrix}\frac{- l_{0} l_{1} \left(I_{2} + 0.25 l_{2}^{2} m_{2}\right) \sin{\left(θ_{0} – θ_{1} \right)} \dot{ω}_{2} + l_{0} l_{2} \left(- 0.5 l_{0} l_{1} m_{1} ω_{0}^{2} \sin{\left(θ_{0} – θ_{1} \right)} + 0.5 l_{0} l_{1} m_{1} \cos{\left(θ_{0} – θ_{1} \right)} \dot{ω}_{0} + \left(I_{1} + 0.25 l_{1}^{2} m_{1}\right) \dot{ω}_{1}\right) \sin{\left(θ_{0} – θ_{2} \right)} – l_{1} l_{2} \cdot \left(0.5 l_{0} l_{1} m_{1} ω_{1}^{2} \sin{\left(θ_{0} – θ_{1} \right)} + 0.5 l_{0} l_{1} m_{1} \cos{\left(θ_{0} – θ_{1} \right)} \dot{ω}_{1} + \left(I_{0} + 0.25 l_{0}^{2} m_{0} + l_{0}^{2} m_{1}\right) \dot{ω}_{0}\right) \sin{\left(θ_{1} – θ_{2} \right)}}{l_{1} l_{2} \sin{\left(θ_{1} – θ_{2} \right)}}\end{matrix}\right]$

Mass Matrix

$\mathbf{M}$ $\, = \,$ $\left[\begin{matrix}1 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & 1.0 I_{0} + 0.25 l_{0}^{2} m_{0} – \frac{0.5 l_{0}^{2} m_{1} \sin{\left(θ_{0} – θ_{2} \right)} \cos{\left(θ_{0} – θ_{1} \right)}}{\sin{\left(θ_{1} – θ_{2} \right)}} + 1.0 l_{0}^{2} m_{1} & \frac{l_{0} \left(- 1.0 I_{1} \sin{\left(θ_{0} – θ_{2} \right)} – \frac{0.125 l_{1}^{2} m_{1} \cos{\left(θ_{0} – 3 θ_{1} + θ_{2} \right)}}{\sin{\left(θ_{1} \right)}} + \frac{0.125 l_{1}^{2} m_{1} \cos{\left(θ_{0} – θ_{1} + θ_{2} \right)}}{\sin{\left(θ_{1} \right)}}\right)}{l_{1} \sin{\left(θ_{1} – θ_{2} \right)}} & \frac{l_{0} \left(I_{2} + 0.25 l_{2}^{2} m_{2}\right) \sin{\left(θ_{0} – θ_{1} \right)}}{l_{2} \sin{\left(θ_{1} – θ_{2} \right)}}\\0 & 0 & 0 & – l_{0} \sin{\left(θ_{0} \right)} & – l_{1} \sin{\left(θ_{1} \right)} & – l_{2} \sin{\left(θ_{2} \right)}\\0 & 0 & 0 & l_{0} \cos{\left(θ_{0} \right)} & l_{1} \cos{\left(θ_{1} \right)} & l_{2} \cos{\left(θ_{2} \right)}\end{matrix}\right]$

Forcing

$\mathbf{F}$ $\, = \,$ $\left[\begin{matrix}ω_{0}\\ω_{1}\\ω_{2}\\\frac{l_{0} \cdot \left(0.5 g m_{2} \sin{\left(θ_{0} – θ_{1} \right)} \cos{\left(θ_{2} \right)} + 0.5 m_{1} \left(g \cos{\left(θ_{1} \right)} – l_{0} ω_{0}^{2} \sin{\left(θ_{0} – θ_{1} \right)}\right) \sin{\left(θ_{0} – θ_{2} \right)} – \left(0.5 g m_{0} \cos{\left(θ_{0} \right)} + 1.0 g m_{1} \cos{\left(θ_{0} \right)} + 0.5 l_{1} m_{1} ω_{1}^{2} \sin{\left(θ_{0} – θ_{1} \right)}\right) \sin{\left(θ_{1} – θ_{2} \right)}\right)}{\sin{\left(θ_{1} – θ_{2} \right)}}\\l_{0} ω_{0}^{2} \cos{\left(θ_{0} \right)} + l_{1} ω_{1}^{2} \cos{\left(θ_{1} \right)} + l_{2} ω_{2}^{2} \cos{\left(θ_{2} \right)}\\l_{0} ω_{0}^{2} \sin{\left(θ_{0} \right)} + l_{1} ω_{1}^{2} \sin{\left(θ_{1} \right)} + l_{2} ω_{2}^{2} \sin{\left(θ_{2} \right)}\end{matrix}\right]$

Analysis¶

Simulation¶

To obtain a callable ordinary differential equation (ODE) function, first we create a dictionary containing numeric values for the previously defined symbolic quantities. Assuming units of kilograms ($kg$), meters ($m$), Newtons ($N$), and seconds ($s$), the gravitational acceleration is specified to be 9.81 $m/s^2$. For this model, we will assume links of length 1, 2, 3, and 4 $m$, respectively, and with mass value of 1, 2, and 3 $kg$ for the first 3 links (the fourth link is fixed to the ground, it is not assigned mass). Moment of inertia values are assumed for a thin rod, where

$I_k = \frac{1}{12} m_k l_k^2$

These values are substituted into the equations for mass matrix and forcing prior to creation of the ODE function. The scipy.integrate.solve_ivp function is used to integrate the initial value problem, and requires and ode function of the form $\dot{\boldsymbol{x}} = f(t, \boldsymbol{x})$. In this case, $f(t, \boldsymbol{x}) = \mathbf{M}^{-1} \boldsymbol{F}$. Since $\mathbf{M}$ and $\boldsymbol{F}$ are state-dependent, these are calculated (and the mass matrix inverted) at each time step in the simulation. Initial states are estimated using an optimization function to minimize the positional constraint equations given a specified $\theta_0(0) = 85^\circ$.

In [14]:

# Create a dictionary of constants to be substituted into the symbolic equations
subs_dict = {g: 9.81, 
             m0: 1.0, m1: 2.0, m2: 3.0, 
             l0: 1.0, l1: 2.0, l2: 3.0, l3: 4.0}
for I, m, l in zip(inertias, masses, lengths[:3]):
  subs_dict[I] = 1.0 / 12.0 * subs_dict[m] * subs_dict[l]**2

# Use lambdify function to create anonymous functions for the mass matrix and 
# forcing, which will yield speed-optimized function given the state arguments,
# and will output a numpy array object   
mass_fcn = lambdify([t]+q+s, M.subs(subs_dict))
forcing_fcn = lambdify([t]+q+s, F.subs(subs_dict))

# Create a callable ordinary differential equation of time and state
def ode(t, x):
  args = append([t], x)
  dx = inv(mass_fcn(*args)) @ forcing_fcn(*args)
  return dx[:, 0]

# Determine initial conditions, and simulate
x0 = solve_ics(deg2rad(85), subs_dict)
sol = solve_ivp(ode, [0, 20], x0, rtol=1e-6)
for k in range(3):
  dtex(f'\\theta_{k}(0)', str(rad2deg(x0[k])))

Optimization terminated successfully.
         Current function value: 0.000000
         Iterations: 70
         Function evaluations: 134

$\theta_0(0)$ $\, = \,$ $85.0$

$\theta_1(0)$ $\, = \,$ $31.301991721073403$

$\theta_2(0)$ $\, = \,$ $-42.722211361919406$

Plotting¶

Generalized coordinates derived from simulating the equations of motion giving the specified constants and initial values are shown. Notable features include the periodicity of the $\theta_0$ coordinate, which oscillates between its initial $85^\circ$, and $405^\circ$. Since the only force acting on the system is gravity, there are no initial rates, and the constraint forces are conservative it does not complete one full rotation. Also, at instants where $\theta_0(t)=180^\circ$, $\theta_1(t) = \theta_2(t) = 0$ because $l_0 + l_3 = l_1 + l_2$, meaning at these times all four links will be in alignment.

In [15]:

"""
The sol structure contains the time variable t, and integrated states y. The
first three indices in y are the generalized coordinates, which are converted
from radians to degrees for plotting. Plotting functions are taken from the 
matplotlib.pyplot which was imported as plt.
"""
for k in range(3):
  plt.plot(sol.t, rad2deg(sol.y[k, :]))
plt.xlabel('Time (sec)')
plt.ylabel('Angle (deg)')
plt.legend(q);

Generalized speeds are similarly plotted. These are distinctly “spiky,” with positive and negative peaks corresponding to the times at which $\theta_0(t) = 180^\circ$.

In [16]:

"""
The remaining indices in the sol structure are the generalized speeds, which are
plotting in a similar manner to the coordinates, including conversion to 
degrees.
"""
for k in range(3):
  plt.plot(sol.t, rad2deg(sol.y[k+3, :]))
plt.xlabel('Time (sec)')
plt.ylabel('Angular Rate (deg)')
plt.legend(s);

An animation of the positions of each of the link interface is generated to visualize the spatial relations between points as a function of time. Interestingly, we can see the time at which $\theta_0=180^\circ$ corresponds to an apparrent “bounce” of the third link. This occurs because it is kinematically impossible given the link geometries for this link to travel past the horizontal, and abruptly reverses direction at this point.

In [17]:

"""
The matplotlib rc function is used to modify the runtime configuration, here 
specifying the jshtml argument for compatibility with showing animations in a 
Google Colaboratory notebook. The matplotlib FuncAnimation function takes a
figure, and commands to initialize and update the figure over a specified set of
frames. Here, a set of equations are created to solve for the position of each
of the intersection points as a function of the generalized coordinates. The 
update function takes these equation, and interpolates the states from the sol
structure as a function of time, then updates the lines in the plot for each
of the times specified in the frames argument of FuncAnimation.
"""
from matplotlib import rc
rc('animation', html='jshtml')

fig, ax = plt.subplots();
pos_eqn = [lambdify(q, point.pos_from(points[0]).subs(subs_dict).to_matrix(inertial)) for point in points]
pos = array([eqn(*x0[:3]) for eqn in pos_eqn])
xdata, ydata = pos[:, 0], pos[:, 1]
ln, = plt.plot(xdata, ydata, '-o');

def init():
    ax.set_xlim(-1.5, 5)
    ax.set_ylim(-1.5, 3)
    return ln,

def update(time):
    x = [interp(time, sol.t, sol.y[k]) for k in range(3)]
    pos = array([eqn(*x) for eqn in pos_eqn])
    xdata = pos[:, 0]
    ydata = pos[:, 1]
    ln.set_data(xdata, ydata)
    return ln,

ani = FuncAnimation(fig, update, frames=linspace(0, 3.14, 93), init_func=init, 
                    blit=True, interval=1000.0 * 3.14 / 93.0);
# Uncomment to display the animation if working in an actual Jupyter Notebook, 
# this will not render in WordPress, however
#ani  

Conclusion¶

Python and the sympy, numpy, and scipy modules are used to derive equations of motion and simulate the motion of a four bar linkage mechanism. The derivation includes the definition of symbolic variables, frames to define angular orientation, points to define position, and positional constraints. Rigid bodies are defined with mass and inertia defined at the center of each link, and gravitational force is applied at the mass centers. Kane’s method is used to derive equations of motion, which are subsequently simulated, and the results plotted for generalized coordinates, speeds, and link positions as a function of time.

Four-bar Linkage Simulation