(* Inverse dynamics for a two joint mechanism using Lagrangian approach *)
(* Moment of inertia about center of mass of each link *)
(* Starting position is hanging straight down */

(* Standard coordinate system. *)
gravity = {0, -G} 
(*Define rotation matrices that allow us to do the forward kinematics.*)
(*R01 rotates from the link 1 frame to the link 0 frame.*)

r01={{Cos[a1],-Sin[a1]},{Sin[a1],Cos[a1]}}
r10=Transpose[r01]
r12={{Cos[a2],-Sin[a2]},{Sin[a2],Cos[a2]}}
r21=Transpose[r12]

(*Deal with time dependence *)
SetAttributes [m1, Constant]
SetAttributes [m2, Constant]
SetAttributes [l1, Constant]
SetAttributes [l2, Constant]
SetAttributes [l1cm, Constant]
SetAttributes [l2cm, Constant]
SetAttributes [I1, Constant]
SetAttributes [I2, Constant]
a1/: Dt[a1, t] = a1d
a2/: Dt[a2, t] = a2d
a1d/: Dt[a1d, t] = a1dd
a2d/: Dt[a2d, t] = a2dd

(*Define link lengths.*)

s1 = {0,-l1}
s2 = {0,-l2}

(*Define the location of the proximal end of each link (in world coordinates) *)

p1 = {0, 0}
p2 = r01 . s1 

(*Define the location of the center of mass with respect
 to the proximal end of each link.*)

cm1 = r01 . {0,-l1cm}
cm2 = r01 . r12 . {0,-l2cm}

(*Compute the location of the center of mass of each link.*)

q1 = p1 + cm1
q2 = p2 + cm2

(*Compute the linear velocity and acceleration of each link cm.*)

qd1 = Dt[q1,t]
qd2 = Dt[q2,t]

qdd1 = Dt[qd1,t]
qdd2 = Dt[qd2,t]

(*Compute the angular velocity of each link.*)

w1 = Dt[a1,t]
w2 = w1 + Dt[a2,t]

(*Compute the angular acceleration of each link.*)

wd1 = Dt[w1,t]
wd2 = Dt[w2,t]

(*Kinetic energy *)

KE = 0.5*(w1*I1*w1) + 0.5*m1*qd1.qd1 + 0.5*(w2*I2*w2) + 0.5*m2*qd2.qd2

(* Potential energy *)

PE = m1*G*q1[[2]] + m2*G*q2[[2]]

L = Expand[ KE - PE ]

eqa1 = Expand[Dt[D[L,a1d],t] - D[L,a1]]
eqa2 = Expand[Dt[D[L,a2d],t] - D[L,a2]]

tau1 = Simplify[eqa1] /. 1.->1
tau2 = Simplify[eqa2] /. 1.->1

tau1 = Simplify[tau1] /. 0.->0
tau2 = Simplify[tau2] /. 0.->0

tau1 = Simplify[tau1] /. 2.->2
tau2 = Simplify[tau2] /. 2.->2

tau1 = Simplify[tau1] /. 1.->1
tau2 = Simplify[tau2] /. 1.->1

tau1 = Simplify[Expand[Simplify[Expand[tau1, Trig->True]]]]
tau2 = Simplify[Expand[Simplify[Expand[tau2, Trig->True]]]]

tau1 = Simplify[tau1] /. 0.->0
tau2 = Simplify[tau2] /. 0.->0

tau1 = Simplify[tau1] /. 1.->1
tau2 = Simplify[tau2] /. 1.->1

tau1 = Expand[tau1]
tau2 = Expand[tau2]

(* To get C code use *)

CForm[tau1]
   a1dd*I1 + a1dd*I2 + a2dd*I2 + a1dd*Power(l1cm,2)*m1 + 
    a1dd*Power(l1,2)*m2 + a1dd*Power(l2cm,2)*m2 + a2dd*Power(l2cm,2)*m2 + 
    2*a1dd*l1*l2cm*m2*Cos(a2) + a2dd*l1*l2cm*m2*Cos(a2) + G*l1cm*m1*Sin(a1) + 
    G*l1*m2*Sin(a1) + G*l2cm*m2*Cos(a2)*Sin(a1) - 
    2.*a1d*a2d*l1*l2cm*m2*Sin(a2) - 1.*Power(a2d,2)*l1*l2cm*m2*Sin(a2) + 
    G*l2cm*m2*Cos(a1)*Sin(a2)

CForm[tau2]
   a1dd*I2 + a2dd*I2 + a1dd*Power(l2cm,2)*m2 + a2dd*Power(l2cm,2)*m2 + 
    a1dd*l1*l2cm*m2*Cos(a2) + Power(a1d,2)*l1*l2cm*m2*Sin(a2) + 
    G*l2cm*m2*Sin(a1 + a2)