In [1]:

    

import sympy
import sympy.physics.mechanics as mech
sympy.init_printing(use_latex='mathjax')
from IPython.display import display


    

In [2]:

    

def bke(vector, frame_i, frame_b, t):
    return (vector.diff(t, frame_b) + frame_b.ang_vel_in(frame_i).cross(vector))


    

In [3]:

    

T, r, m_w, m_p, l, F, x, g, alpha, theta, t, R_x, R_z, N, J_p, J_w, v_x, omega, k_emf, b_damp, V, J_motor, a = \
    sympy.symbols('T r m_w m_p l F x g alpha theta t R_x R_z N J_p J_w v_x omega k_emf b_damp V J_motor a')


    

In [4]:

    

frame_i = mech.ReferenceFrame('i') #inertial frame
frame_b = frame_i.orientnew('b', 'Axis', [theta(t), frame_i.y]) #fixed in pendulum
frame_w = frame_b.orientnew('w', 'Axis', [-alpha(t), frame_i.y]) #fixed in wheel


    

In [5]:

    

point_o = mech.Point('o') 
point_o.set_vel(frame_i, 0) #point o is inertially fixed

point_W = point_o.locatenew('W', frame_i.x*x(t))  #wheel c.m.
point_W.set_vel(frame_b, 0) #point W is fixed in pendulum frame, too
point_W.set_vel(frame_i, point_W.pos_from(point_o).diff(t, frame_i))

point_P = point_W.locatenew('P', frame_b.z*(-l)) #pendulum c.m.
point_P.set_vel(frame_b, 0)
point_P.v2pt_theory(point_W, frame_i, frame_b);


    

In [6]:

    

# Wheel Creation
J_wheel = mech.inertia(frame_w, 0, J_w, 0)
wheel = mech.RigidBody('wheel', point_W, frame_w, m_w, (J_wheel, point_W))

# Pendulum Creation
J_pend = mech.inertia(frame_b, 0, J_p, 0)
pend = mech.RigidBody('pend', point_P, frame_b, m_p, (J_pend, point_P)) #change inertia point to point_p


    

In [ ]:

    

noslip = [(r*(alpha(t) - theta(t)) - x(t))] # constraint equation, same as before?
flist = [(point_P, m_p*g*frame_i.z), (frame_b, T(t)*frame_i.y), (frame_b, b_damp*alpha(t).diff(t)*frame_i.y)] #force list, external torque T, damping b in y direction 
L = mech.Lagrangian(frame_i, wheel, pend)
#l = mech.LagrangesMethod(L, [x(t), theta(t)], forcelist = flist, hol_coneqs=noslip, frame = frame_i)
l = mech.LagrangesMethod(L, [x(t), theta(t), alpha(t)], forcelist = flist, frame = frame_i)
eoms = l.form_lagranges_equations()
eoms


    

    
Out[ ]:





$$\left[\begin{matrix}\frac{m_{p}}{2} \left(2 l \sin{\left (\theta{\left (t \right )} \right )} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2} - 2 l \cos{\left (\theta{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}}  \theta{\left (t \right )} + 2 \frac{d^{2}}{d t^{2}}  x{\left (t \right )}\right) + m_{w} \frac{d^{2}}{d t^{2}}  x{\left (t \right )}\\J_{p} \frac{d^{2}}{d t^{2}}  \theta{\left (t \right )} + \frac{J_{w}}{2} \left(- \frac{d^{2}}{d t^{2}}  \alpha{\left (t \right )} + \frac{d^{2}}{d t^{2}}  \theta{\left (t \right )}\right) - \frac{J_{w}}{2} \frac{d^{2}}{d t^{2}}  \alpha{\left (t \right )} + \frac{J_{w}}{2} \frac{d^{2}}{d t^{2}}  \theta{\left (t \right )} - b_{damp} \frac{d}{d t} \alpha{\left (t \right )} - g l m_{p} \sin{\left (\theta{\left (t \right )} \right )} - l m_{p} \sin{\left (\theta{\left (t \right )} \right )} \frac{d}{d t} \theta{\left (t \right )} \frac{d}{d t} x{\left (t \right )} + \frac{m_{p}}{2} \left(2 l^{2} \frac{d^{2}}{d t^{2}}  \theta{\left (t \right )} + 2 l \sin{\left (\theta{\left (t \right )} \right )} \frac{d}{d t} \theta{\left (t \right )} \frac{d}{d t} x{\left (t \right )} - 2 l \cos{\left (\theta{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}}  x{\left (t \right )}\right) - T{\left (t \right )}\\- \frac{J_{w}}{2} \left(- \frac{d^{2}}{d t^{2}}  \alpha{\left (t \right )} + \frac{d^{2}}{d t^{2}}  \theta{\left (t \right )}\right) + \frac{J_{w}}{2} \frac{d^{2}}{d t^{2}}  \alpha{\left (t \right )} - \frac{J_{w}}{2} \frac{d^{2}}{d t^{2}}  \theta{\left (t \right )}\end{matrix}\right]$$

In [ ]:

    

eom_sol = sympy.solve(eoms, [theta(t).diff(t,2), alpha(t).diff(t,2), x(t).diff(t,2), \
        alpha(t).diff(t)], simplify=False)
eom_sol


    

In [ ]:

    




    

In [44]:

    

theta_ddot = eom_sol[theta(t).diff(t,2)].expand().ratsimp().collect([theta(t), x(t), theta(t).diff(t), x(t).diff(t)], sympy.factor)
theta_ddot


    

    
Out[44]:





$$\frac{1}{\left(J_{p} m_{p} + J_{p} m_{w} + l^{2} m_{p}^{2} \sin^{2}{\left (\theta{\left (t \right )} \right )} + l^{2} m_{p} m_{w}\right) \cos{\left (\theta{\left (t \right )} \right )}} \left(\left(m_{p} + m_{w}\right) \left(b_{damp} \frac{d}{d t} \alpha{\left (t \right )} + g l m_{p} \sin{\left (\theta{\left (t \right )} \right )} + T{\left (t \right )}\right) \cos{\left (\theta{\left (t \right )} \right )} + \left(J_{p} m_{p} \sin{\left (\theta{\left (t \right )} \right )} - J_{p} m_{p} \cos{\left (\theta{\left (t \right )} \right )} \tan{\left (\theta{\left (t \right )} \right )} + J_{p} m_{w} \sin{\left (\theta{\left (t \right )} \right )} - J_{p} m_{w} \cos{\left (\theta{\left (t \right )} \right )} \tan{\left (\theta{\left (t \right )} \right )} + l^{2} m_{p}^{2} \sin^{3}{\left (\theta{\left (t \right )} \right )} - l^{2} m_{p}^{2} \cos{\left (\theta{\left (t \right )} \right )} \tan{\left (\theta{\left (t \right )} \right )} + l^{2} m_{p} m_{w} \sin{\left (\theta{\left (t \right )} \right )} - l^{2} m_{p} m_{w} \cos{\left (\theta{\left (t \right )} \right )} \tan{\left (\theta{\left (t \right )} \right )}\right) \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2}\right)$$

In [35]:

    

#l.forcing
l.mass_matrix


    

    
Out[35]:





$$\left[\begin{matrix}m_{p} + m_{w} & - l m_{p} \cos{\left (\theta{\left (t \right )} \right )} & 0\\- l m_{p} \cos{\left (\theta{\left (t \right )} \right )} & J_{p} + J_{w} + l^{2} m_{p} & - J_{w}\\0 & - J_{w} & J_{w}\end{matrix}\right]$$

In [ ]:

    




    

In [ ]: