(* Inverse dynamics for a free-flying torso using Newton Euler approach *)
(* Moment of inertia about center of mass of each link *)
(* zero position has torso vertical along Y axis, with
   torso center of mass at world origin *)

(* Y vertical X to right coordinate system. *)
gravity = {0, -G} 
(*Define rotation matrices that allow us to do the forward kinematics.*)
(*R01 rotates from the link 1 (torso) frame to the link 0 (world) frame.*)

r01={{Cos[a0],-Sin[a0]},{Sin[a0],Cos[a0]}}
r10=Transpose[r01]

(*Deal with time dependence *)
SetAttributes [m1, Constant]
(* l1 is zero *)
(* l1cm is zero *)
SetAttributes [I1, Constant]
(* location of torso cm is x, y with orientation a0 *)
x/: Dt[x, t] = xd
xd/: Dt[xd, t] = xdd
y/: Dt[y, t] = yd
yd/: Dt[yd, t] = ydd
a0/: Dt[a0, t] = a0d
a0d/: Dt[a0d, t] = a0dd

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

p1 = {x, y}

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

cm1 = {0, 0}

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

q1 = p1 + cm1

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

qd1 = Dt[q1,t]

qdd1 = Dt[qd1,t]

(*Compute net link forces.*)

f1 = m1 * qdd1 - m1 * gravity

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

w1 = Dt[a0,t]

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

wd1 = Dt[w1,t]

(*Compute net link torques.*)

n1 = I1*wd1

(*Compute forces at the joints.*)

f01 = f1

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

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

(*Compute torques at the joints.*)

tau0 = n1 
fx = f01[[1]]
fy = f01[[2]]

**************************************************************

In[41]:= tau0 = n1

Out[41]= a0dd I1

In[42]:= fx = f01[[1]]

Out[42]= m1 xdd

In[43]:= fy = f01[[2]]

Out[43]= G m1 + m1 ydd