10 Modeling a Ball Channel Pendulum

Below is a video of a simple cardboard pendulum that has a metal ball in a semi-circular channel mounted above the pendulum's rotational joint. It is an interesting dynamic system that can be constructed and experimented with. This system seems to behave like a single degree of freedom system, i.e. that the ball's location is a kinematic function of the pendulum's angle. But this may not actually be the case. It depends on the nature of the motion of the ball with respect to the channel. If the ball rolls without slipping in the channel it is a single degree of freedom system. If the ball can slip and roll it is at a minimum a two degree of freedom system. In this notebook we will derive the equations of motion of the system considering the ball slips and doesn't roll in the channel.

In [1]:
from IPython.display import YouTubeVideo
YouTubeVideo('3pJdkssUdfU', width=600, height=480)
Out[1]:

Imports and setup

In [2]:
import sympy as sm
import numpy as np
In [3]:
sm.init_printing()
In [4]:
%matplotlib inline

Free Body Diagram

TODO : Add the free body diagram we drew in class.

Constants

Create a symbol for each of the system's contant parameters.

  • $m_p$: mass of the pendulum
  • $m_b$: mass of the ball
  • $l$: length of the pendulum
  • $r$: radius of the channel
  • $g$: acceleration due to gravity
In [5]:
mp, mb, r, l, g = sm.symbols('m_p, m_b, r, l, g')

Generalized Coordinates

Create functions of time for each generalized coordinate.

  • $\theta(t)$: angle of the pendulum
  • $\phi(t)$: angle of the line from the center of the channel to the ball
In [6]:
t = sm.symbols('t')
In [7]:
theta = sm.Function('theta')(t)
phi = sm.Function('phi')(t)
In [8]:
theta.diff()
Out[8]:
$\displaystyle \frac{d}{d t} \theta{\left(t \right)}$
In [9]:
theta.diff(t)
Out[9]:
$\displaystyle \frac{d}{d t} \theta{\left(t \right)}$

Kinetic Energy

Write the kinetic energy in terms of the generalized coordinates.

In [10]:
Tp = mp * (l * theta.diff())**2 / 2
Tp
Out[10]:
$\displaystyle \frac{l^{2} m_{p} \left(\frac{d}{d t} \theta{\left(t \right)}\right)^{2}}{2}$
In [11]:
Tb = mb / 2 * ((-r * theta.diff() * sm.cos(theta) + phi.diff() * r * sm.cos(theta + phi))**2 +
               (-r * theta.diff() * sm.sin(theta) + phi.diff() * r * sm.sin(theta + phi))**2)
