In [1]:
import sympy as sm

In [2]:
sm.init_printing()


# Define variables¶

In [3]:
m, M, Ic, k, b, l, g = sm.symbols('m, M, I_c, k, b, l, g', real=True, positive=True)

In [4]:
t = sm.symbols('t', real=True)

In [5]:
x = sm.Function('x')(t)
v = sm.Function('v')(t)
theta = sm.Function('theta')(t)
omega = sm.Function('omega')(t)


# Kinetic energy¶

In [6]:
vc_squared = (v - omega*l*sm.sin(theta))**2 + (omega*l*sm.cos(theta))**2
vc_squared

Out[6]:
$\displaystyle l^{2} \omega^{2}{\left(t \right)} \cos^{2}{\left(\theta{\left(t \right)} \right)} + \left(- l \omega{\left(t \right)} \sin{\left(\theta{\left(t \right)} \right)} + v{\left(t \right)}\right)^{2}$
In [7]:
eq_repl = {theta.diff(t): omega, x.diff(t): v}
eq_repl

Out[7]:
$\displaystyle \left\{ \frac{d}{d t} \theta{\left(t \right)} : \omega{\left(t \right)}, \ \frac{d}{d t} x{\left(t \right)} : v{\left(t \right)}\right\}$
In [8]:
T = m*v**2/2 + M*vc_squared/2 + Ic*omega**2/2
T

Out[8]:
$\displaystyle \frac{I_{c} \omega^{2}{\left(t \right)}}{2} + \frac{M \left(l^{2} \omega^{2}{\left(t \right)} \cos^{2}{\left(\theta{\left(t \right)} \right)} + \left(- l \omega{\left(t \right)} \sin{\left(\theta{\left(t \right)} \right)} + v{\left(t \right)}\right)^{2}\right)}{2} + \frac{m v^{2}{\left(t \right)}}{2}$

# Potential energy¶

In [9]:
U = k*x**2/2 -m*g*x - M*g*(x + l*sm.cos(theta))
U

Out[9]:
$\displaystyle - M g \left(l \cos{\left(\theta{\left(t \right)} \right)} + x{\left(t \right)}\right) - g m x{\left(t \right)} + \frac{k x^{2}{\left(t \right)}}{2}$

# Damping¶

In [10]:
R = b*v**2/2
R

Out[10]:
$\displaystyle \frac{b v^{2}{\left(t \right)}}{2}$

# Lagrange's Equation¶

In [11]:
L = T - U
L

Out[11]:
$\displaystyle \frac{I_{c} \omega^{2}{\left(t \right)}}{2} + M g \left(l \cos{\left(\theta{\left(t \right)} \right)} + x{\left(t \right)}\right) + \frac{M \left(l^{2} \omega^{2}{\left(t \right)} \cos^{2}{\left(\theta{\left(t \right)} \right)} + \left(- l \omega{\left(t \right)} \sin{\left(\theta{\left(t \right)} \right)} + v{\left(t \right)}\right)^{2}\right)}{2} + g m x{\left(t \right)} - \frac{k x^{2}{\left(t \right)}}{2} + \frac{m v^{2}{\left(t \right)}}{2}$
In [12]:
leq_v = L.diff(v).diff(t).subs(eq_repl) - L.diff(x) -(-R.diff(v))
leq_v

Out[12]:
$\displaystyle - M g + \frac{M \left(- 2 l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} - 2 l \sin{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \omega{\left(t \right)} + 2 \frac{d}{d t} v{\left(t \right)}\right)}{2} + b v{\left(t \right)} - g m + k x{\left(t \right)} + m \frac{d}{d t} v{\left(t \right)}$
In [13]:
leq_omega = L.diff(omega).diff(t).subs(eq_repl) - L.diff(theta) -(-R.diff(omega))
leq_omega

