(* Inverse dynamics for a planar hand held load using dynamics for identification*)

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

(* Deal with time dependence *)
(* mass *)
SetAttributes [m, Constant]
(* mass*location-of-center-of-mass-x *)
SetAttributes [mcx, Constant]
(* mass*location-of-center-of-mass-y *)
SetAttributes [mcy, Constant]
(* moment of inertia about force/torque sensing point *)
SetAttributes [Ij, Constant]
a/: Dt[a, t] = ad
ad/: Dt[ad, t] = add
x/: Dt[x, t] = xd
xd/: Dt[xd, t] = xdd
y/: Dt[y, t] = yd
yd/: Dt[yd, t] = ydd

(*Define the location of the force torque sensing system (in world coordinates) *)

p = {x, y}
pd = Dt[p,t]
pdd = Dt[pd,t]

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

cm = r01 . {mcx/m,mcy/m}
(*
          mcx Cos[a]   mcy Sin[a]  mcy Cos[a]   mcx Sin[a]
Out[24]= {---------- - ----------, ---------- + ----------}
              m            m           m            m
*)

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

q = p + cm
(*
              mcx Cos[a]   mcy Sin[a]      mcy Cos[a]   mcx Sin[a]
Out[25]= {x + ---------- - ----------, y + ---------- + ----------}
                  m            m               m            m
*)

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

qd = Dt[q,t]
(*
               ad mcy Cos[a]   ad mcx Sin[a]
Out[26]= {xd - ------------- - -------------, 
                     m               m
 
          ad mcx Cos[a]   ad mcy Sin[a]
>    yd + ------------- - -------------}
                m               m
*)

qdd = Dt[qd,t]
(*
                  2
                ad  mcx Cos[a]   add mcy Cos[a]   add mcx Sin[a]
Out[27]= {xdd - -------------- - -------------- - -------------- + 
                      m                m                m
 
        2                                      2
      ad  mcy Sin[a]        add mcx Cos[a]   ad  mcy Cos[a]
>     --------------, ydd + -------------- - -------------- - 
            m                     m                m
 
        2
      ad  mcx Sin[a]   add mcy Sin[a]
>     -------------- - --------------}
            m                m
*)
                                 
(*Compute net link forces.*)

f = Expand[ m * qdd - m * gravity ]
(*
                    2
Out[29]= {m xdd - ad  mcx Cos[a] - add mcy Cos[a] - add mcx Sin[a] + 
 
        2                                              2
>     ad  mcy Sin[a], G m + m ydd + add mcx Cos[a] - ad  mcy Cos[a] - 
 
        2
>     ad  mcx Sin[a] - add mcy Sin[a]}
*)

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

w = Dt[a,t]
(*
ad
*)

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

wd = Dt[w,t]
(*
add
*)

(*Compute net link torques.*)

n = Ij*wd
(*
add Ij
*)

(*Compute forces at the joints.*)

force = f
(*
                    2
Out[33]= {m xdd - ad  mcx Cos[a] - add mcy Cos[a] - add mcx Sin[a] + 
 
        2                                              2
>     ad  mcy Sin[a], G m + m ydd + add mcx Cos[a] - ad  mcy Cos[a] - 
 
        2
>     ad  mcx Sin[a] - add mcy Sin[a]}
*)

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

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

(*Compute torques at the joints.*)

torque = Expand[ n + m*CP [ cm, pdd ] - m*CP[ cm, gravity ] ]
(*
Out[37]= Ij add + G mcx Cos[a] - mcy xdd Cos[a] + mcx ydd Cos[a] - 
 
>    G mcy Sin[a] - mcx xdd Sin[a] - mcy ydd Sin[a]
*)

(*Clean it up*)

force = Expand[force, Trig->True]
(*
                    2
Out[38]= {m xdd - ad  mcx Cos[a] - add mcy Cos[a] - add mcx Sin[a] + 
 
        2                                              2
>     ad  mcy Sin[a], G m + m ydd + add mcx Cos[a] - ad  mcy Cos[a] - 
 
        2
>     ad  mcx Sin[a] - add mcy Sin[a]}
*)

torque = Expand[torque, Trig->True]
(*
Out[39]= Ij add + G mcx Cos[a] - mcy xdd Cos[a] + mcx ydd Cos[a] - 
 
>    G mcy Sin[a] - mcx xdd Sin[a] - mcy ydd Sin[a]
*)

Coefficient[force,m] 
(*
Out[47]= {xdd, G + ydd}
*)
Coefficient[force,mcx] 
(*
              2                                      2
Out[48]= {-(ad  Cos[a]) - add Sin[a], add Cos[a] - ad  Sin[a]}
*)
Coefficient[force,mcy] 
(*
                            2             2
Out[49]= {-(add Cos[a]) + ad  Sin[a], -(ad  Cos[a]) - add Sin[a]}
*)
Coefficient[force,Ij] 
(*
Out[56]= {0, 0}
*)
Coefficient[torque,m] 
(*
Out[57]= 0
*)
Coefficient[torque,mcx] 
(*
Out[58]= G Cos[a] + ydd Cos[a] - xdd Sin[a]
*)
Coefficient[torque,mcy] 
(*
Out[59]= -(xdd Cos[a]) - G Sin[a] - ydd Sin[a]
*)
Coefficient[torque,Ij] 
(*
Out[61]= add
*)