sympy Kane linearization

kane_linearization.py

import sympy
import sympy.utilities
from sympy import symbols
from sympy.physics.mechanics import *

#based on http://pydy.org/index.php?title=Double_Pendulum

q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
l, m, g = symbols('l m g')

N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = N.orientnew('B', 'Axis', [q2, N.z])

A.set_ang_vel(N, u1 * N.z)
B.set_ang_vel(N, u2 * N.z)

O = Point('O')
P = O.locatenew('P', l * A.x)
R = P.locatenew('R', l * B.x)

O.set_vel(N, 0)
P.v2pt_theory(O, N, A)
R.v2pt_theory(P, N, B)

ParP = Particle()
ParR = Particle()

ParP.point = P
ParR.point = R
ParP.mass = m
ParR.mass = m


kd = [q1d - u1, q2d - u2]

tau = dynamicsymbols('tau')        #torque at joint

FL = [  (P, m * g * N.x),   #gravity
        (R, m * g * N.x),   #gravity
        (B,tau*N.z),
        (A,-tau*N.z)  
        ]  
        
BL = [ParP, ParR]

KM = Kane(N)
KM.coords([q1, q2])
KM.speeds([u1, u2])
KM.kindiffeq(kd)

(fr, frstar) = KM.kanes_equations(FL, BL)
kdd = KM.kindiffdict()
mm = KM.mass_matrix_full
fo = KM.forcing_full
qudots = mm.inv() * fo
qudots = qudots.subs(kdd)
qudots.simplify()
mechanics_printing()
qudots_constants = qudots.subs({g:9.8,m:1,l:1})

B1 = sympy.Matrix([sympy.diff(qudots_constants[i],tau) for i in range(len(qudots_constants))])
B2 = mm.inv() * KM.linearize()[1]

print B1
print B2