%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% INERTIA MATRIX
A01_l = subs(A01i,{ai,di,alphai,qi},{ l1, 0, 0, q1})
A12_l = subs(A01i,{ai,di,alphai,qi},{ l2, 0, 0, q2})
pl1 = A01_l*origin
pl2 = A01*A12_l*origin
JP_l1 = [cross(z0,pl1(1:3)-p0(1:3)), [0 0 0]']
JP_l2 = [cross(z0,pl2(1:3)-p0(1:3)), cross(z1,pl2(1:3)-p1(1:3))]
Jtheta_l1 = [z0 [0 0 0]']
Jtheta_l2 = [z0 z1]

B1 = m1*JP_l1'*JP_l1;
B2 = m2*JP_l2'*JP_l2;

R1 = A01(1:3,1:3)
vIl11 = subs(Il11,{Ixx1 Iyy1 Ixy1 Ixz1 Iyz1},{0 0 0 0 0})
Il1 = R1*vIl11*R1'

R2 = A01(1:3,1:3)*A12(1:3,1:3)
vIl22 = subs(Il22,{Ixx2 Iyy2 Ixy2 Ixz2 Iyz2},{0 0 0 0 0})
Il2 = R2*vIl22*R2'

B3 = Jtheta_l1'*Il1*Jtheta_l1
B4 = Jtheta_l2'*Il2*Jtheta_l2

B = simplify(B1+B2+B3+B4)