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
gbar = f.subs({omega.diff(t): 0, v.diff(t): 0})
gbar
I = f.jacobian([omega.diff(t), v.diff(t)])
I
sdot = -I.inv()*gbar
sdot
sdot = -I.LUsolve(gbar)
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)
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 0x7f9af4e4fa90>, <matplotlib.axes._subplots.AxesSubplot object at 0x7f9af4d42ba8>], dtype=object)
sdot
sdot_no_motion = sdot.subs({omega: 0, v: 0})
sm.simplify(sdot_no_motion)
sm.solve(sdot_no_motion, theta, x)