%% set up symbol
syms a1 a2 a3 a1d a2d a3d a1dd a2dd a3dd m1 m2 m3 l1 l2 l3 grav

thetas = [ a1; a2; a3];
thetads = [ a1d; a2d; a3d];
thetadds = [ a1dd; a2dd; a3dd ];
masses = [ m1; m2; m3 ];
lengths = [ l1; l2; l3 ];

inertias = masses .* lengths .* lengths / 12;

%% positions and angles of links
posns = sym( zeros(2, length(thetas) ) );
angles = mycumsum(thetas);
joints = sym( zeros(2, length(thetas) ) );
joints(:,1) = [ 0; 0];
posns(:,1) = l1/2 * [cos(a1); sin(a1) ];
for i = 2:length(thetas)
    joints(:,i) = joints(:,i-1) ...
        + lengths(i-1) * [ cos(angles(i-1)); sin(angles(i-1)) ];
    posns(:,i) = joints(:,i) ...
        + lengths(i)/2 * [ cos(angles(i)); sin(angles(i)) ];
end

%% Velocities of link centers of mass
posnsd = jacobian(posns, thetas) * thetads;

%% Angular velocities of links
angleds = jacobian( angles, thetas ) * thetads;

%% Squared norms of velocities of link centers of mass
sqrdposnsd = posnsd .* posnsd;
sumsqrdposnsd = sym(zeros(length(thetas),1));
for i = 1:length(thetas)
    sumsqrdposnsd(i) = sum(sqrdposnsd(i*2-1:i*2));
end

%% Kinetic energy of system
ke = 1/2 * sum(masses .* sumsqrdposnsd ...
    + inertias .* angleds .* angleds);

%% Potential energy of system
pe = sum(posns(2,:) * grav);

%% Lagrangian
L = ke - pe;

%% Intermediate results
dLdqdot = jacobian( L, thetads ) .';
dLdq = jacobian( L, thetas) .';
dtdLdqdot = jacobian(dLdqdot, [thetas; thetads]) ...
    * [thetads; thetadds];

Q = dtdLdqdot - dLdq;