Out[13]:
$\displaystyle I_{c} \frac{d}{d t} \omega{\left(t \right)} + M g l \sin{\left(\theta{\left(t \right)} \right)} - \frac{M \left(- 2 l^{2} \omega^{2}{\left(t \right)} \sin{\left(\theta{\left(t \right)} \right)} \cos{\left(\theta{\left(t \right)} \right)} - 2 l \left(- l \omega{\left(t \right)} \sin{\left(\theta{\left(t \right)} \right)} + v{\left(t \right)}\right) \omega{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)}\right)}{2} + \frac{M \left(- 4 l^{2} \omega^{2}{\left(t \right)} \sin{\left(\theta{\left(t \right)} \right)} \cos{\left(\theta{\left(t \right)} \right)} + 2 l^{2} \cos^{2}{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \omega{\left(t \right)} - 2 l \left(- l \omega{\left(t \right)} \sin{\left(\theta{\left(t \right)} \right)} + v{\left(t \right)}\right) \omega{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} - 2 l \left(- l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} - l \sin{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \omega{\left(t \right)} + \frac{d}{d t} v{\left(t \right)}\right) \sin{\left(\theta{\left(t \right)} \right)}\right)}{2}$
In [14]:
leq_omega = sm.simplify(leq_omega)
leq_omega

Out[14]:
$\displaystyle I_{c} \frac{d}{d t} \omega{\left(t \right)} + M g l \sin{\left(\theta{\left(t \right)} \right)} + M l^{2} \frac{d}{d t} \omega{\left(t \right)} - M l \sin{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} v{\left(t \right)}$

# Put EoM in explicit first order form¶

In [15]:
f = sm.Matrix([leq_v, leq_omega])
f

Out[15]:
$\displaystyle \left[\begin{matrix}- M g + \frac{M \left(- 2 l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} - 2 l \sin{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \omega{\left(t \right)} + 2 \frac{d}{d t} v{\left(t \right)}\right)}{2} + b v{\left(t \right)} - g m + k x{\left(t \right)} + m \frac{d}{d t} v{\left(t \right)}\\I_{c} \frac{d}{d t} \omega{\left(t \right)} + M g l \sin{\left(\theta{\left(t \right)} \right)} + M l^{2} \frac{d}{d t} \omega{\left(t \right)} - M l \sin{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} v{\left(t \right)}\end{matrix}\right]$
In [16]:
g = f.subs({omega.diff(t): 0, v.diff(t): 0})
g

Out[16]:
$\displaystyle \left[\begin{matrix}- M g - M l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} + b v{\left(t \right)} - g m + k x{\left(t \right)}\\M g l \sin{\left(\theta{\left(t \right)} \right)}\end{matrix}\right]$
In [17]:
I = f.jacobian([omega.diff(t), v.diff(t)])
I

Out[17]:
$\displaystyle \left[\begin{matrix}- M l \sin{\left(\theta{\left(t \right)} \right)} & M + m\\I_{c} + M l^{2} & - M l \sin{\left(\theta{\left(t \right)} \right)}\end{matrix}\right]$
In [18]:
sdot = -I.inv()*g
sdot

Out[18]:
$\displaystyle \left[\begin{matrix}- \frac{M g l \left(M + m\right) \sin{\left(\theta{\left(t \right)} \right)}}{- M^{2} l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)} + \left(I_{c} + M l^{2}\right) \left(M + m\right)} - \frac{M l \left(- M g - M l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} + b v{\left(t \right)} - g m + k x{\left(t \right)}\right) \sin{\left(\theta{\left(t \right)} \right)}}{- M^{2} l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)} + \left(I_{c} + M l^{2}\right) \left(M + m\right)}\\- \frac{M^{2} g l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)}}{- M^{2} l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)} + \left(I_{c} + M l^{2}\right) \left(M + m\right)} - \frac{\left(I_{c} + M l^{2}\right) \left(- M g - M l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} + b v{\left(t \right)} - g m + k x{\left(t \right)}\right)}{- M^{2} l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)} + \left(I_{c} + M l^{2}\right) \left(M + m\right)}\end{matrix}\right]$
In [19]:
sdot = -I.LUsolve(g)
sdot

Out[19]:
$\displaystyle \left[\begin{matrix}- \frac{M g l \sin{\left(\theta{\left(t \right)} \right)} + \frac{M l \left(\frac{M^{2} g l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)}}{I_{c} + M l^{2}} - M g - M l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} + b v{\left(t \right)} - g m + k x{\left(t \right)}\right) \sin{\left(\theta{\left(t \right)} \right)}}{- \frac{M^{2} l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)}}{I_{c} + M l^{2}} + M + m}}{I_{c} + M l^{2}}\\- \frac{\frac{M^{2} g l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)}}{I_{c} + M l^{2}} - M g - M l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} + b v{\left(t \right)} - g m + k x{\left(t \right)}}{- \frac{M^{2} l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)}}{I_{c} + M l^{2}} + M + m}\end{matrix}\right]$
In [20]:
sm.simplify(sdot)

