(* Inverse dynamics for an acrobot 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]
r12={{Cos[a2],0,Sin[a2]},{0,1,0},{-Sin[a2],0, Cos[a2]}}
r21=Transpose[r12]

(*Deal with the derivative nonsense *)
SetAttributes [l1cm, Constant]
SetAttributes [l2cm, Constant]
SetAttributes [l1, Constant]
SetAttributes [l2, 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, 0, -l1}
s2 = {0, 0, -l2}

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

p1 = {0, 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, 0, l1cm}
cm2 = r01 . r12 . {0, 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 = {0,Dt[a1,t],0}
w2 = w1 + {0,Dt[a2,t],0}

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

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

(*Compute net link forces.*)

f1 = m1 * qdd1
f2 = m2 * qdd2

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

i1 = {{ixx1,ixy1,ixz1},{ixy1,iyy1,iyz1},{ixz1,iyz1,izz1}}
i2 = {{ixx2,ixy2,ixz2},{ixy2,iyy2,iyz2},{ixz2,iyz2,izz2}}

(*Compute net link torques.*)

n1 = i1 . wd1
n2 = i2 . wd2

(*Compute forces at the joints.*)

f12 = f2 - m2 * gravity
f01 = f1 + f12 - m1 * gravity

(*Compute torques at the joints.*)

n12 = n2 + CP [ cm2, f12 ] + motortorque
n01 = n1 + n12 + CP [ cm1, f01 ] - CP[ q1-p2, f12 ]

(*Pull out just joint torques.*)

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

CForm[t1]
CForm[t2]

