(* zero position has robot along X axis *)

(* Inverse dynamics for a three 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]

(*Deal with time dependence *)
SetAttributes [m1, Constant]
SetAttributes [l1, Constant]
SetAttributes [l1cm, Constant]
SetAttributes [I1, Constant]
a1/: Dt[a1, t] = a1d
a1d/: Dt[a1d, t] = a1dd

(*Define link lengths.*)

s1 = {0,-l1}

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

p1 = {0, 0}

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

cm1 = r01 . {0,-l1cm}

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

q1 = p1 + cm1

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

qd1 = Dt[q1,t]

qdd1 = Dt[qd1,t]

(*Compute net link forces.*)

f1 = m1 * qdd1 - m1 * gravity

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

w1 = Dt[a1,t]

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

wd1 = Dt[w1,t]

(*Compute net link torques.*)

n1 = I1*wd1

(*Compute forces at the joints.*)

f01 = f1;

(*Define the 2D vector cross product.*)

CP[X_,Y_] := X[[1]]*Y[[2]] - X[[2]]*Y[[1]]

(*Compute torques at the joints.*)

tau1 = n1 + CP[ cm1, f1 ]

tau1 = Expand[tau1, Trig->True]

(* To get C code use *)
CForm[tau1]
(*
a1dd*I1 + a1dd*Power(l1cm,2)*m1 + G*l1cm*m1*Sin(a1)
*)