(* Inverse dynamics for a two joint mechanism using Lagrangian approach *)
(* Moment of inertia about center of mass *)

(* To read in a file of input: << file *)
(* Quit[] or Quit to quit *)

(* the starting position is hanging straight down with z up *)

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

(* the starting position is hanging straight down with z down, x to the side,
and y coming out towards the viewer. *)
(*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 time dependence *)
SetAttributes [m1, Constant]
SetAttributes [m2, Constant]
SetAttributes [l1, Constant]
SetAttributes [l2, Constant]
SetAttributes [l1x, Constant]
SetAttributes [l1y, Constant]
SetAttributes [l1z, Constant]
SetAttributes [l2x, Constant]
SetAttributes [l2y, Constant]
SetAttributes [l2z, Constant]
SetAttributes [I1xx, Constant]
SetAttributes [I1xy, Constant]
SetAttributes [I1xz, Constant]
SetAttributes [I1yy, Constant]
SetAttributes [I1yz, Constant]
SetAttributes [I1zz, Constant]
SetAttributes [I2xx, Constant]
SetAttributes [I2xy, Constant]
SetAttributes [I2xz, Constant]
SetAttributes [I2yy, Constant]
SetAttributes [I2yz, Constant]
SetAttributes [I2zz, 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 . {l1x, l1y, l1z}
cm2 = r01 . r12 . {l2x, l2y, l2z}

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

(* Moment of inertia tensors. We will express them in link coordinates about
the center of mass *)

I1 = {{I1xx, I1xy, I1xz}, {I1xy, I1yy, I1yz}, {I1xz, I1yz, I1zz}}
I2 = {{I2xx, I2xy, I2xz}, {I2xy, I2yy, I2yz}, {I2xz, I2yz, I2zz}}

(*Kinetic energy: m1 and m2 are masses *)

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[[3]] + -m2*G*q2[[3]]

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

(*
In[1]:= << 2-joint-arm-inverse-dynamics-lagrange.m

*)

(* To get C code use
In[4]:= CForm[tau1]
In[5]:= CForm[tau2]
*)