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

eom

Equations of Motion in dRuBbLe¶

dRuBbLe includes a four degree-of-freedom (4-DOF) nonlinear dynamics simulation model to simulate the motion of the player and stool. Figure 1 is a depiction of the player in the game (left), and the corresponding dynamics model diagram (right). The following describes usage of SymPy and it’s Mechanics module to derive equations of motion (EOM) in Python using a Jupyter Notebook. Figure 1 – Dynamics Model Diagram

Setup¶

First we import the NumPy and SymPy mechanics modules in Python. NumPy provides an extensive set of numerical analysis funtions, while SymPy is a symbolic coding environment with the mechanics module standardizing various kinematic transformations and and equation of motion generation. The IPython display modules are imported to generate typeset equations suitable for document publication. Turning on mechanics_printing() will generate more compact typeset equations, where $\frac{\partial x}{\partial t}$ will be written $\dot{x}$, for example.

In [1]:

import numpy as np
from sympy import *
from sympy.physics.mechanics import *
from IPython.display import display, Latex
mechanics_printing()

Generalized Coordinates¶

To begin the dynamics model formulation, we first define the generalized coordinates. The player is defined as a point mass located at point $p$, with positions $(x, y)$ defined relative to the origin in the horizontal and vertical directions, respectively. The stool is also defined as a point mass located at point $s$, which is translated and rotated relative to the player with a transformation (to be described later) requiring an arm extension length $l$ and stool tilt angle $θ$. These four variables form the generalized coordinate vector $\boldsymbol{q}$.

The SymPy Mechanics dynamicsymbols function is used to define these as symbolic variables that are time-varying, and therefore candidate degrees-of-freedom (DOFs). Notably, the left hand side of the equation are the variables to be used in the source code, where th is used in place of $θ$. When typing code that references the tilt angle, th will be used, whereas when the display function is called to typeset the equation, the more visually appealing $θ$ is used, as seen below. Also note that the display adds the $(t)$ to each variable, indicating it is time-varying. The chained assignment operators (i.e. a = b = c) are used such that in the source code the variables generalized_coords and q can be used interchangeably.

In [2]:

x, y, l, th = dynamicsymbols('x y l θ')
generalized_coords = q = Matrix([x, y, l, th])
display(Latex('$\mathbf{q} = \,$' + q._repr_latex_()))

$\mathbf{q} = \,$$\displaystyle \left[\begin{matrix}x{\left(t \right)}\\y{\left(t \right)}\\l{\left(t \right)}\\θ{\left(t \right)}\end{matrix}\right]$

We will also need the derivatives of these coordinates with respect to time, which are obtained by calling the dynamicsymbols function with the second argument 1 indicating the first derivative, and 2 indicating the second derivative.

In [3]:

dxdt, dydt, dldt, dthdt = dynamicsymbols('x y l θ', 1)
ddt_generalized_coords = dqdt = Matrix([dxdt, dydt, dldt, dthdt])
d2xdt2, d2ydt2, d2ldt2, d2thdt2 = dynamicsymbols('x y l θ', 2)
d2dt2_generalized_coords = d2qdt2 = Matrix([d2xdt2, d2ydt2, d2ldt2, d2thdt2])
display(Latex('$\dot{\mathbf{q}} = \,$' + dqdt._repr_latex_()))
display(Latex('$\ddot{\mathbf{q}} = \,$' + d2qdt2._repr_latex_()))

$\dot{\mathbf{q}} = \,$$\displaystyle \left[\begin{matrix}\frac{d}{d t} x{\left(t \right)}\\\frac{d}{d t} y{\left(t \right)}\\\frac{d}{d t} l{\left(t \right)}\\\frac{d}{d t} θ{\left(t \right)}\end{matrix}\right]$

$\ddot{\mathbf{q}} = \,$$\displaystyle \left[\begin{matrix}\frac{d^{2}}{d t^{2}} x{\left(t \right)}\\\frac{d^{2}}{d t^{2}} y{\left(t \right)}\\\frac{d^{2}}{d t^{2}} l{\left(t \right)}\\\frac{d^{2}}{d t^{2}} θ{\left(t \right)}\end{matrix}\right]$

Initial values for the generalized coordinates are represented by a subscript $0$, where $\boldsymbol{q}_0 = (x_0, y_0, l_0, θ_0)^T$. In SymPy, these are defined using a call to the symbols function, which declares them as constants. As before, a slight variation is used for the variable representation in the source code versus in typesetting, where for the latter the underscore is used to ensure that these are typeset as a subscript.

In [4]:

x0, y0, l0, th0 = symbols('x_0 y_0 l_0 θ_0')
q0 = Matrix([x0, y0, l0, th0])
display(Latex('$\mathbf{q}_0 = \,$' + q0._repr_latex_()))

$\mathbf{q}_0 = \,$$\displaystyle \left[\begin{matrix}x_{0}\\y_{0}\\l_{0}\\θ_{0}\end{matrix}\right]$

Constants¶

Next we define the constants, including masses $m_p$ and $m_s$ of the player and stool, an offset distance $d$ between the player center of mass and the point about which the stool rotates, the gravitational constant $’g’$ (represented by grav in source code).

In [5]:

mp, ms, d, grav, t = symbols('m_p m_s d g t')

The spring coefficients $K_y$, $K_l$, and $K_θ$ introduce generalized forces into the system proportional to the subscripted coordinate minus its initial value.

In [6]:

Ky, Kl, Kt = symbols('K_y K_l K_θ')

Likewise, the damping constants $C_x$, $C_y$, $C_l$, and $C_θ$ induce generalized forces proportional to derivatives with respect to time of the generalized coordinates.

In [7]:

Cx, Cy, Cl, Cth = symbols('C_x C_y C_l C_θ')
generalized_damping_forces = Qdamping = Matrix([- Cx * dxdt, - Cy * dydt, - Cl * dldt, - Cth * dthdt])
display(Latex('$\mathbf{Q}_{damping} = \,$' + Qdamping._repr_latex_()))

$\mathbf{Q}_{damping} = \,$$\displaystyle \left[\begin{matrix}- C_{x} \frac{d}{d t} x{\left(t \right)}\\- C_{y} \frac{d}{d t} y{\left(t \right)}\\- C_{l} \frac{d}{d t} l{\left(t \right)}\\- C_{θ} \frac{d}{d t} θ{\left(t \right)}\end{matrix}\right]$

The generalized input forces $Q_x$, $Q_y$, $Q_l$, and $Q_θ$ represent the control variables, from either player inputs or the computer control logic algorithm.

In [8]:

Qx, Qy, Ql, Qth = symbols('Q_x Q_y Q_l Q_θ')
generalized_input_forces = Qinput = Matrix([Qx, Qy, Ql, Qth])
display(Latex('$\mathbf{Q}_{input} = \,$' + Qinput._repr_latex_()))

$\mathbf{Q}_{input} = \,$$\displaystyle \left[\begin{matrix}Q_{x}\\Q_{y}\\Q_{l}\\Q_{θ}\end{matrix}\right]$

Kinematics¶