Tb
Out[11]:
$\displaystyle \frac{m_{b} \left(\left(r \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} \frac{d}{d t} \phi{\left(t \right)} - r \sin{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \theta{\left(t \right)}\right)^{2} + \left(r \cos{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} \frac{d}{d t} \phi{\left(t \right)} - r \cos{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \theta{\left(t \right)}\right)^{2}\right)}{2}$
In [12]:
T = Tp + Tb
T
Out[12]:
$\displaystyle \frac{l^{2} m_{p} \left(\frac{d}{d t} \theta{\left(t \right)}\right)^{2}}{2} + \frac{m_{b} \left(\left(r \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} \frac{d}{d t} \phi{\left(t \right)} - r \sin{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \theta{\left(t \right)}\right)^{2} + \left(r \cos{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} \frac{d}{d t} \phi{\left(t \right)} - r \cos{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \theta{\left(t \right)}\right)^{2}\right)}{2}$

Potential Energy

Each particle (pendulum bob and the ball) has a potential energy associated with how high the mass rises.

In [13]:
U = mp * g * (l - l * sm.cos(theta)) + mb * g * (l + r * sm.cos(theta) - r * sm.cos(theta + phi))
U
Out[13]:
$\displaystyle g m_{b} \left(l - r \cos{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} + r \cos{\left(\theta{\left(t \right)} \right)}\right) + g m_{p} \left(- l \cos{\left(\theta{\left(t \right)} \right)} + l\right)$

Lagrange's equation of the second kind

There are two generalized coordinates with two degrees of freedom and thus two equations of motion.

$$ 0 = f_\theta(\theta, \phi, \dot{\theta}, \dot{\phi}, \ddot{\theta}, \ddot{\phi}, t) \\ 0 = f_\phi(\theta, \phi, \dot{\theta}, \dot{\phi}, \ddot{\theta}, \ddot{\phi}, t) \\ $$
In [14]:
L = T - U
L
Out[14]:
$\displaystyle - g m_{b} \left(l - r \cos{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} + r \cos{\left(\theta{\left(t \right)} \right)}\right) - g m_{p} \left(- l \cos{\left(\theta{\left(t \right)} \right)} + l\right) + \frac{l^{2} m_{p} \left(\frac{d}{d t} \theta{\left(t \right)}\right)^{2}}{2} + \frac{m_{b} \left(\left(r \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} \frac{d}{d t} \phi{\left(t \right)} - r \sin{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \theta{\left(t \right)}\right)^{2} + \left(r \cos{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} \frac{d}{d t} \phi{\left(t \right)} - r \cos{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \theta{\left(t \right)}\right)^{2}\right)}{2}$
In [15]:
f_theta = L.diff(theta.diff()).diff(t) - L.diff(theta)
f_theta = sm.trigsimp(f_theta)
f_theta
Out[15]:
$\displaystyle g l m_{p} \sin{\left(\theta{\left(t \right)} \right)} + g m_{b} r \left(\sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} - \sin{\left(\theta{\left(t \right)} \right)}\right) + l^{2} m_{p} \frac{d^{2}}{d t^{2}} \theta{\left(t \right)} + m_{b} r^{2} \left(\sin{\left(\phi{\left(t \right)} \right)} \left(\frac{d}{d t} \phi{\left(t \right)}\right)^{2} - \cos{\left(\phi{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} \phi{\left(t \right)} + \frac{d^{2}}{d t^{2}} \theta{\left(t \right)}\right)$
In [16]:
f_phi = L.diff(phi.diff()).diff(t) - L.diff(phi)
f_phi = sm.trigsimp(f_phi)
f_phi
Out[16]:
$\displaystyle m_{b} r \left(g \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} - r \cos{\left(\phi{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} \theta{\left(t \right)} + r \frac{d^{2}}{d t^{2}} \phi{\left(t \right)}\right)$

Explicit first order form

Introduce two new variables for the generalized speeds:

$$ \alpha = \dot{\theta} \\ \beta = \dot{\phi} $$
In [17]:
alpha = sm.Function('alpha')(t)
beta = sm.Function('beta')(t)

Substitute these into the two equations of motion:

In [18]:
f_theta = f_theta.subs({theta.diff(): alpha, phi.diff(): beta})
f_theta
Out[18]:
$\displaystyle g l m_{p} \sin{\left(\theta{\left(t \right)} \right)} + g m_{b} r \left(\sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} - \sin{\left(\theta{\left(t \right)} \right)}\right) + l^{2} m_{p} \frac{d}{d t} \alpha{\left(t \right)} + m_{b} r^{2} \left(\beta^{2}{\left(t \right)} \sin{\left(\phi{\left(t \right)} \right)} - \cos{\left(\phi{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \frac{d}{d t} \alpha{\left(t \right)}\right)$
In [19]:
f_phi = f_phi.subs({theta.diff(): alpha, phi.diff(): beta})
f_phi
Out[19]:
$\displaystyle m_{b} r \left(g \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} - r \cos{\left(\phi{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + r \frac{d}{d t} \beta{\left(t \right)}\right)$
In [20]:
f = sm.Matrix([f_theta, f_phi])
f
Out[20]:
$\displaystyle \left[\begin{matrix}g l m_{p} \sin{\left(\theta{\left(t \right)} \right)} + g m_{b} r \left(\sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} - \sin{\left(\theta{\left(t \right)} \right)}\right) + l^{2} m_{p} \frac{d}{d t} \alpha{\left(t \right)} + m_{b} r^{2} \left(\beta^{2}{\left(t \right)} \sin{\left(\phi{\left(t \right)} \right)} - \cos{\left(\phi{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \frac{d}{d t} \alpha{\left(t \right)}\right)\\m_{b} r \left(g \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} - r \cos{\left(\phi{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + r \frac{d}{d t} \beta{\left(t \right)}\right)\end{matrix}\right]$

The equations are motion are based on Newton's second law and Euler's equations, thus it is guaranteed that terms in $\mathbf{f}$ that include $\dot{\mathbf{u}}$ are linear with respect to $\dot{\mathbf{u}}$. So the equations of motion can be written in this matrix form:

$$ \mathbf{f}(\mathbf{q}, \mathbf{u}, \dot{\mathbf{u}}, t) = \mathbf{M}(\mathbf{q}, t)\dot{\mathbf{u}} - \mathbf{G}(\mathbf{q}, \mathbf{u}, t) = 0 $$

$\mathbf{M}$ is called the "mass matrix" of the nonlinear equations. If the derivatives of $\mathbf{f}$ with respect to $\dot{\mathbf{u}}$ are computed, i.e. the Jacobian of $\mathbf{f}$ with respect to $\dot{\mathbf{u}}$, then you can obtain the mass matrix.

In [21]:
u = sm.Matrix([alpha, beta])
u
Out[21]:
$\displaystyle \left[\begin{matrix}\alpha{\left(t \right)}\\\beta{\left(t \right)}\end{matrix}\right]$
In [22]:
M = f.jacobian(u.diff())
M
Out[22]:
$\displaystyle \left[\begin{matrix}l^{2} m_{p} + m_{b} r^{2} & - m_{b} r^{2} \cos{\left(\phi{\left(t \right)} \right)}\\- m_{b} r^{2} \cos{\left(\phi{\left(t \right)} \right)} & m_{b} r^{2}\end{matrix}\right]$
$$\mathbf{G} = \mathbf{M}\dot{\mathbf{u}} - \mathbf{f}$$
In [23]:
# TODO : The `sm.MutableDenseMatrix()` call is due to a regression present in SymPy 1.2 and 1.3.
# Once that is fixed, this should not be necessary.
G = M * sm.MutableDenseMatrix(u.diff()) - f
G.simplify()
G
Out[23]:
$\displaystyle \left[\begin{matrix}- g l m_{p} \sin{\left(\theta{\left(t \right)} \right)} - g m_{b} r \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} + g m_{b} r \sin{\left(\theta{\left(t \right)} \right)} - m_{b} r^{2} \beta^{2}{\left(t \right)} \sin{\left(\phi{\left(t \right)} \right)}\\- g m_{b} r \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)}\end{matrix}\right]$

The first order form has all of the $\dot{\mathbf{u}}$ on the left hand side. This requires solving the linear system of equations:

$$\mathbf{M}\dot{\mathbf{u}}=\mathbf{G}$$

The mathematical solution is:

$$\dot{\mathbf{u}}=\mathbf{M}^{-1}\mathbf{G}$$
In [24]:
udot = M.inv() * G
udot.simplify()
udot
Out[24]:
$\displaystyle \left[\begin{matrix}\frac{- g l m_{p} \sin{\left(\theta{\left(t \right)} \right)} - g m_{b} r \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} \cos{\left(\phi{\left(t \right)} \right)} - g m_{b} r \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} + g m_{b} r \sin{\left(\theta{\left(t \right)} \right)} - m_{b} r^{2} \beta^{2}{\left(t \right)} \sin{\left(\phi{\left(t \right)} \right)}}{l^{2} m_{p} + m_{b} r^{2} \sin^{2}{\left(\phi{\left(t \right)} \right)}}\\- \frac{g \left(l^{2} m_{p} + m_{b} r^{2}\right) \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} + r \left(g l m_{p} \sin{\left(\theta{\left(t \right)} \right)} + g m_{b} r \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} - g m_{b} r \sin{\left(\theta{\left(t \right)} \right)} + m_{b} r^{2} \beta^{2}{\left(t \right)} \sin{\left(\phi{\left(t \right)} \right)}\right) \cos{\left(\phi{\left(t \right)} \right)}}{r \left(l^{2} m_{p} + m_{b} r^{2} \sin^{2}{\left(\phi{\left(t \right)} \right)}\right)}\end{matrix}\right]$

A better way to solve the system of linear equations is to use Guassian elmination. SymPy has a variety of methods for sovling linear systems. The LU decomposition method of Guassian elimination is a generally good choice for this and for large number of degrees of freedom this will provide reasonable computation time. For very large $n$ this should be done numerically instead of symbolically.

In [25]:
udot = M.LUsolve(G)
udot.simplify()
udot
Out[25]:
$\displaystyle \left[\begin{matrix}\frac{- g l m_{p} \sin{\left(\theta{\left(t \right)} \right)} - g m_{b} r \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} \cos{\left(\phi{\left(t \right)} \right)} - g m_{b} r \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} + g m_{b} r \sin{\left(\theta{\left(t \right)} \right)} - m_{b} r^{2} \beta^{2}{\left(t \right)} \sin{\left(\phi{\left(t \right)} \right)}}{l^{2} m_{p} + m_{b} r^{2} \sin^{2}{\left(\phi{\left(t \right)} \right)}}\\- \frac{g \left(l^{2} m_{p} + m_{b} r^{2}\right) \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} + r \left(g l m_{p} \sin{\left(\theta{\left(t \right)} \right)} + g m_{b} r \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} - g m_{b} r \sin{\left(\theta{\left(t \right)} \right)} + m_{b} r^{2} \beta^{2}{\left(t \right)} \sin{\left(\phi{\left(t \right)} \right)}\right) \cos{\left(\phi{\left(t \right)} \right)}}{r \left(l^{2} m_{p} + m_{b} r^{2} \sin^{2}{\left(\phi{\left(t \right)} \right)}\right)}\end{matrix}\right]$

Note the differences in timing below. For systems with a large number of degrees of freedom, this gap in timing will increase significantly.

In [26]:
%timeit M.inv() * G
500 ms ± 7.31 ms per loop (mean ± std. dev. of 7 runs, 1 loop each)
In [27]:
%timeit M.LUsolve(G)
377 µs ± 3.64 µs per loop (mean ± std. dev. of 7 runs, 1000 loops each)

Simulation of the nonlinear system

Resonance has a prepared system that is only missing the equations of motion.

In [28]:
from resonance.nonlinear_systems import BallChannelPendulumSystem
In [29]:
sys = BallChannelPendulumSystem()
In [30]:
sys.constants
Out[30]:
{'mp': 0.012, 'mb': 0.0035, 'r': 0.1, 'l': 0.2, 'g': 9.81}
In [31]:
sys.coordinates
Out[31]:
{'theta': 0.17453292519943295, 'phi': -0.17453292519943295}
In [32]:
sys.speeds
Out[32]:
{'alpha': 0.0, 'beta': 0.0}

The full first order ordinary differential equations are:

$$ \dot{\theta} = \alpha \\ \dot{\phi} = \beta \\ \dot{\alpha} = f_{\alpha}(\theta, \phi, \alpha, \beta, t) \\ \dot{\beta} = f_{\beta}(\theta, \phi, \alpha, \beta, t) $$

where:

$$ \dot{\mathbf{q}}=\begin{bmatrix} \theta \\ \phi \end{bmatrix} \\ \dot{\mathbf{u}}=\mathbf{M}^{-1}\mathbf{G} = \begin{bmatrix} f_{\alpha}(\theta, \phi, \alpha, \beta, t) \\ f_{\beta}(\theta, \phi, \alpha, \beta, t) \end{bmatrix} $$

Introducing:

$$\mathbf{x} = \begin{bmatrix} \mathbf{q} \\ \mathbf{u} \end{bmatrix}= \begin{bmatrix} \theta \\ \phi \\ \alpha \\ \beta \end{bmatrix} $$

we have equations for:

$$\dot{\mathbf{x}} = \begin{bmatrix} \dot{\theta} \\ \dot{\phi} \\ \dot{\alpha} \\ \dot{\beta} \end{bmatrix} = \begin{bmatrix} \alpha \\ \beta \\ f_{\alpha}(\theta, \phi, \alpha, \beta, t) \\ f_{\beta}(\theta, \phi, \alpha, \beta, t) \end{bmatrix} $$

To find $\mathbf{x}$ we must integrate $\dot{\mathbf{x}}$ with respect to time:

$$ \mathbf{x} = \int_{t_0}^{t_f} \dot{\mathbf{x}} dt $$

Resonance uses numerical integration behind the scenes to compute this integral. Numerical integration routines typicall require that you write a function that computes the right hand side of the first order form of the differential equations. This function takes the current state and time and computes the derivative of the states.

SymPy's lambdify function can convert symbolic expression into NumPy aware functions, i.e. Python functions that can accept NumPy arrays.

In [33]:
eval_alphadot = sm.lambdify((phi, theta, alpha, beta, mp, mb, l, r, g), udot[0])
In [34]:
eval_alphadot(1, 2, 3, 4, 5, 6, 7, 8, 9)
Out[34]:
$\displaystyle -9.977772796520863$
In [35]:
eval_betadot = sm.lambdify((phi, theta, alpha, beta, mp, mb, l, r, g), udot[1])
In [36]:
eval_betadot(1, 2, 3, 4, 5, 6, 7, 8, 9)
Out[36]:
$\displaystyle -5.549773658455971$

Now the right hand side function can be written:

In [37]:
def rhs(phi, theta, alpha, beta, mp, mb, l, r, g):
    theta_dot = alpha
    phi_dot = beta
    alpha_dot = eval_alphadot(phi, theta, alpha, beta, mp, mb, l, r, g)
    beta_dot = eval_betadot(phi, theta, alpha, beta, mp, mb, l, r, g)
    return theta_dot, phi_dot, alpha_dot, beta_dot
In [38]:
rhs(1, 2, 3, 4, 5, 6, 7, 8, 9)
Out[38]:
$\displaystyle \left( 3, \ 4, \ -9.977772796520863, \ -5.549773658455971\right)$

This function also works with numpy arrays:

In [39]:
rhs(np.array([1, 2]), np.array([3, 4]), np.array([5, 6]), np.array([7, 8]), 9, 10, 11, 12, 13)
Out[39]:
(array([5, 6]),
 array([7, 8]),
 array([-27.2770872 , -36.73982421]),
 array([-13.91800374,  15.59186174]))

Add this function as the differential equation function of the system.

In [40]:
sys.diff_eq_func = rhs

Now the free_response function can be called to simulation the nonlinear system.

In [41]:
traj = sys.free_response(20, sample_rate=500)
In [42]:
traj[['theta', 'phi']].plot(subplots=True);
In [43]:
sys.animate_configuration(fps=30)
Out[43]:
<matplotlib.animation.FuncAnimation at 0x7fa19f594640>

Equilibrium

This system four equilibrium points.

  1. $\theta = \phi = \alpha = \beta = 0$
  2. $\theta = \pi, \phi=\alpha=\beta=0$
  3. $\theta = \pi, \phi = -\pi, \alpha=\beta=0$
  4. $\theta = 0, \phi = \pi, \alpha=\beta=0$

If you set the velocities and accelerations equal to zero in the equations of motion you can then solve for the coordinates that make these equations equal to zero. This is the static force balance equations.

In [44]:
f_static = f.subs(dict(zip(u.diff(), [0, 0]))).subs(dict(zip(u, [0, 0])))
f_static
Out[44]:
$\displaystyle \left[\begin{matrix}g l m_{p} \sin{\left(\theta{\left(t \right)} \right)} + g m_{b} r \left(\sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} - \sin{\left(\theta{\left(t \right)} \right)}\right)\\g m_{b} r \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)}\end{matrix}\right]$
In [45]:
sm.solve(f_static, theta, phi)
Out[45]:
$\displaystyle \left[ \left\{ \phi{\left(t \right)} : 0, \ \theta{\left(t \right)} : 0\right\}, \ \left\{ \phi{\left(t \right)} : 0, \ \theta{\left(t \right)} : \pi\right\}, \ \left\{ \phi{\left(t \right)} : - \pi, \ \theta{\left(t \right)} : \pi\right\}, \ \left\{ \phi{\left(t \right)} : \pi, \ \theta{\left(t \right)} : 0\right\}\right]$

