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
omegadot
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):
    thetadot = omega
    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)))
    return thetadot, omegadot  # order matters here, match sys.states
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()