Out[20]:
$\displaystyle \left[\begin{matrix}- \frac{M l \left(- M l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} + b v{\left(t \right)} + k x{\left(t \right)}\right) \sin{\left(\theta{\left(t \right)} \right)}}{I_{c} M + I_{c} m + M^{2} l^{2} \cos^{2}{\left(\theta{\left(t \right)} \right)} + M l^{2} m}\\\frac{- M^{2} g l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)} + \left(- I_{c} - M l^{2}\right) \left(- M g - M l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} + b v{\left(t \right)} - g m + k x{\left(t \right)}\right)}{- M^{2} l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)} + \left(I_{c} + M l^{2}\right) \left(M + m\right)}\end{matrix}\right]$

# Simulate the non-linear system¶

In [21]:
from resonance.nonlinear_systems import MultiDoFNonLinearSystem

In [22]:
sys = MultiDoFNonLinearSystem()

In [23]:
sys.constants['m'] = 1.0  # kg
sys.constants['k'] = 10.0  # N/m
sys.constants['b'] = 5.0  # Ns
sys.constants['l'] = 0.5  # m
sys.constants['M'] = 0.5  # kg
sys.constants['Ic'] = 0.5*0.5**2  # kg m**2
sys.constants['g'] = 9.81  # m/s**2

In [24]:
# order of entry matters!
sys.coordinates['theta'] = 1.0  # rad
sys.coordinates['x'] = 0.0  # m
sys.speeds['omega'] = 0.0 # rad/s
sys.speeds['v'] = 0.0 # m/s

In [25]:
sys.states

Out[25]:
_StatesDict([('theta', 1.0), ('x', 0.0), ('omega', 0.0), ('v', 0.0)])
In [26]:
eval_mass = sm.lambdify((m, M), m+M)

In [27]:
eval_mass(1.0, 2.0)

Out[27]:
$\displaystyle 3.0$
In [28]:
g = sm.symbols('g', real=True, positive=True)

In [29]:
eval_sdot = sm.lambdify((theta, x, omega, v, m, k, b, l, M, Ic, g), [sdot[0], sdot[1]])

In [30]:
eval_sdot(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0)

Out[30]:
$\displaystyle \left[ 4.144543155486223, \ 51.08691546911525\right]$
In [31]:
def eval_derivatives(theta, x, omega, v, m, k, b, l, M, Ic, g):
omegadot, vdot = eval_sdot(theta, x, omega, v, m, k, b, l, M, Ic, g)
xdot = v

In [32]:
eval_derivatives(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0)

Out[32]:
$\displaystyle \left( 3.0, \ 4.0, \ 4.144543155486223, \ 51.08691546911525\right)$
In [33]:
sys.diff_eq_func = eval_derivatives

In [34]:
trajectories = sys.free_response(5.0)

In [35]:
%matplotlib widget

In [36]:
trajectories[['x', 'theta']].plot(subplots=True)

Out[36]:
array([<matplotlib.axes._subplots.AxesSubplot object at 0x7f8dab65f4a8>,
<matplotlib.axes._subplots.AxesSubplot object at 0x7f8dab5d3be0>],
dtype=object)

# Find the equilibrium¶

In [37]:
sdot

Out[37]:
$\displaystyle \left[\begin{matrix}- \frac{M g l \sin{\left(\theta{\left(t \right)} \right)} + \frac{M l \left(\frac{M^{2} g l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)}}{I_{c} + M l^{2}} - M g - M l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} + b v{\left(t \right)} - g m + k x{\left(t \right)}\right) \sin{\left(\theta{\left(t \right)} \right)}}{- \frac{M^{2} l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)}}{I_{c} + M l^{2}} + M + m}}{I_{c} + M l^{2}}\\- \frac{\frac{M^{2} g l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)}}{I_{c} + M l^{2}} - M g - M l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} + b v{\left(t \right)} - g m + k x{\left(t \right)}}{- \frac{M^{2} l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)}}{I_{c} + M l^{2}} + M + m}\end{matrix}\right]$
In [38]:
sdot_no_motion = sdot.subs({omega: 0, v: 0})

