(* Inverse dynamics for a two joint mechanism using dynamics for identification *)
(* Moment of inertia about joint 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]
r12={{Cos[a2],-Sin[a2]},{Sin[a2],Cos[a2]}}
r21=Transpose[r12]

(*Deal with time dependence *)
SetAttributes [m1, Constant]
SetAttributes [m2, Constant]
SetAttributes [l1, Constant]
SetAttributes [l2, Constant]
SetAttributes [mcx1, Constant]
SetAttributes [mcy1, Constant]
SetAttributes [mcx2, Constant]
SetAttributes [mcy2, Constant]
SetAttributes [Ij1, Constant]
SetAttributes [Ij2, 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 = {l1,0}
s2 = {l2,0}

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

p1 = {0, 0}
p2 = r01 . s1 
(* {l1 Cos[a1], l1 Sin[a1]} *)
pd1 = Dt[p1,t]
pdd1 = Dt[pd1,t]
pd2 = Dt[p2,t]
pdd2 = Dt[pd2,t]

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

c1 = r01 . {mcx1,mcy1}/m1
(*
          mcx1 Cos[a1] - mcy1 Sin[a1]  mcy1 Cos[a1] + mcx1 Sin[a1]
Out[38]= {---------------------------, ---------------------------}
                      m1                           m1
*)

c2 = r01 . r12 . {mcx2,mcy2}/m2
(* 
Out[39]= {(mcy2 (-(Cos[a2] Sin[a1]) - Cos[a1] Sin[a2]) + 
 
>       mcx2 (Cos[a1] Cos[a2] - Sin[a1] Sin[a2])) / m2, 
 
>    (mcx2 (Cos[a2] Sin[a1] + Cos[a1] Sin[a2]) + 
 
>       mcy2 (Cos[a1] Cos[a2] - Sin[a1] Sin[a2])) / m2}
*)

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

q1 = p1 + c1
(* 
          mcx1 Cos[a1] - mcy1 Sin[a1]  mcy1 Cos[a1] + mcx1 Sin[a1]
Out[40]= {---------------------------, ---------------------------}
                      m1                           m1
*)

q2 = p2 + c2
(* 
Out[41]= {l1 Cos[a1] + (mcy2 (-(Cos[a2] Sin[a1]) - Cos[a1] Sin[a2]) + 
 
>        mcx2 (Cos[a1] Cos[a2] - Sin[a1] Sin[a2])) / m2, 
 
>    l1 Sin[a1] + (mcx2 (Cos[a2] Sin[a1] + Cos[a1] Sin[a2]) + 
 
>        mcy2 (Cos[a1] Cos[a2] - Sin[a1] Sin[a2])) / m2}
*)

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

qd1 = Dt[q1,t]
(*
          -(a1d mcy1 Cos[a1]) - a1d mcx1 Sin[a1]
Out[42]= {--------------------------------------, 
                            m1
 
     a1d mcx1 Cos[a1] - a1d mcy1 Sin[a1]
>    -----------------------------------}
                     m1
*)

qd2 = Dt[q2,t]
(*
Out[43]= {-(a1d l1 Sin[a1]) + (mcx2 
 
>         (-(a1d Cos[a2] Sin[a1]) - a2d Cos[a2] Sin[a1] - 
 
>           a1d Cos[a1] Sin[a2] - a2d Cos[a1] Sin[a2]) + 
 
>        mcy2 (-(a1d Cos[a1] Cos[a2]) - a2d Cos[a1] Cos[a2] + 
 
>           a1d Sin[a1] Sin[a2] + a2d Sin[a1] Sin[a2])) / m2, 
 
>    a1d l1 Cos[a1] + (mcy2 (-(a1d Cos[a2] Sin[a1]) - a2d Cos[a2] Sin[a1] - 
 
>           a1d Cos[a1] Sin[a2] - a2d Cos[a1] Sin[a2]) + 
 
>        mcx2 (a1d Cos[a1] Cos[a2] + a2d Cos[a1] Cos[a2] - 
 
>           a1d Sin[a1] Sin[a2] - a2d Sin[a1] Sin[a2])) / m2}
*)

qdd1 = Dt[qd1,t]
(*
                2
Out[44]= {(-(a1d  mcx1 Cos[a1]) - a1dd mcy1 Cos[a1] - a1dd mcx1 Sin[a1] + 
 
           2
>       a1d  mcy1 Sin[a1]) / m1, 
 
                             2                   2
>    (a1dd mcx1 Cos[a1] - a1d  mcy1 Cos[a1] - a1d  mcx1 Sin[a1] - 
 
>       a1dd mcy1 Sin[a1]) / m1}
*)

qdd2 = Dt[qd2,t]
(*
               2
Out[45]= {-(a1d  l1 Cos[a1]) - a1dd l1 Sin[a1] + 
 
                  2
>     (mcx2 (-(a1d  Cos[a1] Cos[a2]) - 2 a1d a2d Cos[a1] Cos[a2] - 
 
               2
>           a2d  Cos[a1] Cos[a2] - a1dd Cos[a2] Sin[a1] - 
 
>           a2dd Cos[a2] Sin[a1] - a1dd Cos[a1] Sin[a2] - 
 
                                      2
>           a2dd Cos[a1] Sin[a2] + a1d  Sin[a1] Sin[a2] + 
 
                                           2
>           2 a1d a2d Sin[a1] Sin[a2] + a2d  Sin[a1] Sin[a2]) + 
 
>        mcy2 (-(a1dd Cos[a1] Cos[a2]) - a2dd Cos[a1] Cos[a2] + 
 
               2
>           a1d  Cos[a2] Sin[a1] + 2 a1d a2d Cos[a2] Sin[a1] + 
 
               2                      2
>           a2d  Cos[a2] Sin[a1] + a1d  Cos[a1] Sin[a2] + 
 
                                           2
>           2 a1d a2d Cos[a1] Sin[a2] + a2d  Cos[a1] Sin[a2] + 
 
>           a1dd Sin[a1] Sin[a2] + a2dd Sin[a1] Sin[a2])) / m2, 
 
                          2
>    a1dd l1 Cos[a1] - a1d  l1 Sin[a1] + 
 
                  2
>     (mcy2 (-(a1d  Cos[a1] Cos[a2]) - 2 a1d a2d Cos[a1] Cos[a2] - 
 
               2
>           a2d  Cos[a1] Cos[a2] - a1dd Cos[a2] Sin[a1] - 
 
>           a2dd Cos[a2] Sin[a1] - a1dd Cos[a1] Sin[a2] - 
 
                                      2
>           a2dd Cos[a1] Sin[a2] + a1d  Sin[a1] Sin[a2] + 
 
                                           2
>           2 a1d a2d Sin[a1] Sin[a2] + a2d  Sin[a1] Sin[a2]) + 
 
>        mcx2 (a1dd Cos[a1] Cos[a2] + a2dd Cos[a1] Cos[a2] - 
 
               2
>           a1d  Cos[a2] Sin[a1] - 2 a1d a2d Cos[a2] Sin[a1] - 
 
               2                      2
>           a2d  Cos[a2] Sin[a1] - a1d  Cos[a1] Sin[a2] - 
 
                                           2
>           2 a1d a2d Cos[a1] Sin[a2] - a2d  Cos[a1] Sin[a2] - 
 
>           a1dd Sin[a1] Sin[a2] - a2dd Sin[a1] Sin[a2])) / m2}
*)

(*Compute net link forces.*)

f1 = m1 * qdd1 - m1 * gravity
(*
               2
Out[46]= {-(a1d  mcx1 Cos[a1]) - a1dd mcy1 Cos[a1] - a1dd mcx1 Sin[a1] + 
 
         2                                             2
>     a1d  mcy1 Sin[a1], G m1 + a1dd mcx1 Cos[a1] - a1d  mcy1 Cos[a1] - 
 
         2
>     a1d  mcx1 Sin[a1] - a1dd mcy1 Sin[a1]}
*)

f2 = m2 * qdd2 - m2 * gravity
(*
                   2
Out[47]= {m2 (-(a1d  l1 Cos[a1]) - a1dd l1 Sin[a1] + 
 
                    2
>       (mcx2 (-(a1d  Cos[a1] Cos[a2]) - 2 a1d a2d Cos[a1] Cos[a2] - 
 
                 2
>             a2d  Cos[a1] Cos[a2] - a1dd Cos[a2] Sin[a1] - 
 
>             a2dd Cos[a2] Sin[a1] - a1dd Cos[a1] Sin[a2] - 
 
                                        2
>             a2dd Cos[a1] Sin[a2] + a1d  Sin[a1] Sin[a2] + 
 
                                             2
>             2 a1d a2d Sin[a1] Sin[a2] + a2d  Sin[a1] Sin[a2]) + 
 
>          mcy2 (-(a1dd Cos[a1] Cos[a2]) - a2dd Cos[a1] Cos[a2] + 
 
                 2
>             a1d  Cos[a2] Sin[a1] + 2 a1d a2d Cos[a2] Sin[a1] + 
 
                 2                      2
>             a2d  Cos[a2] Sin[a1] + a1d  Cos[a1] Sin[a2] + 
 
                                             2
>             2 a1d a2d Cos[a1] Sin[a2] + a2d  Cos[a1] Sin[a2] + 
 
>             a1dd Sin[a1] Sin[a2] + a2dd Sin[a1] Sin[a2])) / m2), 
 
                                     2
>    G m2 + m2 (a1dd l1 Cos[a1] - a1d  l1 Sin[a1] + 
 
                     2
>        (mcy2 (-(a1d  Cos[a1] Cos[a2]) - 2 a1d a2d Cos[a1] Cos[a2] - 
 
                  2
>              a2d  Cos[a1] Cos[a2] - a1dd Cos[a2] Sin[a1] - 
 
>              a2dd Cos[a2] Sin[a1] - a1dd Cos[a1] Sin[a2] - 
 
                                         2
>              a2dd Cos[a1] Sin[a2] + a1d  Sin[a1] Sin[a2] + 
 
                                              2
>              2 a1d a2d Sin[a1] Sin[a2] + a2d  Sin[a1] Sin[a2]) + 
 
>           mcx2 (a1dd Cos[a1] Cos[a2] + a2dd Cos[a1] Cos[a2] - 
 
                  2
>              a1d  Cos[a2] Sin[a1] - 2 a1d a2d Cos[a2] Sin[a1] - 
 
                  2                      2
>              a2d  Cos[a2] Sin[a1] - a1d  Cos[a1] Sin[a2] - 
 
                                              2
>              2 a1d a2d Cos[a1] Sin[a2] - a2d  Cos[a1] Sin[a2] - 
 
>              a1dd Sin[a1] Sin[a2] - a2dd Sin[a1] Sin[a2])) / m2)}
*)

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

w1 = Dt[a1,t]
(* a1d *)
w2 = w1 + Dt[a2,t]
(* a1d + a2d *)

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

wd1 = Dt[w1,t]
(* a1dd *)
wd2 = Dt[w2,t]
(* a1dd + a2dd *)

(*Compute net link torques.*)

n1 = Ij1*wd1
(* a1dd Ij1 *)

n2 = Ij2*wd2
(* (a1dd + a2dd) Ij2 *)

(*Compute forces at the joints.*)

f12 = f2
(* Huge mess *)

f01 = f1 + f12
(* Huge mess *)

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

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

(*Compute torques at the joints.*)

tau2 = n2 + m2*CP [ c2, pdd2 ] - m2*CP[ c2, gravity ]
(* Huge mess *)

tau1 = n1 + tau2 + m1*CP [ c1, pdd1 ] - m1*CP[ c1, gravity ] + CP[ p2-p1, f12 ]
(* Huge mess *)

(*Clean it up*)

tau1 = Expand[tau1, Trig->True]
(*
                                                 2
Out[64]= a1dd Ij1 + a1dd Ij2 + a2dd Ij2 + a1dd l1  m2 + G l1 m2 Cos[a1] + 
 
>    G mcx1 Cos[a1] + 2 a1dd l1 mcx2 Cos[a2] + a2dd l1 mcx2 Cos[a2] - 
 
                                    2
>    2 a1d a2d l1 mcy2 Cos[a2] - a2d  l1 mcy2 Cos[a2] + 
 
>    G mcx2 Cos[a1] Cos[a2] - G mcy1 Sin[a1] - G mcy2 Cos[a2] Sin[a1] - 
 
                                    2
>    2 a1d a2d l1 mcx2 Sin[a2] - a2d  l1 mcx2 Sin[a2] - 
 
>    2 a1dd l1 mcy2 Sin[a2] - a2dd l1 mcy2 Sin[a2] - G mcy2 Cos[a1] Sin[a2] - 
 
>    G mcx2 Sin[a1] Sin[a2]
*)

tau2 = Expand[tau2, Trig->True]
(*
                                                2
a1dd Ij2 + a2dd Ij2 + a1dd l1 mcx2 Cos[a2] + a1d  l1 mcy2 Cos[a2] + 
                                                          2
>    G mcx2 Cos[a1] Cos[a2] - G mcy2 Cos[a2] Sin[a1] + a1d  l1 mcx2 Sin[a2] - 
>    a1dd l1 mcy2 Sin[a2] - G mcy2 Cos[a1] Sin[a2] - G mcx2 Sin[a1] Sin[a2]
*)

Coefficient[tau1,m1] 
(*
Out[66]= 0
*)
Coefficient[tau1,mcx1] 
(*
Out[67]= G Cos[a1]
*)
Coefficient[tau1,mcy1] 
(*
Out[68]= -(G Sin[a1])
*)
Coefficient[tau1,Ij1] 
(*
Out[69]= a1dd
*)
Coefficient[tau1,m2] 
(*
                2
Out[70]= a1dd l1  + G l1 Cos[a1]
*)
Coefficient[tau1,mcx2] 
(*
Out[71]= 2 a1dd l1 Cos[a2] + a2dd l1 Cos[a2] + G Cos[a1] Cos[a2] - 
 
                               2
>    2 a1d a2d l1 Sin[a2] - a2d  l1 Sin[a2] - G Sin[a1] Sin[a2]
*)
Coefficient[tau1,mcy2] 
(*
                                    2
Out[73]= -2 a1d a2d l1 Cos[a2] - a2d  l1 Cos[a2] - G Cos[a2] Sin[a1] - 
 
>    2 a1dd l1 Sin[a2] - a2dd l1 Sin[a2] - G Cos[a1] Sin[a2]
*)
Coefficient[tau1,Ij2] 
(*
Out[75]= a1dd + a2dd
*)
Coefficient[tau2,m1] 
(*
Out[77]= 0
*)
Coefficient[tau2,mcx1] 
(*
Out[78]= 0
*)
Coefficient[tau2,mcy1] 
(*
Out[79]= 0
*)
Coefficient[tau2,Ij1] 
(*
Out[80]= 0
*)
Coefficient[tau2,m2] 
(*
Out[81]= 0
*)
Coefficient[tau2,mcx2] 
(*
                                                  2
Out[82]= a1dd l1 Cos[a2] + G Cos[a1] Cos[a2] + a1d  l1 Sin[a2] - 
 
>    G Sin[a1] Sin[a2]
*)
Coefficient[tau2,mcy2] 
(*
            2
Out[83]= a1d  l1 Cos[a2] - G Cos[a2] Sin[a1] - a1dd l1 Sin[a2] - 
 
>    G Cos[a1] Sin[a2]
*)
Coefficient[tau2,Ij2] 
(*
Out[84]= a1dd + a2dd
*)