(* Inverse dynamics for a two joint mechanism *)
(* the starting position is hanging straight down with z up *)
gravity = {0, 0, -g} 
zerov = {0, 0, 0} 

(*Define the vector cross product.*)

CP[X_,Y_] := {X[[2]]*Y[[3]] - X[[3]]*Y[[2]],
            -X[[1]]*Y[[3]] + X[[3]]*Y[[1]],
            X[[1]]*Y[[2]] - X[[2]]*Y[[1]]}

(*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],0,Sin[a1]},{0,1,0},{-Sin[a1],0, Cos[a1]}}
r10=Transpose[r01]

(*Deal with the derivative nonsense *)
SetAttributes [l1cm, Constant]
SetAttributes [l1, Constant]
a1/: Dt[a1, t] = a1d
a1d/: Dt[a1d, t] = a1dd

(*Define link lengths.*)

s1 = {0, 0, -l1}

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

p1 = {0, 0, 0}

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

cm1 = r01 . {0, 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 the angular velocity of each link.*)

w1 = {0,Dt[a1,t],0}

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

wd1 = Dt[w1,t]

(*Compute net link forces.*)

f1 = m1 * qdd1

(*Define link inertia tensors (about the com).*)

i1 = {{ixx1,ixy1,ixz1},{ixy1,iyy1,iyz1},{ixz1,iyz1,izz1}}

(*Compute net link torques.*)

n1 = i1 . wd1

(*Compute forces at the joints.*)

f01 = f1 - m1 * gravity

(*Compute torques at the joints.*)

n01 = n1 + CP [ cm1, f01 ]

(*Pull out just joint torques.*)

t1 = Expand[n01[[2]], Trig->True]

CForm[t1]