The inertial reference frame is designated as R. The default convention in SymPy Mechanics is to represent the reference frame R using the unit vectors $\mathbf{\hat{r}}_x$, $\mathbf{\hat{r}}_y$, and $\mathbf{\hat{r}}_z$, however, these default symbols are replaced with unit vectors $\mathbf{\hat{i}}$, $\mathbf{\hat{j}}$, and $\mathbf{\hat{k}}$ for typesetting purposes, where $\mathbf{\hat{i}}$ is horizontal to the right side of the screen, $\mathbf{\hat{j}}$ is vertical and upward, and $\mathbf{\hat{k}}$ is out of the page. It is understood these are spatially fixed such that $\partial\mathbf{\hat{i}} / \partial t = \partial\mathbf{\hat{j}} / \partial t = \partial\mathbf{\hat{k}} / \partial t = 0$. The stool frame is designated as E, with the default unit vectors provided by SymPy Mechanics of $\mathbf{\hat{e}}_x$, $\mathbf{\hat{e}}_y$, and $\mathbf{\hat{e}}_z$, which are oriented relative to the reference frame R by a rotation of $\theta$ about the $\mathbf{\hat{k}}$ axis, where $\mathbf{\hat{k}}$ is accessed in the source code as R.z. The direction cosine matrix (DCM) that transforms from the R to the E frame is given by $\mathbf{R}_e$.

In [9]:

R = ReferenceFrame('R')
R.pretty_vecs = ['i', 'j', 'k']
R.latex_vecs = ['\\mathbf{\\hat{i}}', '\\mathbf{\\hat{j}}', '\\mathbf{\\hat{k}}']
E = ReferenceFrame('E')
E.orient(R, 'Axis', [th, R.z])
Re = E.dcm(R)
display(Latex('$\mathbf{R}_e = \,$' + Re._repr_latex_()))

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

The origin is defined as a Point designated O, and its velocity is set to zero using the .set_vel() method. The ensuing points of interest will be defined relative to the origin.

In [10]:

origin = Point('O')
origin.set_vel(R, 0)

Positions¶

The player’s center of mass is located at the point p, which has the position vector $\mathbf{r}_p$ given by the generalized coordinates $x$ and $y$, measured relative to the origin in the R frame. The .set_pos() method is used to define this position, with the source code R.x and R.y to access the unit vectors $\mathbf{\hat{i}}$ and $\mathbf{\hat{j}}$, respectively.

In [11]:

player_cm = Point('p')
player_cm.set_pos(origin, x * R.x + y * R.y)
rp = player_cm.pos_from(origin)
display(Latex('$\mathbf{r}_p = \,$' + rp._repr_latex_()))

$\mathbf{r}_p = \,$$\displaystyle x\mathbf{\hat{i}} + y\mathbf{\hat{j}}$

The stool center of mass is located at the point s, which has the position vector $\mathbf{r}_s$, which is offset vertically from p by a constant distance $d$ in the $\partial\mathbf{\hat{j}}$ direction, and by $l(t)$ in the $\mathbf{\hat{e}}_y$ direction (accessed as E.y in the source code).

In [12]:

rotation_center = Point('d')
rotation_center.set_pos(player_cm, d * R.y)
stool_cm = Point('s')
stool_cm.set_pos(rotation_center, l * E.y)
rs = stool_cm.pos_from(origin)
display(Latex('$\mathbf{r}_s = \,$' + rs._repr_latex_() + '$\, = \,$' + rs.express(R)._repr_latex_()))

$\mathbf{r}_s = \,$$\displaystyle l\mathbf{\hat{e}_y} + x\mathbf{\hat{i}} + (d + y)\mathbf{\hat{j}}$$\, = \,$$\displaystyle (- l \operatorname{sin}\left(θ\right) + x)\mathbf{\hat{i}} + (d + l \operatorname{cos}\left(θ\right) + y)\mathbf{\hat{j}}$

Velocities¶

The velocity of the point p, given by $\mathbf{v}_p$, is calculated by taking its time derivative evaluated in the frame R.

In [13]:

vp = rp.dt(R) 
player_cm.set_vel(R, vp)
display(Latex('$\mathbf{v}_p = \,$' + vp._repr_latex_()))

$\mathbf{v}_p = \,$$\displaystyle \dot{x}\mathbf{\hat{i}} + \dot{y}\mathbf{\hat{j}}$

Similarly, the velocity of the point s, given by $\mathbf{v}_s$ is calculated in the same way. By default, the velocity of s relative to p is given in the rotated stool frame, with a tangential term $l\dot{\theta}\hat{\mathbf{e}}_x$ and radial term $\dot{l}\hat{\mathbf{e}}_y$. The .express() method is used to give the equivalent velocity in the inertial $\hat{\mathbf{i}}$ and $\hat{\mathbf{j}}$ coordinates.

In [14]:

vs = rs.dt(R) 
stool_cm.set_vel(R, vs)
display(Latex('$\mathbf{v}_s = \,$' + vs._repr_latex_() + '$\, = \,$' + vs.express(R)._repr_latex_()))

$\mathbf{v}_s = \,$$\displaystyle – l \dot{θ}\mathbf{\hat{e}_x} + \dot{l}\mathbf{\hat{e}_y} + \dot{x}\mathbf{\hat{i}} + \dot{y}\mathbf{\hat{j}}$$\, = \,$$\displaystyle (- l \operatorname{cos}\left(θ\right) \dot{θ} – \operatorname{sin}\left(θ\right) \dot{l} + \dot{x})\mathbf{\hat{i}} + (- l \operatorname{sin}\left(θ\right) \dot{θ} + \operatorname{cos}\left(θ\right) \dot{l} + \dot{y})\mathbf{\hat{j}}$

Lagrange’s Method¶

First the equations will be derived using Lagrange’s method, without utilizing the Mechanics module (except for using the position and velocity vectors already defined).

Energy¶

The kinetic energy, $T = \frac{1}{2} m_p \mathbf{v}_p \cdot \mathbf{v}_p + \frac{1}{2} m_s \mathbf{v}_s \cdot \mathbf{v}_s$, is calculated where $m_p$ and $m_s$ are the point masses assigned to the player and stool, and $(\,\cdot\,)$ represents the dot product.

In [15]:

T = simplify(0.5 * mp * dot(vp, vp) + 0.5 * ms * dot(vs, vs))
display(Latex('$T = $'), T)

$T = $

$\displaystyle 0.5 m_{p} \dot{x}^{2} + 0.5 m_{p} \dot{y}^{2} + 0.5 m_{s} l^{2} \dot{θ}^{2} – 1.0 m_{s} l \operatorname{sin}\left(θ\right) \dot{y} \dot{θ} – 1.0 m_{s} l \operatorname{cos}\left(θ\right) \dot{x} \dot{θ} – 1.0 m_{s} \operatorname{sin}\left(θ\right) \dot{l} \dot{x} + 1.0 m_{s} \operatorname{cos}\left(θ\right) \dot{l} \dot{y} + 0.5 m_{s} \dot{l}^{2} + 0.5 m_{s} \dot{x}^{2} + 0.5 m_{s} \dot{y}^{2}$

The potential energy $V$ is the sum of the gravitational potential $(m_p \mathbf{r}_p + m_s \mathbf{r_s}) \cdot g \hat{\mathbf{j}}$ plus spring potential $K_y (y – y_0)^2$, $K_l (l – l_0)^2$, and $K_\theta \theta^2$.

In [16]:

V = dot(mp * rp + ms * rs, grav * R.y) + 0.5 * ( Ky * (y - y0)**2 + Kl * (l - l0)**2 + Kt * th**2 )
V = simplify(V)
display(Latex('$V = $'), V)

$V = $