In [39]:
sm.simplify(sdot_no_motion)

Out[39]:
$\displaystyle \left[\begin{matrix}- \frac{M k l x{\left(t \right)} \sin{\left(\theta{\left(t \right)} \right)}}{I_{c} M + I_{c} m + M^{2} l^{2} \cos^{2}{\left(\theta{\left(t \right)} \right)} + M l^{2} m}\\\frac{- M^{2} g l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)} - \left(I_{c} + M l^{2}\right) \left(- M g - g m + k x{\left(t \right)}\right)}{- M^{2} l^{2} \sin^{2}{\left(\theta{\left(t \right)} \right)} + \left(I_{c} + M l^{2}\right) \left(M + m\right)}\end{matrix}\right]$
In [40]:
sm.solve(sdot_no_motion, theta, x)

Out[40]:
$\displaystyle \left[ \left\{ \theta{\left(t \right)} : 0, \ x{\left(t \right)} : \frac{g \left(M + m\right)}{k}\right\}, \ \left\{ \theta{\left(t \right)} : 0, \ x{\left(t \right)} : \frac{g \left(M + m\right)}{k}\right\}, \ \left\{ \theta{\left(t \right)} : \pi, \ x{\left(t \right)} : \frac{g \left(M + m\right)}{k}\right\}, \ \left\{ \theta{\left(t \right)} : \pi, \ x{\left(t \right)} : \frac{g \left(M + m\right)}{k}\right\}\right]$

# Linearize¶

In [41]:
f

