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]:
gbar = f.subs({omega.diff(t): 0, v.diff(t): 0})
gbar

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()*gbar
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(gbar)
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['x'] = 0.0  # m
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]:
eval_sdot = sm.lambdify((theta, x, omega, v, m, k, b, l, M, Ic, g), [sdot[0], sdot[1]])

In [29]:
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[29]:
$\displaystyle \left[ 4.144543155486223, \ 51.08691546911525\right]$
In [30]:
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 [31]:
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[31]:
$\displaystyle \left( 3.0, \ 4.0, \ 4.144543155486223, \ 51.08691546911525\right)$
In [32]:
sys.diff_eq_func = eval_derivatives

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

In [34]:
%matplotlib widget

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

Out[35]:
array([<matplotlib.axes._subplots.AxesSubplot object at 0x7f9af4e4fa90>,
<matplotlib.axes._subplots.AxesSubplot object at 0x7f9af4d42ba8>],
dtype=object)

# Find the equilibrium¶

In [36]:
sdot

Out[36]:
$\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 [37]:
sdot_no_motion = sdot.subs({omega: 0, v: 0})

In [38]:
sm.simplify(sdot_no_motion)

Out[38]:
$\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 [39]:
sm.solve(sdot_no_motion, theta, x)

Out[39]:
$\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]$