$\displaystyle 0.5 K_{l} \left(l_{0} – l\right)^{2} + 0.5 K_{y} \left(y_{0} – y\right)^{2} + 0.5 K_{θ} θ^{2} + g m_{s} l \operatorname{cos}\left(θ\right) + g \left(m_{p} y + m_{s} \left(d + y\right)\right)$

The Lagrangian $L = T – V$

In [17]:

L = T - V
display(Latex('$L = T - V = $'), L)

$L = T – V = $

$\displaystyle – 0.5 K_{l} \left(l_{0} – l\right)^{2} – 0.5 K_{y} \left(y_{0} – y\right)^{2} – 0.5 K_{θ} θ^{2} – g m_{s} l \operatorname{cos}\left(θ\right) – g \left(m_{p} y + m_{s} \left(d + y\right)\right) + 0.5 m_{p} \dot{x}^{2} + 0.5 m_{p} \dot{y}^{2} + 0.5 m_{s} l^{2} \dot{θ}^{2} – 1.0 m_{s} l \operatorname{sin}\left(θ\right) \dot{y} \dot{θ} – 1.0 m_{s} l \operatorname{cos}\left(θ\right) \dot{x} \dot{θ} – 1.0 m_{s} \operatorname{sin}\left(θ\right) \dot{l} \dot{x} + 1.0 m_{s} \operatorname{cos}\left(θ\right) \dot{l} \dot{y} + 0.5 m_{s} \dot{l}^{2} + 0.5 m_{s} \dot{x}^{2} + 0.5 m_{s} \dot{y}^{2}$

Equations of Motion¶

Equations of motion are calculated using Lagrange’s method where $\frac{d}{dt} \left( \frac{\partial L}{\partial\dot{\mathbf{q}}}\right) – \frac{\partial L}{\partial \mathbf{q}} = \mathbf{Q}_{damping} + \mathbf{Q}_{input}$, or equivalently $\frac{d}{dt} \left( \frac{\partial L}{\partial\dot{\mathbf{q}}}\right) – \frac{\partial L}{\partial \mathbf{q}} – \mathbf{Q}_{damping} – \mathbf{Q}_{input} = \mathbf{0}$.

In [18]:

# Initialize the eom matrix
eom = Matrix([None, None, None, None])

for k in range(4):
    # Apply Lagrange's method
    tmp = diff(diff(L, dqdt[k]), t) - diff(L, q[k]) - Qdamping[k] - Qinput[k]
    
    # Simplify and insert into eom matrix
    eom[k] = simplify(tmp)

    # Display result
    display(Latex('EOM for ' + q[k]._repr_latex_() + ' is : '), eom[k])

EOM for $\displaystyle x{\left(t \right)}$ is :

$\displaystyle C_{x} \dot{x} – Q_{x} + 1.0 m_{p} \ddot{x} + 1.0 m_{s} l \operatorname{sin}\left(θ\right) \dot{θ}^{2} – 1.0 m_{s} l \operatorname{cos}\left(θ\right) \ddot{θ} – 1.0 m_{s} \operatorname{sin}\left(θ\right) \ddot{l} – 2.0 m_{s} \operatorname{cos}\left(θ\right) \dot{l} \dot{θ} + 1.0 m_{s} \ddot{x}$

EOM for $\displaystyle y{\left(t \right)}$ is :

$\displaystyle C_{y} \dot{y} – 1.0 K_{y} \left(y_{0} – y\right) – Q_{y} + g \left(m_{p} + m_{s}\right) + 1.0 m_{p} \ddot{y} – 1.0 m_{s} l \operatorname{sin}\left(θ\right) \ddot{θ} – 1.0 m_{s} l \operatorname{cos}\left(θ\right) \dot{θ}^{2} – 2.0 m_{s} \operatorname{sin}\left(θ\right) \dot{l} \dot{θ} + 1.0 m_{s} \operatorname{cos}\left(θ\right) \ddot{l} + 1.0 m_{s} \ddot{y}$

EOM for $\displaystyle l{\left(t \right)}$ is :

$\displaystyle 1.0 C_{l} \dot{l} – 1.0 K_{l} l_{0} + 1.0 K_{l} l – 1.0 Q_{l} + 1.0 g m_{s} \operatorname{cos}\left(θ\right) – 1.0 m_{s} l \dot{θ}^{2} – 1.0 m_{s} \operatorname{sin}\left(θ\right) \ddot{x} + 1.0 m_{s} \operatorname{cos}\left(θ\right) \ddot{y} + 1.0 m_{s} \ddot{l}$

EOM for $\displaystyle θ{\left(t \right)}$ is :

$\displaystyle C_{θ} \dot{θ} + 1.0 K_{θ} θ – Q_{θ} – g m_{s} l \operatorname{sin}\left(θ\right) + 1.0 m_{s} l^{2} \ddot{θ} – 1.0 m_{s} l \operatorname{sin}\left(θ\right) \ddot{y} – 1.0 m_{s} l \operatorname{cos}\left(θ\right) \ddot{x} + 2.0 m_{s} l \dot{l} \dot{θ}$

Integrable Form¶

Next solve the equations of motion for the accelerations, $\partial{\boldsymbol{q}} / \partial t$, using the SymPy solve function. This returns a dictionary where the keys are the symbolic variables for $\partial \mathbf{q} / \partial t$. The loop simplifies the solution equations, and places them into a list that is given in the original order of eom.

In [19]:

# Solve for the accelerations
eom_solved = solve(eom, d2qdt2)

# Initialize the eoms matrix
eoms = Matrix([None, None, None, None])

for k in range(4):
    # Insert the equation from the eom_solved dictionary indexed using d2qdt2
    eoms[k] = simplify(eom_solved[d2qdt2[k]])
    
    # Display the equation
    print(' ')
    display(Latex(d2qdt2[k]._repr_latex_() + '$\, = \,$'), eoms[k])

$\displaystyle \frac{d^{2}}{d t^{2}} x{\left(t \right)}$$\, = \,$

$\displaystyle \frac{- C_{l} l \operatorname{sin}\left(θ\right) \dot{l} – C_{x} l \dot{x} – C_{θ} \operatorname{cos}\left(θ\right) \dot{θ} + K_{l} l_{0} l \operatorname{sin}\left(θ\right) – K_{l} l^{2} \operatorname{sin}\left(θ\right) – K_{θ} θ \operatorname{cos}\left(θ\right) + Q_{l} l \operatorname{sin}\left(θ\right) + Q_{x} l + Q_{θ} \operatorname{cos}\left(θ\right)}{m_{p} l}$

$\displaystyle \frac{d^{2}}{d t^{2}} y{\left(t \right)}$$\, = \,$

$\displaystyle \frac{- g m_{p} l + \left(- C_{θ} \dot{θ} – K_{θ} θ + Q_{θ}\right) \operatorname{sin}\left(θ\right) + \left(C_{l} \operatorname{cos}\left(θ\right) \dot{l} – C_{y} \dot{y} – K_{l} l_{0} \operatorname{cos}\left(θ\right) + K_{l} l \operatorname{cos}\left(θ\right) + K_{y} y_{0} – K_{y} y – Q_{l} \operatorname{cos}\left(θ\right) + Q_{y}\right) l}{m_{p} l}$

$\displaystyle \frac{d^{2}}{d t^{2}} l{\left(t \right)}$$\, = \,$