Let's look at the simulation with the initial condition very close to an equilibrium point:

In [46]:
sys.coordinates['theta'] = np.deg2rad(180.0000001)
sys.coordinates['phi'] = -np.deg2rad(180.00000001)
In [47]:
traj = sys.free_response(20, sample_rate=500)
In [48]:
sys.animate_configuration(fps=30)
Out[48]:
<matplotlib.animation.FuncAnimation at 0x7fa19f3b9280>
In [49]:
traj[['theta', 'phi']].plot(subplots=True);

This equlibrium point is an unstable equlibrium.

Linearizing the system

The equations of motion can be linearized about one of the equilibrium points. This can be done by computing the linear terms of the multivariate Taylor Series expansion. This expansion can be expressed as:

$$ \mathbf{f}_{linear} = \mathbf{f}(\mathbf{v}_{eq}) + \mathbf{J}_f(\mathbf{v}_{eq}) (\mathbf{v} - \mathbf{v}_{eq}) $$

where $\mathbf{J}_f$ is the Jacobian of $\mathbf{f}$ with respect to $\mathbf{v}$ and:

$$ \mathbf{v} = \begin{bmatrix} \theta\\ \phi \\ \alpha \\ \beta \\ \dot{\alpha} \\ \dot{\beta} \end{bmatrix} $$

