(* Inverse dynamics for a one joint mechanism using Newton Euler 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 net link forces.*)

f1 = m1 * qdd1 - m1 * gravity

(*
          2
{m1 (-(a1d  l1cm Cos[a1]) - a1dd l1cm Sin[a1]), 
                                       2
>    G m1 + m1 (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 *)

(*Compute net link torques.*)

n1 = I1*wd1

(* a1dd I1 *)

(*Compute forces at the joints.*)

f01 = f1;

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

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

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

(*Compute torques at the joints.*)

tau1 = n1 + CP[ cm1, f1 ]

tau1 = Expand[tau1, Trig->True]

(*
                   2
a1dd I1 + a1dd 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)
*)