$\displaystyle \frac{m_{p} m_{s} l \dot{θ}^{2} + m_{p} \left(- C_{l} \dot{l} + K_{l} l_{0} – K_{l} l + Q_{l}\right) + m_{s} \left(- C_{l} \dot{l} – C_{x} \operatorname{sin}\left(θ\right) \dot{x} + C_{y} \operatorname{cos}\left(θ\right) \dot{y} + K_{l} l_{0} – K_{l} l – K_{y} y_{0} \operatorname{cos}\left(θ\right) + K_{y} y \operatorname{cos}\left(θ\right) + Q_{l} + Q_{x} \operatorname{sin}\left(θ\right) – Q_{y} \operatorname{cos}\left(θ\right)\right)}{m_{p} m_{s}}$

$\displaystyle \frac{d^{2}}{d t^{2}} θ{\left(t \right)}$$\, = \,$

$\displaystyle \frac{- C_{x} m_{s} l \operatorname{cos}\left(θ\right) \dot{x} – C_{y} m_{s} l \operatorname{sin}\left(θ\right) \dot{y} – C_{θ} m_{p} \dot{θ} – C_{θ} m_{s} \dot{θ} + K_{y} m_{s} y_{0} l \operatorname{sin}\left(θ\right) – K_{y} m_{s} l y \operatorname{sin}\left(θ\right) – K_{θ} m_{p} θ – K_{θ} m_{s} θ + Q_{x} m_{s} l \operatorname{cos}\left(θ\right) + Q_{y} m_{s} l \operatorname{sin}\left(θ\right) + Q_{θ} m_{p} + Q_{θ} m_{s} – 2.0 m_{p} m_{s} l \dot{l} \dot{θ}}{m_{p} m_{s} l^{2}}$

Built-in Methods¶

Now the EOM are derived using Kane’s method as-implemented in Sympy Mechanics.

Generalized Speeds¶

To facilitate EOM generation using Kane’s method, we define a set of generalized speeds. In this case, we will use substition variables $u = \frac{\partial x}{\partial t}$, $v = \frac{\partial y}{\partial t}$, $w = \frac{\partial l}{\partial t}$, and $ω = \frac{\partial θ}{\partial t}$.

In [20]:

u, v, w, omeg = dynamicsymbols('u v w ω')
generalized_speeds = s = Matrix([u, v, w, omeg])
dudt, dvdt, dwdt, domegdt = dynamicsymbols('u v w ω', 1)
ddt_generalized_speeds = dsdt = Matrix([dudt, dvdt, dwdt, domegdt])
display(Latex('$\mathbf{s} = \,$' + generalized_speeds._repr_latex_()))

$\mathbf{s} = \,$$\displaystyle \left[\begin{matrix}u{\left(t \right)}\\v{\left(t \right)}\\w{\left(t \right)}\\ω{\left(t \right)}\end{matrix}\right]$

A kinematic equation will be used in Kane’s method to define the equality $\mathbf{s} = \dot{\mathbf{q}}$, equivalently $\mathbf{s} – \dot{\mathbf{q}} = \mathbf{0}$. The latter convention is commonly used by the SymPy solvers, where a matrix representing a system of equations will be assumed to be in equilibrium, or in other words equal to a matrix of zeros with the same dimension.

In [21]:

kinematic_equation = s - dqdt
display(kinematic_equation)

$\displaystyle \left[\begin{matrix}u – \dot{x}\\v – \dot{y}\\w – \dot{l}\\ω – \dot{θ}\end{matrix}\right]$

Bodies¶

The player and stool are both defined as Particle objects, in other words point masses. The first argument to the class is the name of the particle, second article is a Point object corresponding to the centers of mass, and third object is the mass, in this case symbolic. These are added to the bodies list.

In [22]:

player = Particle('player', player_cm, mp)
stool = Particle('stool', stool_cm, ms)
bodies = [player, stool]

Loads¶

The bodies are acted on by various forces and moments, including from gravity, spring extension, damping, and player input. For each constituent load, the force or moment terms are placed in a tuple, where the first term in said tuple is the load vector acting on the player, and the second is the load vector acting on the stool. The loads variable is a list of tuples, where in each tuple the first entry is either a Point (for forces) or RefereneFrame (for moments) object, and the second entry in the tuple is the net force or moment vector acting on said object.

In [23]:

gravitational_forces = Fg = - mp * grav * R.y, -ms * grav * R.y
spring_forces = Fk = - Ky * (y - y0) * R.y + Kl * (l - l0) * E.y, - Kl * (l - l0) * E.y
spring_moments = Mk = Kt * th * E.z, - Kt * th * E.z
damping_forces = Fd = - Cx * dxdt * R.x - Cy * dydt * R.y + Cl * dldt * E.y, - Cl * dldt * E.y
damping_moments = Md = Cth * dthdt * E.z, - Cth * dthdt * E.z
input_forces = Fi = Qx * R.x + Qy * R.y - Ql * E.y, Ql * E.y
input_moments = Mi = - Qth * E.z, Qth * E.z
loads = [(player_cm, Fg[0] + Fk[0] + Fd[0] + Fi[0]),  # Forces on the player
         (stool_cm, Fg[1] + Fk[1] + Fd[1] + Fi[1]),   # Forces on the stool
         (R, Mk[0] + Md[0] + Mi[0]),                  # Moments on the player, R-frame
         (E, Mk[1] + Md[1] + Mi[1])]                  # Moments on the stool, E-frame
display(Latex('Gravitational Forces : '), Fg)
display(Latex('Spring Forces : '), Fk)
display(Latex('Spring Moments : '), Mk)
display(Latex('Damping Forces : '), Fd)
display(Latex('Damping Moments : '), Md)
display(Latex('Input Forces : '), Fi)
display(Latex('Input Moments : '), Mi)
# display(Latex('Net Forces and Moments: '), loads)

Gravitational Forces :

$\displaystyle \left( – g m_{p}\mathbf{\hat{j}}, \ – g m_{s}\mathbf{\hat{j}}\right)$

Spring Forces :

$\displaystyle \left( – K_{y} \left(- y_{0} + y\right)\mathbf{\hat{j}} + K_{l} \left(- l_{0} + l\right)\mathbf{\hat{e}_y}, \ – K_{l} \left(- l_{0} + l\right)\mathbf{\hat{e}_y}\right)$

Spring Moments :

$\displaystyle \left( K_{θ} θ\mathbf{\hat{e}_z}, \ – K_{θ} θ\mathbf{\hat{e}_z}\right)$

Damping Forces :

$\displaystyle \left( – C_{x} \dot{x}\mathbf{\hat{i}} – C_{y} \dot{y}\mathbf{\hat{j}} + C_{l} \dot{l}\mathbf{\hat{e}_y}, \ – C_{l} \dot{l}\mathbf{\hat{e}_y}\right)$

Damping Moments :

$\displaystyle \left( C_{θ} \dot{θ}\mathbf{\hat{e}_z}, \ – C_{θ} \dot{θ}\mathbf{\hat{e}_z}\right)$

Input Forces :

$\displaystyle \left( Q_{x}\mathbf{\hat{i}} + Q_{y}\mathbf{\hat{j}} – Q_{l}\mathbf{\hat{e}_y}, \ Q_{l}\mathbf{\hat{e}_y}\right)$

Input Moments :

$\displaystyle \left( – Q_{θ}\mathbf{\hat{e}_z}, \ Q_{θ}\mathbf{\hat{e}_z}\right)$

Kane’s Equations¶

