import sympy as sm
sm.init_printing() # improves symbolic math display
mb, mc, Ib, Ic, r, l, d, g = sm.symbols('m_b, m_c, I_b, I_c, r, l, d, g')
mb
Ib
r+Ib**2
sm.sin(r) + sm.sqrt(mb)
t = sm.symbols('t')
theta = sm.Function('theta')(t)
theta
omega = sm.Function('omega')(t)
omega
sm.diff(theta, t)
sm.diff(theta**2+sm.sin(theta**2/2), t)
d = 2*r*sm.sin(theta/2)
d
h = l - sm.sqrt(l**2 - d**2)
h
v = sm.diff(h, t)
v
v = h.diff(t)
v
v = v.subs({theta.diff(t): omega})
v
T = (mb + mc)*v**2/2
T
sm.S(1)/2*(mb+mc)*v**2
T = (mb + mc)*v**2/2 + (Ib + Ic)*omega**2/2 # no time dervitives!!
T
U = (mb + mc)*g*h # no time derivatives
U
L = T - U
L.diff(omega)
L.diff(theta)
L.diff(omega).diff(t)
L.diff(omega).diff(t).subs({theta.diff(t): omega})
f = L.diff(omega).diff(t).subs({theta.diff(t): omega}) - L.diff(theta)
f
g = f.subs({omega.diff(t): 0})
g
I_sys = f.coeff(omega.diff(t))
I_sys
omegadot = -g/I_sys
omegadot
f.diff(omega.diff(t))
from resonance.nonlinear_systems import SingleDoFNonLinearSystem
import numpy as np
sys = SingleDoFNonLinearSystem()
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
sys.constants
{'m_b': 1, 'm_c': 2.6, 'r': 0.3, 'l': 0.75, 'g': 9.81, 'I_b': 0.09, 'I_c': 0.23399999999999999}
sys.coordinates['theta'] = np.deg2rad(10.0) # rad
sys.speeds['omega'] = 0.0
str(omegadot).replace('(t)', '').replace('sin(', 'np.sin(').replace('cos(', 'np.cos(')
'(-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))'
omegadot.free_symbols
sys.states
_StatesDict([('theta', 0.17453292519943295), ('omega', 0.0)])
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
calc_derivatives(1.0, 2.0, 10.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0)
sys.diff_eq_func = calc_derivatives
traj = sys.free_response(10.0)
%matplotlib widget
traj.plot(subplots=True)
Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …
array([<matplotlib.axes._subplots.AxesSubplot object at 0x7f928324bd30>, <matplotlib.axes._subplots.AxesSubplot object at 0x7f92831c2c88>, <matplotlib.axes._subplots.AxesSubplot object at 0x7f9283182128>], dtype=object)
def calc_height(theta, r, l):
return l - np.sqrt(l**2 - (2*r*np.sin(theta/2))**2)
sys.add_measurement('h', calc_height)
traj = sys.free_response(10.0)
traj
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
traj.plot(subplots=True)
Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …
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)
f
f_lin = f.subs({
sm.sin(theta/2): theta/2,
sm.cos(theta/2): 1,
omega**2: 0,
theta**2: 0,
})
f_lin
w = sm.symbols('w', real=True, positive=True)
sm.sqrt(w**2)
m = f_lin.coeff(omega.diff(t))
m
k = f_lin.coeff(theta)
k
wn = sm.sqrt(k/m)
wn
period = 2*sm.pi/wn
period
T = sm.symbols('T')
period_eq = sm.Eq(T, period)
period_eq
sm.solve(period_eq, Ic)
from resonance.linear_systems import SingleDoFLinearSystem
linsys = SingleDoFLinearSystem()
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
linsys.coordinates['theta'] = np.deg2rad(10.0)
linsys.speeds['omega'] = 0.0
m, k
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
linsys.canonical_coeffs_func = calc_coeffs
traj = linsys.free_response(10.0)
traj.plot(subplots=True)
Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …
array([<matplotlib.axes._subplots.AxesSubplot object at 0x7f92832b7c88>, <matplotlib.axes._subplots.AxesSubplot object at 0x7f9282f0dda0>, <matplotlib.axes._subplots.AxesSubplot object at 0x7f9282ebbd68>], dtype=object)
from resonance.linear_systems import SingleDoFLinearSystem as CarModel
sys = CarModel()