(* Inverse dynamics for a one joint mechanism using Lagrangian approach *)
(* Moment of inertia about center of mass of each link *)
(* zero position has robot along X axis, angle is standard angle *)

(* 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]

(*Deal with time dependence *)
SetAttributes [m1, Constant]
SetAttributes [l1, Constant]
SetAttributes [l1cm, Constant]
SetAttributes [I1, Constant]
a1/: Dt[a1, t] = a1d
a1d/: Dt[a1d, t] = a1dd

(*Define link lengths.*)

s1 = {l1,0}

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

p1 = {0, 0}

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

cm1 = r01 . {l1cm,0}

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

q1 = p1 + cm1

(* {l1cm Cos[a1], l1cm Sin[a1]} *)

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

qd1 = Dt[q1,t]

(* {-(a1d l1cm Sin[a1]), a1d l1cm Cos[a1]} *)

qdd1 = Dt[qd1,t]

(*
      2
{-(a1d  l1cm Cos[a1]) - a1dd l1cm Sin[a1], 
                            2
>    a1dd l1cm Cos[a1] - a1d  l1cm Sin[a1]}
*)

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

w1 = Dt[a1,t]

(* a1d *)

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

wd1 = Dt[w1,t]

(* a1dd *)

(*Kinetic energy *)

KE = 0.5*(w1*I1*w1) + 0.5*m1*qd1.qd1

(*
       2                 2     2        2      2     2        2
0.5 a1d  I1 + 0.5 m1 (a1d  l1cm  Cos[a1]  + a1d  l1cm  Sin[a1] )
*)
(* Potential energy *)

PE = m1*G*q1[[2]]

(*
G l1cm m1 Sin[a1]
*)

L = Expand[ KE - PE ]

(*
       2             2     2           2
0.5 a1d  I1 + 0.5 a1d  l1cm  m1 Cos[a1]  - G l1cm m1 Sin[a1] + 
            2     2           2
>    0.5 a1d  l1cm  m1 Sin[a1]
*)

eqa1 = Expand[Dt[D[L,a1d],t] - D[L,a1]]

tau1 = Simplify[eqa1] /. 1.->1
tau1 = Simplify[tau1] /. 0.->0
tau1 = Simplify[tau1] /. 2.->2
tau1 = Simplify[Expand[Simplify[Expand[tau1, Trig->True]]]]

(*
               2
a1dd (I1 + l1cm  m1) + G l1cm m1 Cos[a1]
*)

(* To get C code use *)
CForm[tau1]

(*
a1dd*I1 + a1dd*Power(l1cm,2)*m1 + G*l1cm*m1*Cos(a1)
*)