The built-in Kane’s Method class is used to derive EOM. When initiating the object, the first argument is the reference frame R, then the keyword arguments q_ind for the generalized coordinate vector, u_ind for the generalized speed vector, and kd_eqs for the kinematic equations relating the two. The .kanes_equations() method is called for the defined bodies and loads, which yields $\mathbf{F}_r$, which are the applied and constraint forces, and $\mathbf{F}_r^*$, which are the body forces, respectively. The class also automates the generation of the mass matrix $\mathbf{M}$ and forcing vector $\mathbf{F}$, which can be used to shorten the system of equations to $\mathbf{M}\dot{\mathbf{s}} = \mathbf{F}$.

In [24]:

kane = KanesMethod(R, q_ind=q, u_ind=s, kd_eqs=kinematic_equation)
fr, frstar = kane.kanes_equations(bodies, loads)
display(Latex('$\mathbf{F}_r = $'), fr)
display(Latex('$\mathbf{F}_r^* = $'), frstar)
display(Latex('$\mathbf{M} = $'), kane.mass_matrix)
display(Latex('$\mathbf{F} = $'), kane.forcing)

$\mathbf{F}_r = $

$\displaystyle \left[\begin{matrix}- C_{x} \dot{x} + Q_{x} – \left(- C_{l} \dot{l} – K_{l} \left(- l_{0} + l\right) + Q_{l}\right) \operatorname{sin}\left(θ\right) – \left(C_{l} \dot{l} + K_{l} \left(- l_{0} + l\right) – Q_{l}\right) \operatorname{sin}\left(θ\right)\\- C_{y} \dot{y} – K_{y} \left(- y_{0} + y\right) + Q_{y} – g m_{p} – g m_{s} + \left(- C_{l} \dot{l} – K_{l} \left(- l_{0} + l\right) + Q_{l}\right) \operatorname{cos}\left(θ\right) + \left(C_{l} \dot{l} + K_{l} \left(- l_{0} + l\right) – Q_{l}\right) \operatorname{cos}\left(θ\right)\\- C_{l} \dot{l} – K_{l} \left(- l_{0} + l\right) + Q_{l} – g m_{s} \operatorname{cos}\left(θ\right)\\- C_{θ} \dot{θ} – K_{θ} θ + Q_{θ} + g m_{s} l \operatorname{sin}\left(θ\right)\end{matrix}\right]$

$\mathbf{F}_r^* = $

$\displaystyle \left[\begin{matrix}- m_{s} l ω^{2} \operatorname{sin}\left(θ\right) + m_{s} l \operatorname{cos}\left(θ\right) \dot{ω} + 2 m_{s} w ω \operatorname{cos}\left(θ\right) + m_{s} \operatorname{sin}\left(θ\right) \dot{w} – \left(m_{p} + m_{s}\right) \dot{u}\\m_{s} l ω^{2} \operatorname{cos}\left(θ\right) + m_{s} l \operatorname{sin}\left(θ\right) \dot{ω} + 2 m_{s} w ω \operatorname{sin}\left(θ\right) – m_{s} \operatorname{cos}\left(θ\right) \dot{w} – \left(m_{p} + m_{s}\right) \dot{v}\\m_{s} l ω^{2} + m_{s} \operatorname{sin}\left(θ\right) \dot{u} – m_{s} \operatorname{cos}\left(θ\right) \dot{v} – m_{s} \dot{w}\\- m_{s} l^{2} \dot{ω} – 2 m_{s} l w ω + m_{s} l \operatorname{sin}\left(θ\right) \dot{v} + m_{s} l \operatorname{cos}\left(θ\right) \dot{u}\end{matrix}\right]$

$\mathbf{M} = $

$\displaystyle \left[\begin{matrix}m_{p} + m_{s} & 0 & – m_{s} \operatorname{sin}\left(θ\right) & – m_{s} l \operatorname{cos}\left(θ\right)\\0 & m_{p} + m_{s} & m_{s} \operatorname{cos}\left(θ\right) & – m_{s} l \operatorname{sin}\left(θ\right)\\- m_{s} \operatorname{sin}\left(θ\right) & m_{s} \operatorname{cos}\left(θ\right) & m_{s} & 0\\- m_{s} l \operatorname{cos}\left(θ\right) & – m_{s} l \operatorname{sin}\left(θ\right) & 0 & m_{s} l^{2}\end{matrix}\right]$

$\mathbf{F} = $

$\displaystyle \left[\begin{matrix}- C_{x} \dot{x} + Q_{x} – m_{s} l ω^{2} \operatorname{sin}\left(θ\right) + 2 m_{s} w ω \operatorname{cos}\left(θ\right) – \left(- C_{l} \dot{l} – K_{l} \left(- l_{0} + l\right) + Q_{l}\right) \operatorname{sin}\left(θ\right) – \left(C_{l} \dot{l} + K_{l} \left(- l_{0} + l\right) – Q_{l}\right) \operatorname{sin}\left(θ\right)\\- C_{y} \dot{y} – K_{y} \left(- y_{0} + y\right) + Q_{y} – g m_{p} – g m_{s} + m_{s} l ω^{2} \operatorname{cos}\left(θ\right) + 2 m_{s} w ω \operatorname{sin}\left(θ\right) + \left(- C_{l} \dot{l} – K_{l} \left(- l_{0} + l\right) + Q_{l}\right) \operatorname{cos}\left(θ\right) + \left(C_{l} \dot{l} + K_{l} \left(- l_{0} + l\right) – Q_{l}\right) \operatorname{cos}\left(θ\right)\\- C_{l} \dot{l} – K_{l} \left(- l_{0} + l\right) + Q_{l} – g m_{s} \operatorname{cos}\left(θ\right) + m_{s} l ω^{2}\\- C_{θ} \dot{θ} – K_{θ} θ + Q_{θ} + g m_{s} l \operatorname{sin}\left(θ\right) – 2 m_{s} l w ω\end{matrix}\right]$

Integrable form¶

As with the prior Lagrange method solution, we solve the system of equations to yield the derivatives with respect to time of the generalized speeds, which are equivalent to the generalized accelerations $\ddot{\mathbf{q}}$. We verify that the built-in Kane’s method yielded the same solution as the earlier Lagrange method by subtracting the two equation vectors. In this verification step, we make the substitutions $v=\frac{\partial l}{\partial t}$ and $w=\frac{\partial \omega}{\partial t}$, which ensures that the same symbols are used in each.

In [25]:

# Solve for the accelerations (derivatives of generalized speeds)
kane_solved = solve(fr + frstar, dsdt)

# Initialize the kanes matrix
kanes = Matrix([None, None, None, None])

for k in range(4):
    # Insert the equation from the eom_solved dictionary indexed using d2qdt2
    kanes[k] = simplify(kane_solved[dsdt[k]])
    
    # Display the equation
    display(Latex(dsdt[k]._repr_latex_() + '$\, = \,$'), kanes[k])  

$\displaystyle \frac{d}{d t} u{\left(t \right)}$$\, = \,$

$\displaystyle \frac{d}{d t} v{\left(t \right)}$$\, = \,$

$\displaystyle \frac{d}{d t} w{\left(t \right)}$$\, = \,$

