# 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.


```python
from IPython.display import YouTubeVideo
YouTubeVideo('3pJdkssUdfU', width=600, height=480)
```

# Imports and setup


```python
import sympy as sm
import numpy as np
```


```python
sm.init_printing()
```


```python
%matplotlib notebook
```

# 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


```python
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


```python
t = sm.symbols('t')
```


```python
theta = sm.Function('theta')(t)
phi = sm.Function('phi')(t)
```


```python
theta.diff()
```


```python
theta.diff(t)
```

# Kinetic Energy

Write the kinetic energy in terms of the generalized coordinates.


```python
Tp = mp * (l * theta.diff())**2 / 2
Tp
```


```python
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
```


```python
T = Tp + Tb
T
```

# Potential Energy

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


```python
U = mp * g * (l - l * sm.cos(theta)) + mb * g * (l + r * sm.cos(theta) - r * sm.cos(theta + phi))
U
```

# 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) \\
$$


```python
L = T - U
L
```


```python
f_theta = L.diff(theta.diff()).diff(t) - L.diff(theta)
f_theta = sm.trigsimp(f_theta)
f_theta
```


```python
f_phi = L.diff(phi.diff()).diff(t) - L.diff(phi)
f_phi = sm.trigsimp(f_phi)
f_phi
```

# Explicit first order form

Introduce two new variables for the generalized speeds:

$$
\alpha = \dot{\theta} \\
\beta = \dot{\phi}
$$


```python
alpha = sm.Function('alpha')(t)
beta = sm.Function('beta')(t)
```

Substitute these into the two equations of motion:


```python
f_theta = f_theta.subs({theta.diff(): alpha, phi.diff(): beta})
f_theta
```


```python
f_phi = f_phi.subs({theta.diff(): alpha, phi.diff(): beta})
f_phi
```


```python
f = sm.Matrix([f_theta, f_phi])
f
```

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](https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant) of $\mathbf{f}$ with respect to $\dot{\mathbf{u}}$, then you can obtain the mass matrix.


```python
u = sm.Matrix([alpha, beta])
u
```


```python
M = f.jacobian(u.diff())
M
```

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


```python
# 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
```

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}$$


```python
udot = M.inv() * G
udot.simplify()
udot
```

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.


```python
udot = M.LUsolve(G)
udot.simplify()
udot
```

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


```python
%timeit M.inv() * G
```


```python
%timeit M.LUsolve(G)
```

# Simulation of the nonlinear system

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


```python
from resonance.nonlinear_systems import BallChannelPendulumSystem
```


```python
sys = BallChannelPendulumSystem()
```


```python
sys.constants
```


```python
sys.coordinates
```


```python
sys.speeds
```

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.


```python
eval_alphadot = sm.lambdify((phi, theta, alpha, beta, mp, mb, l, r, g), udot[0])
```


```python
eval_alphadot(1, 2, 3, 4, 5, 6, 7, 8, 9)
```


```python
eval_betadot = sm.lambdify((phi, theta, alpha, beta, mp, mb, l, r, g), udot[1])
```


```python
eval_betadot(1, 2, 3, 4, 5, 6, 7, 8, 9)
```

Now the right hand side function can be written:


```python
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
```


```python
rhs(1, 2, 3, 4, 5, 6, 7, 8, 9)
```

This function also works with numpy arrays:


```python
rhs(np.array([1, 2]), np.array([3, 4]), np.array([5, 6]), np.array([7, 8]), 9, 10, 11, 12, 13)
```

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


```python
sys.diff_eq_func = rhs
```

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


```python
traj = sys.free_response(20, sample_rate=500)
```


```python
traj[['theta', 'phi']].plot(subplots=True);
```


```python
sys.animate_configuration(fps=30)
```

# 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.


```python
f_static = f.subs(dict(zip(u.diff(), [0, 0]))).subs(dict(zip(u, [0, 0])))
f_static
```


```python
sm.solve(f_static, theta, phi)
```

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


```python
sys.coordinates['theta'] = np.deg2rad(180.0000001)
sys.coordinates['phi'] = -np.deg2rad(180.00000001)
```


```python
traj = sys.free_response(20, sample_rate=500)
```


```python
sys.animate_configuration(fps=30)
```


```python
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$.


```python
f
```


```python
v = sm.Matrix([theta, phi, alpha, beta, alpha.diff(), beta.diff()])
v
```


```python
v0 = sm.zeros(len(v), 1)
v0
```


```python
v_eq_sub = dict(zip(v, v0))
v_eq_sub
```

The linear equations are then:


```python
f_lin = f.subs(v_eq_sub) + f.jacobian(v).subs(v_eq_sub) * (v - v0)
f_lin
```

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.


```python
q = sm.Matrix([theta, phi])
q
```


```python
u
```


```python
M = f_lin.jacobian(u.diff())
M
```


```python
C = f_lin.jacobian(u)
C
```


```python
K = f_lin.jacobian(q)
K
```


```python
F = sm.simplify(f_lin - M * u.diff() - C * u - K * q)
F
```

# Simulate the linear system


```python
from resonance.linear_systems import BallChannelPendulumSystem
```


```python
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.


```python
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
```


```python
lin_sys.canonical_coeffs_func = canon_coeff_matrices
```


```python
M_num, C_num, K_num = lin_sys.canonical_coefficients()
```


```python
M_num
```


```python
C_num
```


```python
K_num
```


```python
lin_traj = lin_sys.free_response(20, sample_rate=500)
```


```python
lin_traj[['theta', 'phi']].plot(subplots=True);
```

# Compare the nonlinear and linear simulations


```python
sys.coordinates['theta'] = np.deg2rad(10)
sys.coordinates['phi'] = np.deg2rad(-10)
```


```python
lin_sys.coordinates['theta'] = np.deg2rad(10)
lin_sys.coordinates['phi'] = np.deg2rad(-10)
```


```python
traj = sys.free_response(10.0)
```


```python
lin_traj = lin_sys.free_response(10.0)
```


```python
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$']);
```

94ecf9e0f5efbf19048c5aa3600913ebd4738c34
ipynb
Jupyter Notebook
notebooks/10/10_modeling_a_ball_channel_pendulum.ipynb
gbrault/resonance
bf66993a98fbbb857511f83bc072449b98f0b4c2
MIT
2017-11-10T16:44:04.000Z
2022-01-13T12:22:02.000Z
notebooks/10/10_modeling_a_ball_channel_pendulum.ipynb
gbrault/resonance
bf66993a98fbbb857511f83bc072449b98f0b4c2
MIT
2017-07-19T20:16:13.000Z
2020-03-10T04:13:46.000Z
notebooks/10/10_modeling_a_ball_channel_pendulum.ipynb
gbrault/resonance
bf66993a98fbbb857511f83bc072449b98f0b4c2
MIT
2018-04-05T22:58:43.000Z
2021-01-14T04:06:26.000Z
Qwen/Qwen-72B
1. YES
2. YES