Gilbert's method of generating the bicycle equations of motion.

gilbert_whipple.py

from sympy import *
from sympy.physics.mechanics import *

print('Calculation of Linearized Bicycle \"A\" Matrix, with States: Roll, Steer, Roll Rate, Steer Rate')

Vector.simp = False
mechanics_printing()

q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5')
q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1)
u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6')
u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1)

WFrad, WRrad, htangle, forkoffset = symbols('WFrad WRrad htangle forkoffset')
forklength, framelength, forkcg1 = symbols('forklength framelength forkcg1')
forkcg3, framecg1, framecg3, Iwr11 = symbols('forkcg3 framecg1 framecg3 Iwr11')
Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11')
Iframe22, Iframe33, Iframe31, Ifork11 = symbols('Iframe22 Iframe33 Iframe31 Ifork11')
Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g')
mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr')

N = ReferenceFrame('N')
Y = N.orientnew('Y', 'Axis', [q1, N.z])
R = Y.orientnew('R', 'Axis', [q2, Y.x])
Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y])
WR = ReferenceFrame('WR')
TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y])
Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x])
TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y])
WF = ReferenceFrame('WF')

WR_cont = Point('WR_cont')
WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z)
Steer = WR_mc.locatenew('Steer', framelength * Frame.z)
Frame_mc = WR_mc.locatenew('Frame_mc', -framecg1 * Frame.x + framecg3 * Frame.z)
Fork_mc = Steer.locatenew('Fork_mc', -forkcg1 * Fork.x + forkcg3 * Fork.z)
WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z)
WF_cont = WF_mc.locatenew('WF_cont', WFrad*(dot(Fork.y, Y.z)*Fork.y - Y.z).normalize())

Y.set_ang_vel(N, u1 * Y.z)
R.set_ang_vel(Y, u2 * R.x)
WR.set_ang_vel(Frame, u3 * Frame.y)
Frame.set_ang_vel(R, u4 * Frame.y)
Fork.set_ang_vel(Frame, u5 * Fork.x)
WF.set_ang_vel(Fork, u6 * Fork.y)

WR_cont.set_vel(N, 0)
WR_mc.v2pt_theory(WR_cont, N, WR)
Steer.v2pt_theory(WR_mc, N, Frame)
Frame_mc.v2pt_theory(WR_mc, N, Frame)
Fork_mc.v2pt_theory(Steer, N, Fork)
WF_mc.v2pt_theory(Steer, N, Fork)
WF_cont.v2pt_theory(WF_mc, N, WF)

Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0, Iframe31), Frame_mc)
Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0, Ifork31), Fork_mc)
WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc)
WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc)

BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I)
BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I)
BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I)
BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I)

print('Before Forming the List of Nonholonomic Constraints.')
kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5]
conlist_speed = [WF_cont.vel(N) & Y.x, WF_cont.vel(N) & Y.y, WF_cont.vel(N) & Y.z]
conlist_coord = [WF_cont.pos_from(WR_cont) & Y.z]

FL = [(Frame_mc, -mframe * g * Y.z), (Fork_mc, -mfork * g * Y.z), (WF_mc, -mwf * g * Y.z), (WR_mc, -mwr * g * Y.z)]
BL = [BodyFrame, BodyFork, BodyWR, BodyWF]

KM = Kane(N)
KM.coords([q1, q2, q5], qdep=[q4], coneqs=conlist_coord)
print('Before Handling of Dependent Speeds.')
KM.speeds([u2, u3, u5], udep=[u1, u4, u6], coneqs=conlist_speed)
KM.kindiffeq(kd)
print('Before Forming Generalized Active and Inertia Forces, Fr and Fr*')
(fr, frstar) = KM.kanes_equations(FL, BL)
print('Base Equations of Motion Computed')
PaperRadRear  =  0.3
PaperRadFront =  0.35
HTA           =  evalf.N(pi/2-pi/10)
TrailPaper    =  0.08
rake          =  evalf.N(-(TrailPaper*sin(HTA)-(PaperRadFront*cos(HTA))))
PaperWb       =  1.02
PaperFrameCgX =  0.3
PaperFrameCgZ =  0.9
PaperForkCgX  =  0.9
PaperForkCgZ  =  0.7
FrameLength   =  evalf.N(PaperWb*sin(HTA)-(rake-(PaperRadFront-PaperRadRear)*cos(HTA)))
FrameCGNorm   =  evalf.N((PaperFrameCgZ - PaperRadRear-(PaperFrameCgX/sin(HTA))*cos(HTA))*sin(HTA))
FrameCGPar    =  evalf.N((PaperFrameCgX / sin(HTA) + (PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) * cos(HTA)))
tempa         =  evalf.N((PaperForkCgZ - PaperRadFront))
tempb         =  evalf.N((PaperWb-PaperForkCgX))
tempc         =  evalf.N(sqrt(tempa**2+tempb**2))
PaperForkL    =  evalf.N((PaperWb*cos(HTA)-(PaperRadFront-PaperRadRear)*sin(HTA)))
ForkCGNorm    =  evalf.N(rake+(tempc * sin(pi/2-HTA-acos(tempa/tempc))))
ForkCGPar     =  evalf.N(tempc * cos((pi/2-HTA)-acos(tempa/tempc))-PaperForkL)
v = Symbol('v')

val_dict = {WFrad: PaperRadFront,
            WRrad: PaperRadRear,
            htangle: HTA,
            forkoffset: rake,
            forklength: PaperForkL,
            framelength: FrameLength,
            forkcg1: ForkCGPar,
            forkcg3: ForkCGNorm,
            framecg1: FrameCGNorm,
            framecg3: FrameCGPar,
            Iwr11: 0.0603,
            Iwr22: 0.12,
            Iwf11: 0.1405,
            Iwf22: 0.28,
            Ifork11: 0.05892,
            Ifork22: 0.06,
            Ifork33: 0.00708,
            Ifork31: 0.00756,
            Iframe11: 9.2,
            Iframe22: 11,
            Iframe33: 2.8,
            Iframe31: -2.4,
            mfork: 4,
            mframe: 85,
            mwf: 3,
            mwr: 2,
            g: 9.81,
            q1: 0,
            q2: 0,
            q4: 0,
            q5: 0,
            u1: 0,
            u2: 0,
            u3: v/PaperRadRear,
            u4: 0,
            u5: 0,
            u6: v/PaperRadFront}

kdd = KM.kindiffdict()
print('Before Linearization of the \"Forcing\" Term')
forcing_lin = KM.linearize()[0].subs(val_dict)
MM_full = (KM._k_kqdot).row_join(zeros(4, 6)).col_join((zeros(6, 4)).row_join(KM.mass_matrix))
print('Before Substitution of Numerical Values')
MM_full = MM_full.subs(val_dict)
forcing_lin = forcing_lin.subs(val_dict)
print('Before .evalf() call')

MM_full = MM_full.evalf()
forcing_lin = forcing_lin.evalf()
Amat = MM_full.inv() * forcing_lin
A = Amat.extract([1,2,4,6],[1,2,3,5])
print(A)
print('v = 1')
print(A.subs(v, 1).eigenvals())
print('v = 2')
print(A.subs(v, 2).eigenvals())
print('v = 3')
print(A.subs(v, 3).eigenvals())
print('v = 4')
print(A.subs(v, 4).eigenvals())
print('v = 5')
print(A.subs(v, 5).eigenvals())