In [1]:
import sympy as sm

In [2]:
sm.init_printing()  # improves symbolic math display

In [3]:
mb, mc, Ib, Ic, r, l, d, g = sm.symbols('m_b, m_c, I_b, I_c, r, l, d, g')

In [4]:
mb

Out[4]:
$\displaystyle m_{b}$
In [5]:
Ib

Out[5]:
$\displaystyle I_{b}$
In [6]:
r+Ib**2

Out[6]:
$\displaystyle I_{b}^{2} + r$
In [7]:
sm.sin(r) + sm.sqrt(mb)

Out[7]:
$\displaystyle \sqrt{m_{b}} + \sin{\left(r \right)}$
In [8]:
t = sm.symbols('t')

In [9]:
theta = sm.Function('theta')(t)
theta

Out[9]:
$\displaystyle \theta{\left(t \right)}$
In [10]:
omega = sm.Function('omega')(t)
omega

Out[10]:
$\displaystyle \omega{\left(t \right)}$
In [11]:
sm.diff(theta, t)

Out[11]:
$\displaystyle \frac{d}{d t} \theta{\left(t \right)}$
In [12]:
sm.diff(theta**2+sm.sin(theta**2/2), t)

Out[12]:
$\displaystyle \theta{\left(t \right)} \cos{\left(\frac{\theta^{2}{\left(t \right)}}{2} \right)} \frac{d}{d t} \theta{\left(t \right)} + 2 \theta{\left(t \right)} \frac{d}{d t} \theta{\left(t \right)}$
In [13]:
d = 2*r*sm.sin(theta/2)
d

Out[13]:
$\displaystyle 2 r \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)}$
In [14]:
h = l - sm.sqrt(l**2 - d**2)
h

Out[14]:
$\displaystyle l - \sqrt{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}$
In [15]:
v = sm.diff(h, t)
v

Out[15]:
$\displaystyle \frac{2 r^{2} \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)} \frac{d}{d t} \theta{\left(t \right)}}{\sqrt{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}}$
In [16]:
v = h.diff(t)
v

Out[16]:
$\displaystyle \frac{2 r^{2} \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)} \frac{d}{d t} \theta{\left(t \right)}}{\sqrt{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}}$
In [17]:
v = v.subs({theta.diff(t): omega})
v

Out[17]:
$\displaystyle \frac{2 r^{2} \omega{\left(t \right)} \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{\sqrt{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}}$
In [18]:
T = (mb + mc)*v**2/2
T

Out[18]:
$\displaystyle \frac{2 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}$
In [19]:
sm.S(1)/2*(mb+mc)*v**2

Out[19]:
$\displaystyle \frac{4 r^{4} \left(\frac{m_{b}}{2} + \frac{m_{c}}{2}\right) \omega^{2}{\left(t \right)} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}$
In [20]:
T = (mb + mc)*v**2/2 + (Ib + Ic)*omega**2/2 # no time dervitives!!
T

Out[20]:
$\displaystyle \frac{2 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \frac{\left(I_{b} + I_{c}\right) \omega^{2}{\left(t \right)}}{2}$
In [21]:
U = (mb + mc)*g*h  # no time derivatives
U

Out[21]:
$\displaystyle g \left(l - \sqrt{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}\right) \left(m_{b} + m_{c}\right)$
In [22]:
L = T - U

In [23]:
L.diff(omega)

Out[23]:
$\displaystyle \frac{4 r^{4} \left(m_{b} + m_{c}\right) \omega{\left(t \right)} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \left(I_{b} + I_{c}\right) \omega{\left(t \right)}$
In [24]:
L.diff(theta)

Out[24]:
$\displaystyle - \frac{2 g r^{2} \left(m_{b} + m_{c}\right) \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{\sqrt{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}} + \frac{8 r^{6} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{\left(l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}\right)^{2}} - \frac{2 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \frac{2 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}$
In [25]:
L.diff(omega).diff(t)

Out[25]:
$\displaystyle \frac{16 r^{6} \left(m_{b} + m_{c}\right) \omega{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \frac{d}{d t} \theta{\left(t \right)}}{\left(l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}\right)^{2}} - \frac{4 r^{4} \left(m_{b} + m_{c}\right) \omega{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)} \frac{d}{d t} \theta{\left(t \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \frac{4 r^{4} \left(m_{b} + m_{c}\right) \omega{\left(t \right)} \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \frac{d}{d t} \theta{\left(t \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \frac{4 r^{4} \left(m_{b} + m_{c}\right) \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \frac{d}{d t} \omega{\left(t \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \left(I_{b} + I_{c}\right) \frac{d}{d t} \omega{\left(t \right)}$
In [26]:
L.diff(omega).diff(t).subs({theta.diff(t): omega})

Out[26]:
$\displaystyle \frac{16 r^{6} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{\left(l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}\right)^{2}} - \frac{4 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \frac{4 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \frac{4 r^{4} \left(m_{b} + m_{c}\right) \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \frac{d}{d t} \omega{\left(t \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \left(I_{b} + I_{c}\right) \frac{d}{d t} \omega{\left(t \right)}$
In [27]:
f = L.diff(omega).diff(t).subs({theta.diff(t): omega}) - L.diff(theta)
f

Out[27]:
$\displaystyle \frac{2 g r^{2} \left(m_{b} + m_{c}\right) \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{\sqrt{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}} + \frac{8 r^{6} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{\left(l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}\right)^{2}} - \frac{2 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \frac{2 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \frac{4 r^{4} \left(m_{b} + m_{c}\right) \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \frac{d}{d t} \omega{\left(t \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \left(I_{b} + I_{c}\right) \frac{d}{d t} \omega{\left(t \right)}$
In [28]:
g = f.subs({omega.diff(t): 0})
g

Out[28]:
$\displaystyle \frac{2 g r^{2} \left(m_{b} + m_{c}\right) \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{\sqrt{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}} + \frac{8 r^{6} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{\left(l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}\right)^{2}} - \frac{2 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \frac{2 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}$
In [29]:
I_sys = f.coeff(omega.diff(t))
I_sys

Out[29]:
$\displaystyle I_{b} + I_{c} + \frac{4 r^{4} \left(m_{b} + m_{c}\right) \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}$
In [30]:
omegadot = -g/I_sys

Out[30]:
$\displaystyle \frac{- \frac{2 g r^{2} \left(m_{b} + m_{c}\right) \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{\sqrt{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}} - \frac{8 r^{6} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{\left(l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}\right)^{2}} + \frac{2 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} - \frac{2 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}}{I_{b} + I_{c} + \frac{4 r^{4} \left(m_{b} + m_{c}\right) \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}}$
In [31]:
f.diff(omega.diff(t))

Out[31]:
$\displaystyle I_{b} + I_{c} + \frac{4 r^{4} \left(m_{b} + m_{c}\right) \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}$
In [32]:
from resonance.nonlinear_systems import SingleDoFNonLinearSystem
import numpy as np

In [33]:
sys = SingleDoFNonLinearSystem()

In [34]:
sys.constants['m_b'] = 1  # kg
sys.constants['m_c'] = 2.6  # kg
sys.constants['r'] = 0.3  # m
sys.constants['l'] = 0.75  # m
sys.constants['g'] = 9.81  # m/s**2
sys.constants['I_b'] = 1.0*0.3**2  # kg m**2
sys.constants['I_c'] = 2.6*0.3**2  # kg m**2

In [35]:
sys.constants

Out[35]:
{'m_b': 1,
'm_c': 2.6,
'r': 0.3,
'l': 0.75,
'g': 9.81,
'I_b': 0.09,
'I_c': 0.23399999999999999}
In [36]:
sys.coordinates['theta'] = np.deg2rad(10.0)  # rad
sys.speeds['omega'] = 0.0

In [37]:
str(omegadot).replace('(t)', '').replace('sin(', 'np.sin(').replace('cos(', 'np.cos(')

Out[37]:
'(-2*g*r**2*(m_b + m_c)*np.sin(theta/2)*np.cos(theta/2)/sqrt(l**2 - 4*r**2*np.sin(theta/2)**2) - 8*r**6*(m_b + m_c)*omega**2*np.sin(theta/2)**3*np.cos(theta/2)**3/(l**2 - 4*r**2*np.sin(theta/2)**2)**2 + 2*r**4*(m_b + m_c)*omega**2*np.sin(theta/2)**3*np.cos(theta/2)/(l**2 - 4*r**2*np.sin(theta/2)**2) - 2*r**4*(m_b + m_c)*omega**2*np.sin(theta/2)*np.cos(theta/2)**3/(l**2 - 4*r**2*np.sin(theta/2)**2))/(I_b + I_c + 4*r**4*(m_b + m_c)*np.sin(theta/2)**2*np.cos(theta/2)**2/(l**2 - 4*r**2*np.sin(theta/2)**2))'
In [38]:
omegadot.free_symbols

Out[38]:
$\displaystyle \left\{I_{b}, I_{c}, g, l, m_{b}, m_{c}, r, t\right\}$
In [39]:
sys.states

Out[39]:
_StatesDict([('theta', 0.17453292519943295), ('omega', 0.0)])
In [40]:
def calc_derivatives(theta, omega, l, g, r, m_b, m_c, I_b, I_c):
omegadot = ((-2*g*r**2*(m_b + m_c)*np.sin(theta/2)*np.cos(theta/2)/np.sqrt(l**2 - 4*r**2*np.sin(theta/2)**2) -
8*r**6*(m_b + m_c)*omega**2*np.sin(theta/2)**3*np.cos(theta/2)**3/(l**2 - 4*r**2*np.sin(theta/2)**2)**2 + 2*r**4*(m_b + m_c)*omega**2*np.sin(theta/2)**3*np.cos(theta/2)/(l**2 - 4*r**2*np.sin(theta/2)**2) - 2*r**4*(m_b + m_c)*omega**2*np.sin(theta/2)*np.cos(theta/2)**3/(l**2 - 4*r**2*np.sin(theta/2)**2))/(I_b + I_c +
4*r**4*(m_b + m_c)*np.sin(theta/2)**2*np.cos(theta/2)**2/(l**2 - 4*r**2*np.sin(theta/2)**2)))

In [41]:
calc_derivatives(1.0, 2.0, 10.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0)

Out[41]:
$\displaystyle \left( 2.0, \ -4.341602071001939\right)$
In [42]:
sys.diff_eq_func = calc_derivatives

In [43]:
traj = sys.free_response(10.0)

In [44]:
%matplotlib widget

In [45]:
traj.plot(subplots=True)

Out[45]:
array([<matplotlib.axes._subplots.AxesSubplot object at 0x7f928324bd30>,
<matplotlib.axes._subplots.AxesSubplot object at 0x7f92831c2c88>,
<matplotlib.axes._subplots.AxesSubplot object at 0x7f9283182128>],
dtype=object)
In [46]:
def calc_height(theta, r, l):
return l - np.sqrt(l**2 - (2*r*np.sin(theta/2))**2)

In [47]:
sys.add_measurement('h', calc_height)

In [48]:
traj = sys.free_response(10.0)

In [49]:
traj

Out[49]:
theta theta_acc omega h
time
0.00 0.174533 -2.265874 0.000000 0.001825
0.01 0.174420 -2.264439 -0.022654 0.001823
0.02 0.174080 -2.260137 -0.045279 0.001816
0.03 0.173514 -2.252970 -0.067847 0.001804
0.04 0.172723 -2.242947 -0.090329 0.001788
... ... ... ... ...
9.96 -0.032323 0.424660 0.619826 0.000063
9.97 -0.026105 0.343017 0.623665 0.000041
9.98 -0.019852 0.260890 0.626685 0.000024
9.99 -0.013574 0.178396 0.628881 0.000011
10.00 -0.007277 0.095650 0.630252 0.000003

1001 rows × 4 columns

In [50]:
traj.plot(subplots=True)

Out[50]:
array([<matplotlib.axes._subplots.AxesSubplot object at 0x7f92830ae5f8>,
<matplotlib.axes._subplots.AxesSubplot object at 0x7f92830dc860>,
<matplotlib.axes._subplots.AxesSubplot object at 0x7f928308ec88>,
<matplotlib.axes._subplots.AxesSubplot object at 0x7f9283042f60>],
dtype=object)
In [51]:
f

Out[51]:
$\displaystyle \frac{2 g r^{2} \left(m_{b} + m_{c}\right) \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{\sqrt{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}} + \frac{8 r^{6} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{\left(l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}\right)^{2}} - \frac{2 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \frac{2 r^{4} \left(m_{b} + m_{c}\right) \omega^{2}{\left(t \right)} \sin{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{3}{\left(\frac{\theta{\left(t \right)}}{2} \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \frac{4 r^{4} \left(m_{b} + m_{c}\right) \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \cos^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)} \frac{d}{d t} \omega{\left(t \right)}}{l^{2} - 4 r^{2} \sin^{2}{\left(\frac{\theta{\left(t \right)}}{2} \right)}} + \left(I_{b} + I_{c}\right) \frac{d}{d t} \omega{\left(t \right)}$
In [52]:
f_lin = f.subs({
sm.sin(theta/2): theta/2,
sm.cos(theta/2): 1,
omega**2: 0,
theta**2: 0,
})
f_lin

Out[52]:
$\displaystyle \frac{g r^{2} \left(m_{b} + m_{c}\right) \theta{\left(t \right)}}{\sqrt{l^{2}}} + \left(I_{b} + I_{c}\right) \frac{d}{d t} \omega{\left(t \right)}$
In [53]:
w = sm.symbols('w', real=True, positive=True)

In [54]:
sm.sqrt(w**2)

Out[54]:
$\displaystyle w$
In [55]:
m = f_lin.coeff(omega.diff(t))
m

Out[55]:
$\displaystyle I_{b} + I_{c}$
In [56]:
k = f_lin.coeff(theta)
k

Out[56]:
$\displaystyle \frac{g r^{2} \left(m_{b} + m_{c}\right)}{\sqrt{l^{2}}}$
In [57]:
wn = sm.sqrt(k/m)
wn

Out[57]:
$\displaystyle \sqrt{\frac{g r^{2} \left(m_{b} + m_{c}\right)}{\left(I_{b} + I_{c}\right) \sqrt{l^{2}}}}$
In [58]:
period = 2*sm.pi/wn
period

Out[58]:
$\displaystyle \frac{2 \pi}{\sqrt{\frac{g r^{2} \left(m_{b} + m_{c}\right)}{\left(I_{b} + I_{c}\right) \sqrt{l^{2}}}}}$
In [59]:
T = sm.symbols('T')

In [60]:
period_eq = sm.Eq(T, period)
period_eq

Out[60]:
$\displaystyle T = \frac{2 \pi}{\sqrt{\frac{g r^{2} \left(m_{b} + m_{c}\right)}{\left(I_{b} + I_{c}\right) \sqrt{l^{2}}}}}$
In [61]:
sm.solve(period_eq, Ic)

Out[61]:
$\displaystyle \left[ \frac{- \pi^{2} I_{b} \sqrt{l^{2}} + \frac{T^{2} g r^{2} \left(m_{b} + m_{c}\right)}{4}}{\pi^{2} \sqrt{l^{2}}}\right]$
In [62]:
from resonance.linear_systems import SingleDoFLinearSystem

In [63]:
linsys = SingleDoFLinearSystem()

In [64]:
linsys.constants['m_b'] = 1  # kg
linsys.constants['m_c'] = 2.6  # kg
linsys.constants['r'] = 0.3  # m
linsys.constants['l'] = 0.75  # m
linsys.constants['g'] = 9.81  # m/s**2
linsys.constants['I_b'] = 1.0*0.3**2  # kg m**2
linsys.constants['I_c'] = 2.6*0.3**2  # kg m**2

In [65]:
linsys.coordinates['theta'] = np.deg2rad(10.0)
linsys.speeds['omega'] = 0.0

In [66]:
m, k

Out[66]:
$\displaystyle \left( I_{b} + I_{c}, \ \frac{g r^{2} \left(m_{b} + m_{c}\right)}{\sqrt{l^{2}}}\right)$
In [67]:
def calc_coeffs(I_b, I_c, g, r, m_b, m_c, l):
m = I_b + I_c
b = 0.0
k = g*r**2*(m_b+m_c)/l

return m, b, k

In [68]:
linsys.canonical_coeffs_func = calc_coeffs

In [69]:
traj = linsys.free_response(10.0)

In [70]:
traj.plot(subplots=True)

Out[70]:
array([<matplotlib.axes._subplots.AxesSubplot object at 0x7f92832b7c88>,
<matplotlib.axes._subplots.AxesSubplot object at 0x7f9282f0dda0>,
<matplotlib.axes._subplots.AxesSubplot object at 0x7f9282ebbd68>],
dtype=object)
In [71]:
from resonance.linear_systems import SingleDoFLinearSystem as CarModel

In [72]:
sys = CarModel()