In our case let's linearize about the static position where $\theta=\phi=0$.

In [50]:
f
Out[50]:
$\displaystyle \left[\begin{matrix}g l m_{p} \sin{\left(\theta{\left(t \right)} \right)} + g m_{b} r \left(\sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} - \sin{\left(\theta{\left(t \right)} \right)}\right) + l^{2} m_{p} \frac{d}{d t} \alpha{\left(t \right)} + m_{b} r^{2} \left(\beta^{2}{\left(t \right)} \sin{\left(\phi{\left(t \right)} \right)} - \cos{\left(\phi{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \frac{d}{d t} \alpha{\left(t \right)}\right)\\m_{b} r \left(g \sin{\left(\phi{\left(t \right)} + \theta{\left(t \right)} \right)} - r \cos{\left(\phi{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + r \frac{d}{d t} \beta{\left(t \right)}\right)\end{matrix}\right]$
In [51]:
v = sm.Matrix([theta, phi, alpha, beta, alpha.diff(), beta.diff()])
v
Out[51]:
$\displaystyle \left[\begin{matrix}\theta{\left(t \right)}\\\phi{\left(t \right)}\\\alpha{\left(t \right)}\\\beta{\left(t \right)}\\\frac{d}{d t} \alpha{\left(t \right)}\\\frac{d}{d t} \beta{\left(t \right)}\end{matrix}\right]$
In [52]:
v0 = sm.zeros(len(v), 1)
v0
Out[52]:
$\displaystyle \left[\begin{matrix}0\\0\\0\\0\\0\\0\end{matrix}\right]$
In [53]:
v_eq_sub = dict(zip(v, v0))
v_eq_sub
Out[53]:
$\displaystyle \left\{ \alpha{\left(t \right)} : 0, \ \beta{\left(t \right)} : 0, \ \phi{\left(t \right)} : 0, \ \theta{\left(t \right)} : 0, \ \frac{d}{d t} \alpha{\left(t \right)} : 0, \ \frac{d}{d t} \beta{\left(t \right)} : 0\right\}$

The linear equations are then:

In [54]:
f_lin = f.subs(v_eq_sub) + f.jacobian(v).subs(v_eq_sub) * (v - v0)
f_lin
Out[54]:
$\displaystyle \left[\begin{matrix}g l m_{p} \theta{\left(t \right)} + g m_{b} r \phi{\left(t \right)} - m_{b} r^{2} \frac{d}{d t} \beta{\left(t \right)} + \left(l^{2} m_{p} + m_{b} r^{2}\right) \frac{d}{d t} \alpha{\left(t \right)}\\g m_{b} r \phi{\left(t \right)} + g m_{b} r \theta{\left(t \right)} - m_{b} r^{2} \frac{d}{d t} \alpha{\left(t \right)} + m_{b} r^{2} \frac{d}{d t} \beta{\left(t \right)}\end{matrix}\right]$

Note that all of the terms that involve the coordinates, speeds, and their derivatives are linear terms, i.e. simple linear coefficients. These linear equations can be put into this canonical form:

$$\mathbf{M}\dot{\mathbf{u}} + \mathbf{C}\mathbf{u} + \mathbf{K} \mathbf{q} = \mathbf{F}$$

with:

  • $\mathbf{M}$ as the mass matrix
  • $\mathbf{C}$ as the damping matrix
  • $\mathbf{K}$ as the stiffness matrix
  • $\mathbf{F}$ as the forcing vector

The Jacobian can again be utlized to extract the linear coefficients.

In [55]:
q = sm.Matrix([theta, phi])
q
Out[55]:
$\displaystyle \left[\begin{matrix}\theta{\left(t \right)}\\\phi{\left(t \right)}\end{matrix}\right]$
In [56]:
u
Out[56]:
$\displaystyle \left[\begin{matrix}\alpha{\left(t \right)}\\\beta{\left(t \right)}\end{matrix}\right]$
In [57]:
M = f_lin.jacobian(u.diff())
M
Out[57]:
$\displaystyle \left[\begin{matrix}l^{2} m_{p} + m_{b} r^{2} & - m_{b} r^{2}\\- m_{b} r^{2} & m_{b} r^{2}\end{matrix}\right]$
In [58]:
C = f_lin.jacobian(u)
C
Out[58]:
$\displaystyle \left[\begin{matrix}0 & 0\\0 & 0\end{matrix}\right]$
In [59]:
K = f_lin.jacobian(q)
K
Out[59]:
$\displaystyle \left[\begin{matrix}g l m_{p} & g m_{b} r\\g m_{b} r & g m_{b} r\end{matrix}\right]$
In [60]:
F = sm.simplify(f_lin - M * u.diff() - C * u - K * q)
F
Out[60]:
$\displaystyle \left[\begin{matrix}0\\0\end{matrix}\right]$

Simulate the linear system

In [61]:
from resonance.linear_systems import BallChannelPendulumSystem
In [62]:
lin_sys = BallChannelPendulumSystem()

For linear systems, a function that calculates the canonical coefficient matrices should be created. Each of the canonical matrices should be created as 2 x 2 NumPy arrays.

In [63]:
def canon_coeff_matrices(mp, mb, l, g, r):
    M = np.array([[mp * l**2 + mb * r**2, -mb * r**2],
                  [-mb * r**2, mb * r**2]])
    C = np.zeros((2, 2))
    K = np.array([[g * l * mp, g * mb * r],
                  [g * mb * r, g * mb * r]])
    return M, C, K
In [64]:
lin_sys.canonical_coeffs_func = canon_coeff_matrices
In [65]:
M_num, C_num, K_num = lin_sys.canonical_coefficients()
In [66]:
M_num
Out[66]:
array([[ 5.15e-04, -3.50e-05],
       [-3.50e-05,  3.50e-05]])
In [67]:
C_num
Out[67]:
array([[0., 0.],
       [0., 0.]])
In [68]:
K_num
Out[68]:
array([[0.023544 , 0.0034335],
       [0.0034335, 0.0034335]])
In [69]:
lin_traj = lin_sys.free_response(20, sample_rate=500)
In [70]:
lin_traj[['theta', 'phi']].plot(subplots=True);

Compare the nonlinear and linear simulations

In [71]:
sys.coordinates['theta'] = np.deg2rad(10)
sys.coordinates['phi'] = np.deg2rad(-10)
In [72]:
lin_sys.coordinates['theta'] = np.deg2rad(10)
lin_sys.coordinates['phi'] = np.deg2rad(-10)
In [73]:
traj = sys.free_response(10.0)
In [74]:
lin_traj = lin_sys.free_response(10.0)
In [75]:
axes = traj[['theta', 'phi']].plot(subplots=True, color='red')
axes = lin_traj[['theta', 'phi']].plot(subplots=True, color='blue', ax=axes)
axes[0].legend([r'nonlin $\theta$', r'lin $\theta$'])
axes[1].legend([r'nonlin $\phi$', r'lin $\phi$']);