$\displaystyle \frac{m_{p} m_{s} l ω^{2} + m_{p} \left(- C_{l} \dot{l} + K_{l} l_{0} – K_{l} l + Q_{l}\right) + m_{s} \left(- C_{l} \dot{l} – C_{x} \operatorname{sin}\left(θ\right) \dot{x} + C_{y} \operatorname{cos}\left(θ\right) \dot{y} + K_{l} l_{0} – K_{l} l – K_{y} y_{0} \operatorname{cos}\left(θ\right) + K_{y} y \operatorname{cos}\left(θ\right) + Q_{l} + Q_{x} \operatorname{sin}\left(θ\right) – Q_{y} \operatorname{cos}\left(θ\right)\right)}{m_{p} m_{s}}$

$\displaystyle \frac{d}{d t} ω{\left(t \right)}$$\, = \,$

In [26]:

display(simplify(kanes-eoms).subs(s[2], dqdt[2]).subs(s[3], dqdt[3]))

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

Lagrange’s Method¶

The built-in LagrangesMethod class can be easily initiated using the Lagrangian L and generalized force vector q that were defined earlier, which are the first two arguments when initializing the object. The optional keyword argument forcelist is used, which is created using the same procedure as the loads used in Kane’s method, but omitting gravitational and spring forces which are included in the potential energy definition. When using the forcelist keyword argument, the additional frame keyword argument is also required, which provides the inertial reference frame R. Similar to prior methods, we can call the mass matrix and forcing vectors, and subtract the newly derived EOMs from the prior methods to compare and verify the same result.

In [27]:

# Build the list of forces acting on points, or moments acting on frames
force_list = [(player_cm, Fd[0] + Fi[0]),  # Forces on the player
              (stool_cm, Fd[1] + Fi[1]),   # Forces on the stool
              (R, Md[0] + Mi[0]),          # Moments on the player, R-frame
              (E, Md[1] + Mi[1])]          # Moments on the stool, E-frame

# Initialize the LagrangesMethod object
LM = LagrangesMethod(L, q, forcelist=force_list, frame=R)

# Call the internal method to calculate EOM
lagrange_eom = simplify(LM.form_lagranges_equations())
display(lagrange_eom)

# Display the mass matrix
display(Latex('$\mathbf{M} = \,$' + LM.mass_matrix._repr_latex_()))

# Display the forcing matrix
forcing = simplify(LM.forcing)
display(Latex('$\mathbf{F} = \, $' + forcing._repr_latex_()))

# Verify same result as before
display(eom - lagrange_eom)  

$\displaystyle \left[\begin{matrix}C_{x} \dot{x} – Q_{x} + 1.0 m_{p} \ddot{x} + 1.0 m_{s} l \operatorname{sin}\left(θ\right) \dot{θ}^{2} – 1.0 m_{s} l \operatorname{cos}\left(θ\right) \ddot{θ} – 1.0 m_{s} \operatorname{sin}\left(θ\right) \ddot{l} – 2.0 m_{s} \operatorname{cos}\left(θ\right) \dot{l} \dot{θ} + 1.0 m_{s} \ddot{x}\\C_{y} \dot{y} – 1.0 K_{y} \left(y_{0} – y\right) – Q_{y} + g \left(m_{p} + m_{s}\right) + 1.0 m_{p} \ddot{y} – 1.0 m_{s} l \operatorname{sin}\left(θ\right) \ddot{θ} – 1.0 m_{s} l \operatorname{cos}\left(θ\right) \dot{θ}^{2} – 2.0 m_{s} \operatorname{sin}\left(θ\right) \dot{l} \dot{θ} + 1.0 m_{s} \operatorname{cos}\left(θ\right) \ddot{l} + 1.0 m_{s} \ddot{y}\\1.0 C_{l} \dot{l} – 1.0 K_{l} l_{0} + 1.0 K_{l} l – 1.0 Q_{l} + 1.0 g m_{s} \operatorname{cos}\left(θ\right) – 1.0 m_{s} l \dot{θ}^{2} – 1.0 m_{s} \operatorname{sin}\left(θ\right) \ddot{x} + 1.0 m_{s} \operatorname{cos}\left(θ\right) \ddot{y} + 1.0 m_{s} \ddot{l}\\C_{θ} \dot{θ} + 1.0 K_{θ} θ – Q_{θ} – g m_{s} l \operatorname{sin}\left(θ\right) + 1.0 m_{s} l^{2} \ddot{θ} – 1.0 m_{s} l \operatorname{sin}\left(θ\right) \ddot{y} – 1.0 m_{s} l \operatorname{cos}\left(θ\right) \ddot{x} + 2.0 m_{s} l \dot{l} \dot{θ}\end{matrix}\right]$

$\mathbf{M} = \,$$\displaystyle \left[\begin{matrix}1.0 m_{p} + 1.0 m_{s} & 0 & – 1.0 m_{s} \sin{\left(θ{\left(t \right)} \right)} & – 1.0 m_{s} l{\left(t \right)} \cos{\left(θ{\left(t \right)} \right)}\\0 & 1.0 m_{p} + 1.0 m_{s} & 1.0 m_{s} \cos{\left(θ{\left(t \right)} \right)} & – 1.0 m_{s} l{\left(t \right)} \sin{\left(θ{\left(t \right)} \right)}\\- 1.0 m_{s} \sin{\left(θ{\left(t \right)} \right)} & 1.0 m_{s} \cos{\left(θ{\left(t \right)} \right)} & 1.0 m_{s} & 0\\- 1.0 m_{s} l{\left(t \right)} \cos{\left(θ{\left(t \right)} \right)} & – 1.0 m_{s} l{\left(t \right)} \sin{\left(θ{\left(t \right)} \right)} & 0 & 1.0 m_{s} l^{2}{\left(t \right)}\end{matrix}\right]$

$\mathbf{F} = \, $$\displaystyle \left[\begin{matrix}- C_{x} \frac{d}{d t} x{\left(t \right)} + Q_{x} – 1.0 m_{s} l{\left(t \right)} \sin{\left(θ{\left(t \right)} \right)} \left(\frac{d}{d t} θ{\left(t \right)}\right)^{2} + 2.0 m_{s} \cos{\left(θ{\left(t \right)} \right)} \frac{d}{d t} l{\left(t \right)} \frac{d}{d t} θ{\left(t \right)}\\- C_{y} \frac{d}{d t} y{\left(t \right)} + K_{y} \left(y_{0} – y{\left(t \right)}\right) + Q_{y} – g \left(m_{p} + m_{s}\right) + 1.0 m_{s} l{\left(t \right)} \cos{\left(θ{\left(t \right)} \right)} \left(\frac{d}{d t} θ{\left(t \right)}\right)^{2} + 2.0 m_{s} \sin{\left(θ{\left(t \right)} \right)} \frac{d}{d t} l{\left(t \right)} \frac{d}{d t} θ{\left(t \right)}\\- C_{l} \frac{d}{d t} l{\left(t \right)} + K_{l} \left(l_{0} – l{\left(t \right)}\right) + Q_{l} – g m_{s} \cos{\left(θ{\left(t \right)} \right)} + 1.0 m_{s} l{\left(t \right)} \left(\frac{d}{d t} θ{\left(t \right)}\right)^{2}\\- C_{θ} \frac{d}{d t} θ{\left(t \right)} – 1.0 K_{θ} θ{\left(t \right)} + Q_{θ} + g m_{s} l{\left(t \right)} \sin{\left(θ{\left(t \right)} \right)} – 2.0 m_{s} l{\left(t \right)} \frac{d}{d t} l{\left(t \right)} \frac{d}{d t} θ{\left(t \right)}\end{matrix}\right]$

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

