(* Inverse dynamics for a torso plus a thigh 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 *)

(*Deal with time dependence *)
SetAttributes [Mtorso, Constant]
SetAttributes [Mthigh, Constant]
SetAttributes [TorsotoHip, Constant] (* should be negative *)
SetAttributes [HiptoThighCOM, Constant] (* should be negative *)
SetAttributes [Itorso, Constant]
SetAttributes [Ithigh, 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
(* joints *)
hipL/: Dt[hipL, t] = hipLd
hipLd/: Dt[hipLd, t] = hipLdd

(* 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]
rTorsoHipL={{Cos[hipL],-Sin[hipL]},{Sin[hipL],Cos[hipL]}}
rHipLTorso=Transpose[rTorsoHipL]

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

Ptorso = {x, y}
PthighL = Ptorso + r01 . {0, TorsotoHip}

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

CMtorso = {0, 0}
CMthighL = r01 . rTorsoHipL . {0, HiptoThighCOM}

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

Qtorso = Ptorso + CMtorso
QthighL = PthighL + CMthighL

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

Qtorsod = Dt[Qtorso,t]
QthighLd = Dt[QthighL,t]

Qtorsodd = Dt[Qtorsod,t]
QthighLdd = Dt[QthighLd,t]

(* Compute net link forces. *)

Ftorso = Mtorso * Qtorsodd - Mtorso * gravity
FthighL = Mthigh * QthighLdd - Mthigh * gravity

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

Wtorso = Dt[a0,t]
WthighL = Wtorso + Dt[hipL,t]

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

Wtorsod = Dt[Wtorso,t]
WthighLd = Dt[WthighL,t]

(*Compute net link torques.*)

Ntorso = Itorso*Wtorsod
NthighL = Ithigh*WthighLd

(*Compute forces at the joints.*)

FtorsoThighL = FthighL
FworldTorso = Ftorso + FtorsoThighL

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

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

(*Compute torques at the joints.*)

tauHipL = NthighL + CP[ CMthighL, FthighL ] 
tau0 = Ntorso + tauHipL + CP[ CMtorso, Ftorso ] + CP[ PthighL-Ptorso, FtorsoThighL ]
fx = FworldTorso[[1]]
fy = FworldTorso[[2]]

(**************************************************************)

tau0 = Simplify[Expand[Simplify[Expand[tau0, Trig->True]]]]
tauHipL = Simplify[Expand[Simplify[Expand[tauHipL, Trig->True]]]]
fx = Simplify[Expand[Simplify[Expand[fx, Trig->True]]]]
fy = Simplify[Expand[Simplify[Expand[fy, Trig->True]]]]

(**************************************************************)
(**************************************************************)

(* Compute terms of H in H*acceleration = stuff *)

a00 = Simplify[Expand[Simplify[Coefficient[fx,xdd]]]]
a01 = Simplify[Expand[Simplify[Coefficient[fx,ydd]]]]
a02 = Simplify[Expand[Simplify[Coefficient[fx,a0dd]]]]
a03 = Simplify[Expand[Simplify[Coefficient[fx,hipLdd]]]]

CForm[a00]
Mthigh + Mtorso

CForm[a01]
0

CForm[a02]
-(Mthigh*(TorsotoHip*Cos(a0) + HiptoThighCOM*Cos(a0 + hipL)))

CForm[a03]
-(HiptoThighCOM*Mthigh*Cos(a0 + hipL))

CForm[Simplify[Expand[Simplify[Expand[fx - xdd*a00 - ydd*a01 - a0dd*a02 - hipLdd*a03]]]]]
   Mthigh*(Power(a0d,2)*TorsotoHip*Sin(a0) + 
      Power(a0d + hipLd,2)*HiptoThighCOM*Sin(a0 + hipL))

(**************************************************************)

a10 = Simplify[Expand[Simplify[Coefficient[fy,xdd]]]]
a11 = Simplify[Expand[Simplify[Coefficient[fy,ydd]]]]
a12 = Simplify[Expand[Simplify[Coefficient[fy,a0dd]]]]
a13 = Simplify[Expand[Simplify[Coefficient[fy,hipLdd]]]]

CForm[a10]
0

CForm[a11]
Mthigh + Mtorso

CForm[a12]
-(Mthigh*(TorsotoHip*Sin(a0) + HiptoThighCOM*Sin(a0 + hipL)))

CForm[a13]
-(HiptoThighCOM*Mthigh*Sin(a0 + hipL))

CForm[Simplify[Expand[Simplify[Expand[fy - xdd*a10 - ydd*a11 - a0dd*a12 - hipLdd*a13]]]]]
   G*(Mthigh + Mtorso) - Power(a0d,2)*Mthigh*TorsotoHip*Cos(a0) - 
    Power(a0d + hipLd,2)*HiptoThighCOM*Mthigh*Cos(a0 + hipL)

(**************************************************************)

a20 = Simplify[Expand[Simplify[Coefficient[tau0,xdd]]]]
a21 = Simplify[Expand[Simplify[Coefficient[tau0,ydd]]]]
a22 = Simplify[Expand[Simplify[Coefficient[tau0,a0dd]]]]
a23 = Simplify[Expand[Simplify[Coefficient[tau0,hipLdd]]]]

CForm[a20]
-(Mthigh*(TorsotoHip*Cos(a0) + HiptoThighCOM*Cos(a0 + hipL)))

CForm[a21]
-(Mthigh*(TorsotoHip*Sin(a0) + HiptoThighCOM*Sin(a0 + hipL)))

CForm[a22]
   Ithigh + Itorso + Power(HiptoThighCOM,2)*Mthigh + 
    Mthigh*Power(TorsotoHip,2) + 2*HiptoThighCOM*Mthigh*TorsotoHip*Cos(hipL)

CForm[a23]
   Ithigh + Power(HiptoThighCOM,2)*Mthigh + 
    HiptoThighCOM*Mthigh*TorsotoHip*Cos(hipL)

CForm[Simplify[Expand[Simplify[Expand[tau0 - xdd*a20 - ydd*a21 - a0dd*a22 - hipLdd*a23]]]]]
   -(Mthigh*(G*TorsotoHip*Sin(a0) + 
        HiptoThighCOM*(hipLd*(2*a0d + hipLd)*TorsotoHip*Sin(hipL) + 
           G*Sin(a0 + hipL))))

(**************************************************************)

a30 = Simplify[Expand[Simplify[Coefficient[tauHipL,xdd]]]]
a31 = Simplify[Expand[Simplify[Coefficient[tauHipL,ydd]]]]
a32 = Simplify[Expand[Simplify[Coefficient[tauHipL,a0dd]]]]
a33 = Simplify[Expand[Simplify[Coefficient[tauHipL,hipLdd]]]]

CForm[a30]
-(HiptoThighCOM*Mthigh*Cos(a0 + hipL))

CForm[a31]
-(HiptoThighCOM*Mthigh*Sin(a0 + hipL))

CForm[a32]
   Ithigh + Power(HiptoThighCOM,2)*Mthigh + 
    HiptoThighCOM*Mthigh*TorsotoHip*Cos(hipL)

CForm[a33]
Ithigh + Power(HiptoThighCOM,2)*Mthigh

CForm[Simplify[Expand[Simplify[Expand[tauHipL - xdd*a30 - ydd*a31 - a0dd*a32 - hipLdd*a33]]]]]
   HiptoThighCOM*Mthigh*(Power(a0d,2)*TorsotoHip*Sin(hipL) - 
      G*Sin(a0 + hipL))

(**************************************************************)
(**************************************************************)
(**************************************************************)
(**************************************************************)
(**************************************************************)
(**************************************************************)
(**************************************************************)
(**************************************************************)
(**************************************************************)
(**************************************************************)
(**************************************************************)
(**************************************************************)
(**************************************************************)