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

(* 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 *)
SetAttributes [m, Constant]
SetAttributes [cmx, Constant]
SetAttributes [cmy, Constant]
(* About center of mass *)
SetAttributes [Icm, 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 proximal end of each link (in world coordinates) *)

p = {x, y}

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

cm = r01 . {cmx,cmy}
(*
{cmx Cos[a] - cmy Sin[a], cmy Cos[a] + cmx Sin[a]}
*)

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

q = p + cm
(*
{x + cmx Cos[a] - cmy Sin[a], y + cmy Cos[a] + cmx Sin[a]}
*)

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

qd = Dt[q,t]
(*
{xd - ad cmy Cos[a] - ad cmx Sin[a], 
>    yd + ad cmx Cos[a] - ad cmy Sin[a]}
*)

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

f = m * qdd - m * gravity
(*
            2
{m (xdd - ad  cmx Cos[a] - add cmy Cos[a] - add cmx Sin[a] + 
          2
>       ad  cmy Sin[a]), G m + m 
                                 2                2
>      (ydd + add cmx Cos[a] - ad  cmy Cos[a] - ad  cmx Sin[a] - 
>        add cmy 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 = I*wd
(*
add I
*)

(*Compute forces at the joints.*)

force = f
(*
            2
{m (xdd - ad  cmx Cos[a] - add cmy Cos[a] - add cmx Sin[a] + 
          2
>       ad  cmy Sin[a]), G m + m 
                                 2                2
>      (ydd + add cmx Cos[a] - ad  cmy Cos[a] - ad  cmx Sin[a] - 
>        add cmy 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 = n + CP [ cm, f ]
(*
Out[32]= I add - m (cmy Cos[a] + cmx Sin[a]) 
               2
>     (xdd - ad  cmx Cos[a] - add cmy Cos[a] - add cmx Sin[a] + 
          2
>       ad  cmy Sin[a]) + (cmx Cos[a] - cmy Sin[a]) 
                                         2                2
>     (G m + m (ydd + add cmx Cos[a] - ad  cmy Cos[a] - ad  cmx Sin[a] - 
>          add cmy Sin[a]))
*)

(*Clean it up*)

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

torque = Expand[torque, Trig->True]
(*
               2            2
I add + add cmx  m + add cmy  m + cmx G m Cos[a] - 
 
>    cmy m xdd Cos[a] + cmx m ydd Cos[a] - cmy G m Sin[a] - 
 
>    cmx m xdd Sin[a] - cmy m ydd Sin[a]
*)