An alternate approach to using the LagrangesMethod class is to use the built-in Lagrangian function to create the variable $L$. By default, the bodies (player and stool) have known kinetic energy as a function of the mass and velocities that were previously assigned. If we don’t assign potential energy to the bodies, then calling the Lagrangian function yields $L=T$. In this case, the gravitational and spring forces can be brought into the equations using the loads list previously defined when applying Kane’s method.

In [28]:

# Generate the Lagrangian, first argument is the reference frame, additional arguments are the bodies
L2 = Lagrangian(R, player, stool)

# Initialize the LagrangesMethod object using LG, using the loads list that includes gravity and springs
LM2 = LagrangesMethod(L2, q, forcelist=loads, frame=R)

# Call the internal method to calculate EOM
lagrange_eom2 = simplify(LM2.form_lagranges_equations())
display(lagrange_eom2)

# Verify same result as before
display(simplify(lagrange_eom2 - eom))

$\displaystyle \left[\begin{matrix}C_{x} \dot{x} – Q_{x} + m_{p} \ddot{x} + m_{s} l \operatorname{sin}\left(θ\right) \dot{θ}^{2} – m_{s} l \operatorname{cos}\left(θ\right) \ddot{θ} – m_{s} \operatorname{sin}\left(θ\right) \ddot{l} – 2 m_{s} \operatorname{cos}\left(θ\right) \dot{l} \dot{θ} + m_{s} \ddot{x}\\C_{y} \dot{y} – K_{y} y_{0} + K_{y} y – Q_{y} + g m_{p} + g m_{s} + m_{p} \ddot{y} – m_{s} l \operatorname{sin}\left(θ\right) \ddot{θ} – m_{s} l \operatorname{cos}\left(θ\right) \dot{θ}^{2} – 2 m_{s} \operatorname{sin}\left(θ\right) \dot{l} \dot{θ} + m_{s} \operatorname{cos}\left(θ\right) \ddot{l} + m_{s} \ddot{y}\\C_{l} \dot{l} – K_{l} l_{0} + K_{l} l – Q_{l} + g m_{s} \operatorname{cos}\left(θ\right) – m_{s} l \dot{θ}^{2} – m_{s} \operatorname{sin}\left(θ\right) \ddot{x} + m_{s} \operatorname{cos}\left(θ\right) \ddot{y} + m_{s} \ddot{l}\\C_{θ} \dot{θ} + K_{θ} θ – Q_{θ} – g m_{s} l \operatorname{sin}\left(θ\right) + m_{s} l^{2} \ddot{θ} – m_{s} l \operatorname{sin}\left(θ\right) \ddot{y} – m_{s} l \operatorname{cos}\left(θ\right) \ddot{x} + 2 m_{s} l \dot{l} \dot{θ}\end{matrix}\right]$

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

Finally, we may also define the player and stool potential energy manually. In this case, force_list should be of the reduced form where the graviational and spring force terms are removed.

In [29]:

# Define the player and stool potential energy
player.potential_energy = mp * grav * y + 0.5 * Ky * (y -  y0)**2
stool.potential_energy = ms * grav * (y + d + l * cos(th)) + 1/2 * Kl * (l - l0)**2 + 1/2 * Kt * th**2

# Generate the Lagrangian that includes potential energy
L3 = simplify(Lagrangian(R, player, stool))

# Initialize the LagrangesMethod object using LG, using the loads list that includes gravity and springs
LM3 = LagrangesMethod(L3, q, forcelist=force_list, frame=R)

# Call the internal method to calculate EOM
lagrange_eom3 = simplify(LM3.form_lagranges_equations())
display(lagrange_eom3)

# Verify same result as before
display(simplify(lagrange_eom3 - eom))

$\displaystyle \left[\begin{matrix}C_{x} \dot{x} – Q_{x} + 1.0 m_{p} \ddot{x} + 1.0 m_{s} l \operatorname{sin}\left(θ\right) \dot{θ}^{2} – 1.0 m_{s} l \operatorname{cos}\left(θ\right) \ddot{θ} – 1.0 m_{s} \operatorname{sin}\left(θ\right) \ddot{l} – 2.0 m_{s} \operatorname{cos}\left(θ\right) \dot{l} \dot{θ} + 1.0 m_{s} \ddot{x}\\C_{y} \dot{y} – 1.0 K_{y} y_{0} + 1.0 K_{y} y – Q_{y} + 1.0 g m_{p} + 1.0 g m_{s} + 1.0 m_{p} \ddot{y} – 1.0 m_{s} l \operatorname{sin}\left(θ\right) \ddot{θ} – 1.0 m_{s} l \operatorname{cos}\left(θ\right) \dot{θ}^{2} – 2.0 m_{s} \operatorname{sin}\left(θ\right) \dot{l} \dot{θ} + 1.0 m_{s} \operatorname{cos}\left(θ\right) \ddot{l} + 1.0 m_{s} \ddot{y}\\1.0 C_{l} \dot{l} – 1.0 K_{l} l_{0} + 1.0 K_{l} l – 1.0 Q_{l} + 1.0 g m_{s} \operatorname{cos}\left(θ\right) – 1.0 m_{s} l \dot{θ}^{2} – 1.0 m_{s} \operatorname{sin}\left(θ\right) \ddot{x} + 1.0 m_{s} \operatorname{cos}\left(θ\right) \ddot{y} + 1.0 m_{s} \ddot{l}\\C_{θ} \dot{θ} + 1.0 K_{θ} θ – Q_{θ} – 1.0 g m_{s} l \operatorname{sin}\left(θ\right) + 1.0 m_{s} l^{2} \ddot{θ} – 1.0 m_{s} l \operatorname{sin}\left(θ\right) \ddot{y} – 1.0 m_{s} l \operatorname{cos}\left(θ\right) \ddot{x} + 2.0 m_{s} l \dot{l} \dot{θ}\end{matrix}\right]$

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

Code Generation¶

Next we substitute variables into the equation to create printed text that can be inserted into ordinary Python source code. The printed output represents the code to be implemented in a function of the form du = f(u), where u is the list of state variables, and du are the derivatives of u with respect to time. Additionally, this code presumes the existence of a parameter structure p containing the constants, and generalized force variables Qx, Qy, Ql, Qth.

This is done in two steps, first using the .subs() method which replaces symbolic variables in the equation, but returns an object of the symbolic data type. The second step is to change to a string data type, then use the .replace() method to swap in new strings that are shorter and/or more compatible with standard Python source code. It is worth noting that this entire operation could be using the second, string replacement method, but the symbolic substition is inclued for demonstration purposes.

The process begins by defining symbolic or string replacements. The symbols in dq are shortened form for the dynamic symbols previously created in the dqdt. Where the latter would print long strings intended for typeset display (i.e. Derivative(x(t), t) which is typeset as $\frac{\partial}{\partial t} x(t)$), the replacement are 2 or 3 ascii characters (i.e. dx).

The list qstr are simply the named set of state variables in the list u, which are included for readability of the string formatted equations of motion.

The string replacements are defined in replist, which is a $N\times 2$ list. In each row, the first entry (replist[n][0]) is the “old string”, or expected pattern in the original string output, and the second entry (replist[n][1]) is the “new string”, which replaces it.

In [30]:

# Define shortened symbols to replace using the subs command
dx, dy, dl, dth = symbols('dx dy dl dθ')
dq = Matrix([dx, dy, dl, dth]) 

