(* 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 *)
gravity = {0, 0, G} 

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

(*Compute the linear velocity and acceleration of each joint location.*)

pd1 = Dt[p1,t]
pd2 = Dt[p2,t]
pdd1 = Dt[pd1,t]
pdd2 = Dt[pd2,t]

(*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 proximal joint *)

(* Test case expressing I about CM
I1 = {{I1xx, I1xy, I1xz}, {I1xy, I1yy + m1*l1x*l1x + m1*l1z*l1z, I1yz},
 {I1xz, I1yz, I1zz}}
I2 = {{I2xx, I2xy, I2xz}, {I2xy, I2yy + m2*l2x*l2x + m2*l2z*l2z, I2yz},
 {I2xz, I2yz, I2zz}}
*)
(* Expressing I about joint *)
I1 = {{I1xx, I1xy, I1xz}, {I1xy, I1yy, I1yz}, {I1xz, I1yz, I1zz}}
I2 = {{I2xx, I2xy, I2xz}, {I2xy, I2yy, I2yz}, {I2xz, I2yz, I2zz}}

(* ALL VECTORS EXPRESSED IN ABSOLUTE COORDINATES *)

(*Compute net link forces.*)

(*
f1 = m1 * qdd1 + m1*gravity
f2 = m2 * qdd2 + m2*gravity
*)
f1 = m1 * pdd1 + m1*gravity + m1*CP[wd1,cm1] + m1*CP[w1,CP[w1,cm1]]
f2 = m2 * pdd2 + m2*gravity + m2*CP[wd2,cm2] + m2*CP[w2,CP[w2,cm2]]

(*Compute net link torques.*)

n1 = I1 . wd1 + CP[w1,I1.w1] + m1*CP[cm1,pdd1] + m1*CP[cm1,gravity]
n2 = I2 . wd2 + CP[w2,I2.w2] + m2*CP[cm2,pdd2] + m2*CP[cm2,gravity]

(*Compute forces at the joints.*)

f12 = f2
f01 = f1 + f12

(*Compute torques at the joints.*)

n12 = n2
n01 = n1 + n12 + CP[ p2-p1, f12 ]

(*Pull out just joint torques.*)

tau1 = Expand[n01[[2]], Trig->True]
tau2 = Expand[n12[[2]], Trig->True]

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

In[2]:= tau1

                                                   2
Out[2]= a1dd I1yy + a1dd I2yy + a2dd I2yy + a1dd l1  m2 - G l1x m1 Cos[a1] + 
 
                                      2
>    2 a1d a2d l1 l2x m2 Cos[a2] + a2d  l1 l2x m2 Cos[a2] - 
 
>    2 a1dd l1 l2z m2 Cos[a2] - a2dd l1 l2z m2 Cos[a2] - 
 
>    G l2x m2 Cos[a1 + a2] - G l1z m1 Sin[a1] + G l1 m2 Sin[a1] + 
 
>    2 a1dd l1 l2x m2 Sin[a2] + a2dd l1 l2x m2 Sin[a2] + 
 
                                      2
>    2 a1d a2d l1 l2z m2 Sin[a2] + a2d  l1 l2z m2 Sin[a2] - 
 
>    G l2z m2 Sin[a1 + a2]

In[3]:= tau2

                                   2
Out[3]= a1dd I2yy + a2dd I2yy - a1d  l1 l2x m2 Cos[a2] - 
 
>    a1dd l1 l2z m2 Cos[a2] - G l2x m2 Cos[a1 + a2] + 
 
                                 2
>    a1dd l1 l2x m2 Sin[a2] - a1d  l1 l2z m2 Sin[a2] - G l2z m2 Sin[a1 + a2]

In[4]:= Simplify[tau1] /. {l1x->0, l2x->0}

                                                   2
Out[4]= a1dd I1yy + a1dd I2yy + a2dd I2yy + a1dd l1  m2 - 
 
>    2 a1dd l1 l2z m2 Cos[a2] - a2dd l1 l2z m2 Cos[a2] - G l1z m1 Sin[a1] + 
 
                                                        2
>    G l1 m2 Sin[a1] + 2 a1d a2d l1 l2z m2 Sin[a2] + a2d  l1 l2z m2 Sin[a2] - 
 
>    G l2z m2 Sin[a1 + a2]

In[5]:= Simplify[tau2] /. {l1x->0, l2x->0}

Out[5]= a1dd I2yy + a2dd I2yy - a1dd l1 l2z m2 Cos[a2] - 
 
        2
>    a1d  l1 l2z m2 Sin[a2] - G l2z m2 Sin[a1 + a2]


*)

(* To get C code use
*)