import sympy as sm
sm.init_printing()
m, M, Ic, k, b, l, g = sm.symbols('m, M, I_c, k, b, l, g', real=True, positive=True)
t = sm.symbols('t', real=True)
x = sm.Function('x')(t)
v = sm.Function('v')(t)
theta = sm.Function('theta')(t)
omega = sm.Function('omega')(t)
vc_squared = (v - omega*l*sm.sin(theta))**2 + (omega*l*sm.cos(theta))**2
vc_squared
eq_repl = {theta.diff(t): omega, x.diff(t): v}
eq_repl
T = m*v**2/2 + M*vc_squared/2 + Ic*omega**2/2
T
U = k*x**2/2 -m*g*x - M*g*(x + l*sm.cos(theta))
U
R = b*v**2/2
R
L = T - U
L
leq_v = L.diff(v).diff(t).subs(eq_repl) - L.diff(x) -(-R.diff(v))
leq_v
leq_omega = L.diff(omega).diff(t).subs(eq_repl) - L.diff(theta) -(-R.diff(omega))
leq_omega
leq_omega = sm.simplify(leq_omega)
leq_omega
f = sm.Matrix([leq_v, leq_omega])
f
g = f.subs({omega.diff(t): 0, v.diff(t): 0})
g
I = f.jacobian([omega.diff(t), v.diff(t)])
I
sdot = -I.inv()*g
sdot
sdot = -I.LUsolve(g)
sdot
sm.simplify(sdot)
from resonance.nonlinear_systems import MultiDoFNonLinearSystem
sys = MultiDoFNonLinearSystem()
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
# 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
sys.states
_StatesDict([('theta', 1.0), ('x', 0.0), ('omega', 0.0), ('v', 0.0)])
eval_mass = sm.lambdify((m, M), m+M)
eval_mass(1.0, 2.0)
g = sm.symbols('g', real=True, positive=True)
eval_sdot = sm.lambdify((theta, x, omega, v, m, k, b, l, M, Ic, g), [sdot[0], sdot[1]])
eval_sdot(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0)
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)
thetadot = omega
xdot = v
return thetadot, xdot, omegadot, vdot
eval_derivatives(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0)
sys.diff_eq_func = eval_derivatives
trajectories = sys.free_response(5.0)
%matplotlib widget
trajectories[['x', 'theta']].plot(subplots=True)
Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …
array([<matplotlib.axes._subplots.AxesSubplot object at 0x7f8dab65f4a8>, <matplotlib.axes._subplots.AxesSubplot object at 0x7f8dab5d3be0>], dtype=object)
sdot
sdot_no_motion = sdot.subs({omega: 0, v: 0})
sm.simplify(sdot_no_motion)
sm.solve(sdot_no_motion, theta, x)
f
vbar = sm.Matrix([omega.diff(t), v.diff(t), omega, v, theta, x])
vbar
vbareq = sm.Matrix([0, 0, 0, 0, 0, g*(M + m)/k])
vbareq
vbar_repl = {k: v for k, v in zip(vbar, vbareq)} # dictionary comphrension
vbar_repl
flin = f.subs(vbar_repl) + f.jacobian(vbar).subs(vbar_repl)*(vbar - vbareq)
flin
f.jacobian(vbar)
cbar = sm.Matrix([theta, x])
cbar
K = flin.jacobian(cbar)
K
sbar = sm.Matrix([omega, v])
sbar
C = flin.jacobian(sbar)
C
M_mat = flin.jacobian(sbar.diff(t))
M_mat
zero_repl = {k: v for k, v in zip(vbar, [0, 0, 0, 0, 0, 0])}
zero_repl
F = -flin.subs(zero_repl)
F
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
import numpy as np
np.array([[1, 2], [3, 4]])
array([[1, 2], [3, 4]])
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
sys.canonical_coeffs_func = calc_coeff_mats
sys.canonical_coefficients()
(array([[0. , 1.5 ], [0.375, 0. ]]), array([[0., 5.], [0., 0.]]), array([[ 0. , 10. ], [ 2.4525, 0. ]]))
sys.coordinates['x'] = 0.1
traj = sys.free_response(5.0)
traj[['x', 'theta']].plot(subplots=True)
Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …
array([<matplotlib.axes._subplots.AxesSubplot object at 0x7f8dab554f60>, <matplotlib.axes._subplots.AxesSubplot object at 0x7f8dab444ba8>], dtype=object)