(* Inverse dynamics for a two joint mechanism using Newton Euler approach *)
(* Moment of inertia about center of mass of each link *)

(* 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 net link forces.*)

f1 = m1 * qdd1 - m1 * gravity
f2 = m2 * qdd2 - m2 * gravity

(*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]

(*Compute net link torques.*)

n1 = I1*wd1
n2 = I2*wd2

(*Compute forces at the joints.*)

f12 = f2
f01 = f1 + f12

(*Define the 2D vector cross product.*)
CP[X_,Y_] := X[[1]]*Y[[2]] - X[[2]]*Y[[1]]

(*Compute torques at the joints.*)

tau2 = n2 + CP [ cm2, f2 ]
tau1 = n1 + tau2 + CP [ cm1, f1 ] + CP[ p2-p1, f12 ]

(*Clean it up*)

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

(* 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) - 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) + G*l2cm*m2*Cos(a2)*Sin(a1) + 
    Power(a1d,2)*l1*l2cm*m2*Sin(a2) + G*l2cm*m2*Cos(a1)*Sin(a2)