Out[41]:
$\displaystyle \left[\begin{matrix}- M g + \frac{M \left(- 2 l \omega^{2}{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} - 2 l \sin{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \omega{\left(t \right)} + 2 \frac{d}{d t} v{\left(t \right)}\right)}{2} + b v{\left(t \right)} - g m + k x{\left(t \right)} + m \frac{d}{d t} v{\left(t \right)}\\I_{c} \frac{d}{d t} \omega{\left(t \right)} + M g l \sin{\left(\theta{\left(t \right)} \right)} + M l^{2} \frac{d}{d t} \omega{\left(t \right)} - M l \sin{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} v{\left(t \right)}\end{matrix}\right]$
In [42]:
vbar = sm.Matrix([omega.diff(t), v.diff(t), omega, v, theta, x])
vbar

Out[42]:
$\displaystyle \left[\begin{matrix}\frac{d}{d t} \omega{\left(t \right)}\\\frac{d}{d t} v{\left(t \right)}\\\omega{\left(t \right)}\\v{\left(t \right)}\\\theta{\left(t \right)}\\x{\left(t \right)}\end{matrix}\right]$
In [43]:
vbareq = sm.Matrix([0, 0, 0, 0, 0, g*(M + m)/k])
vbareq

Out[43]:
$\displaystyle \left[\begin{matrix}0\\0\\0\\0\\0\\\frac{g \left(M + m\right)}{k}\end{matrix}\right]$
In [44]:
vbar_repl = {k: v for k, v in zip(vbar, vbareq)}  # dictionary comphrension
vbar_repl

Out[44]:
$\displaystyle \left\{ \omega{\left(t \right)} : 0, \ \theta{\left(t \right)} : 0, \ v{\left(t \right)} : 0, \ x{\left(t \right)} : \frac{g \left(M + m\right)}{k}, \ \frac{d}{d t} \omega{\left(t \right)} : 0, \ \frac{d}{d t} v{\left(t \right)} : 0\right\}$
In [45]:
flin = f.subs(vbar_repl) + f.jacobian(vbar).subs(vbar_repl)*(vbar - vbareq)
flin

Out[45]:
$\displaystyle \left[\begin{matrix}- M g + b v{\left(t \right)} - g m + g \left(M + m\right) + k \left(- \frac{g \left(M + m\right)}{k} + x{\left(t \right)}\right) + \left(M + m\right) \frac{d}{d t} v{\left(t \right)}\\M g l \theta{\left(t \right)} + \left(I_{c} + M l^{2}\right) \frac{d}{d t} \omega{\left(t \right)}\end{matrix}\right]$
In [46]:
f.jacobian(vbar)

Out[46]:
$\displaystyle \left[\begin{matrix}- M l \sin{\left(\theta{\left(t \right)} \right)} & M + m & - 2 M l \omega{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} & b & \frac{M \left(2 l \omega^{2}{\left(t \right)} \sin{\left(\theta{\left(t \right)} \right)} - 2 l \cos{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \omega{\left(t \right)}\right)}{2} & k\\I_{c} + M l^{2} & - M l \sin{\left(\theta{\left(t \right)} \right)} & 0 & 0 & M g l \cos{\left(\theta{\left(t \right)} \right)} - M l \cos{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} v{\left(t \right)} & 0\end{matrix}\right]$

# Linear canonical form¶

In [47]:
cbar = sm.Matrix([theta, x])
cbar

Out[47]:
$\displaystyle \left[\begin{matrix}\theta{\left(t \right)}\\x{\left(t \right)}\end{matrix}\right]$
In [48]:
K = flin.jacobian(cbar)
K

Out[48]:
$\displaystyle \left[\begin{matrix}0 & k\\M g l & 0\end{matrix}\right]$
In [49]:
sbar = sm.Matrix([omega, v])
sbar

Out[49]:
$\displaystyle \left[\begin{matrix}\omega{\left(t \right)}\\v{\left(t \right)}\end{matrix}\right]$
In [50]:
C = flin.jacobian(sbar)
C

Out[50]:
$\displaystyle \left[\begin{matrix}0 & b\\0 & 0\end{matrix}\right]$
In [51]:
M_mat = flin.jacobian(sbar.diff(t))
M_mat

Out[51]:
$\displaystyle \left[\begin{matrix}0 & M + m\\I_{c} + M l^{2} & 0\end{matrix}\right]$
In [52]:
zero_repl = {k: v for k, v in zip(vbar, [0, 0, 0, 0, 0, 0])}
zero_repl

Out[52]:
$\displaystyle \left\{ \omega{\left(t \right)} : 0, \ \theta{\left(t \right)} : 0, \ v{\left(t \right)} : 0, \ x{\left(t \right)} : 0, \ \frac{d}{d t} \omega{\left(t \right)} : 0, \ \frac{d}{d t} v{\left(t \right)} : 0\right\}$
In [53]:
F = -flin.subs(zero_repl)
F

Out[53]:
$\displaystyle \left[\begin{matrix}M g + g m\\0\end{matrix}\right]$

# Simulate the linear system¶

In [54]:
from resonance.linear_systems import MultiDoFLinearSystem

sys = MultiDoFLinearSystem()

sys.constants['m'] = 1.0  # kg
sys.constants['k'] = 10.0  # N/m
sys.constants['b'] = 5.0  # Ns
sys.constants['l'] = 0.5  # m
sys.constants['M'] = 0.5  # kg
sys.constants['Ic'] = 0.5*0.5**2  # kg m**2
sys.constants['g'] = 9.81  # m/s**2
sys.coordinates['theta'] = 1.0  # rad
sys.coordinates['x'] = 0.0  # m
sys.speeds['omega'] = 0.0 # rad/s
sys.speeds['v'] = 0.0 # m/s

In [55]:
import numpy as np

In [56]:
np.array([[1, 2], [3, 4]])

Out[56]:
array([[1, 2],
[3, 4]])
In [57]:
def calc_coeff_mats(m, k, b, l, M, Ic, g):
M_mat = np.array([[0, M+m],
[Ic+m*l**2, 0]])
C = np.array([[0, b],
[0, 0]])
K = np.array([[0, k],
[M*g*l, 0]])
return M_mat, C, K

In [58]:
sys.canonical_coeffs_func = calc_coeff_mats

In [59]:
sys.canonical_coefficients()

Out[59]:
(array([[0.   , 1.5  ],
[0.375, 0.   ]]), array([[0., 5.],
[0., 0.]]), array([[ 0.    , 10.    ],
[ 2.4525,  0.    ]]))
In [60]:
sys.coordinates['x'] = 0.1

In [61]:
traj = sys.free_response(5.0)

In [62]:
traj[['x', 'theta']].plot(subplots=True)

Out[62]:
array([<matplotlib.axes._subplots.AxesSubplot object at 0x7f8dab554f60>,
<matplotlib.axes._subplots.AxesSubplot object at 0x7f8dab444ba8>],
dtype=object)