# Define the variable names that are included in the list 'u'
qstr = 'x', 'y', 'l', 'th', 'dx', 'dy', 'dl', 'dth'

# Define the list of variables to replace in the string
replist = [['sin(θ(t))', 's'], ['cos(θ(t))', 'c'],
           ['x(t)', 'x'], ['y(t)', 'y'], ['l(t)', 'l'], ['θ(t)', 'th'], ['dθ', 'dth'],
           ['x_0', 'p.x0'], ['y_0', 'p.y0'], ['l_0', 'p.l0'],  
           ['m_p', 'p.mp'], ['m_s', 'p.ms'], ['g', 'p.g'],
           ['K_x', 'p.Kx'], ['K_y', 'p.Ky'], ['K_l', 'p.Kl'], ['K_θ', 'p.Kth'],
           ['C_x', 'p.Cx'], ['C_y', 'p.Cy'], ['C_l', 'p.Cl'], ['C_θ', 'p.Cth'], 
           ['Q_x', 'Qx'], ['Q_y', 'Qy'],['Q_l', 'Ql'], ['Q_θ', 'Qth']]

# Initialize the string formatted EOMs
eoms_print = ['du[4] = ', 'du[5] = ', 'du[6] = ', 'du[7] = ']

# Print string that unpacks the state variables
for k in range(4):
    print(qstr[k], '= u[' + str(k) + ']')
    print(' ')
    
# Print the shortened sine and cosine    
print('s = sin(th)')
print(' ')
print('c = cos(th)')
print(' ')    

# Print the velocity assignments
for k in range(4):
    print('du[' + str(k) + '] =', qstr[k+4], '= u[' + str(k+4) + ']')
    print(' ')
    
for k in range(4):
    # Load a copy of the eoms into the tmp_symbolic variable
    tmp_symbolic = eoms[k]
    
    # Replace the velocity terms with shortened variable
    # names (i.e. Derivative(x(t), t) replaced with dx)
    for j in range(4):
        tmp_symbolic = tmp_symbolic.subs(dqdt[j], dq[j])
    
    # Create a temporary string formatted equation
    tmp_string = str(tmp_symbolic)
    
    # Replace old_string with new_string from the replist
    for old_string, new_string in replist:
        tmp_string = tmp_string.replace(old_string, new_string)

    # Replace the print string with tmp, and print result
    eoms_print[k] += tmp_string
    print(eoms_print[k])
    print(' ')

x = u[0]
 
y = u[1]
 
l = u[2]
 
th = u[3]
 
s = sin(th)
 
c = cos(th)
 
du[0] = dx = u[4]
 
du[1] = dy = u[5]
 
du[2] = dl = u[6]
 
du[3] = dth = u[7]
 
du[4] = (-p.Cl*dl*l*s - p.Cx*dx*l - p.Cth*dth*c + p.Kl*p.l0*l*s - p.Kl*l**2*s - p.Kth*th*c + Ql*l*s + Qx*l + Qth*c)/(p.mp*l)
 
du[5] = (-p.g*p.mp*l + (-p.Cth*dth - p.Kth*th + Qth)*s + (p.Cl*dl*c - p.Cy*dy - p.Kl*p.l0*c + p.Kl*l*c + p.Ky*p.y0 - p.Ky*y - Ql*c + Qy)*l)/(p.mp*l)
 
du[6] = (dth**2*p.mp*p.ms*l + p.mp*(-p.Cl*dl + p.Kl*p.l0 - p.Kl*l + Ql) + p.ms*(-p.Cl*dl - p.Cx*dx*s + p.Cy*dy*c + p.Kl*p.l0 - p.Kl*l - p.Ky*p.y0*c + p.Ky*y*c + Ql + Qx*s - Qy*c))/(p.mp*p.ms)
 
du[7] = (-p.Cx*dx*p.ms*l*c - p.Cy*dy*p.ms*l*s - p.Cth*dth*p.mp - p.Cth*dth*p.ms + p.Ky*p.ms*p.y0*l*s - p.Ky*p.ms*l*y*s - p.Kth*p.mp*th - p.Kth*p.ms*th + Qx*p.ms*l*c + Qy*p.ms*l*s + Qth*p.mp + Qth*p.ms - 2.0*dl*dth*p.mp*p.ms*l)/(p.mp*p.ms*l**2)

Mass Matrix¶

The mass matrix is calculated by taking the derivative of each equation of motion with respect to each acceleration term. The primary terms are found along the diagonal, the first three reflect the player and stool mass for the translational DOFs, and the fourth represents the effective moment of inertia for the stool mass eccentricity from its rotation center. The mass matrix inverse is also calculated.

In [31]:

M = Matrix([[diff(eom[j], d2qdt2[k]) for j in range(4)] for k in range(4)])
       
display(Latex('$\mathbf{M} = $'), M)        
display(Latex('$\mathbf{M}^{-1} = $'), simplify(M.inv()))

$\mathbf{M} = $

$\displaystyle \left[\begin{matrix}1.0 m_{p} + 1.0 m_{s} & 0 & – 1.0 m_{s} \operatorname{sin}\left(θ\right) & – 1.0 m_{s} l \operatorname{cos}\left(θ\right)\\0 & 1.0 m_{p} + 1.0 m_{s} & 1.0 m_{s} \operatorname{cos}\left(θ\right) & – 1.0 m_{s} l \operatorname{sin}\left(θ\right)\\- 1.0 m_{s} \operatorname{sin}\left(θ\right) & 1.0 m_{s} \operatorname{cos}\left(θ\right) & 1.0 m_{s} & 0\\- 1.0 m_{s} l \operatorname{cos}\left(θ\right) & – 1.0 m_{s} l \operatorname{sin}\left(θ\right) & 0 & 1.0 m_{s} l^{2}\end{matrix}\right]$

$\mathbf{M}^{-1} = $

$\displaystyle \left[\begin{matrix}\frac{1.0}{m_{p}} & 0 & \frac{1.0 \operatorname{sin}\left(θ\right)}{m_{p}} & \frac{1.0 \operatorname{cos}\left(θ\right)}{m_{p} l}\\0 & \frac{1.0}{m_{p}} & – \frac{1.0 \operatorname{cos}\left(θ\right)}{m_{p}} & \frac{1.0 \operatorname{sin}\left(θ\right)}{m_{p} l}\\\frac{1.0 \operatorname{sin}\left(θ\right)}{m_{p}} & – \frac{1.0 \operatorname{cos}\left(θ\right)}{m_{p}} & \frac{1.0}{m_{s}} + \frac{1.0}{m_{p}} & 0\\\frac{1.0 \operatorname{cos}\left(θ\right)}{m_{p} l} & \frac{1.0 \operatorname{sin}\left(θ\right)}{m_{p} l} & 0 & \frac{1.0 \left(m_{p} + m_{s}\right)}{m_{p} m_{s} l^{2}}\end{matrix}\right]$

Equations of Motion in dRuBbLe

All Notebooks

Equations of Motion in dRuBbLe¶

Setup¶

Generalized Coordinates¶

Constants¶

Kinematics¶

Positions¶

Velocities¶

Lagrange’s Method¶

Energy¶

Equations of Motion¶

Integrable Form¶

Built-in Methods¶

Generalized Speeds¶

Bodies¶

Loads¶

Kane’s Equations¶

Integrable form¶

Lagrange’s Method¶

Code Generation¶

Mass Matrix¶

Share this:

Like this: