(* Inverse dynamics for a four joint pendulum using Newton Euler approach *)
(* Moment of inertia about center of mass of each link *)
(* zero position has robot hanging down along Y axis *)

(* 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]
r12={{Cos[a2],-Sin[a2]},{Sin[a2],Cos[a2]}}
r21=Transpose[r12]
r23={{Cos[a3],-Sin[a3]},{Sin[a3],Cos[a3]}}
r32=Transpose[r23]
r34={{Cos[a4],-Sin[a4]},{Sin[a4],Cos[a4]}}
r43=Transpose[r34]

(*Deal with time dependence *)
SetAttributes [m1, Constant]
SetAttributes [m2, Constant]
SetAttributes [m3, Constant]
SetAttributes [m4, Constant]
SetAttributes [l1, Constant]
SetAttributes [l2, Constant]
SetAttributes [l3, Constant]
SetAttributes [l4, Constant]
SetAttributes [l1cm, Constant]
SetAttributes [l2cm, Constant]
SetAttributes [l3cm, Constant]
SetAttributes [l4cm, Constant]
SetAttributes [I1, Constant]
SetAttributes [I2, Constant]
SetAttributes [I3, Constant]
SetAttributes [I4, Constant]
a1/: Dt[a1, t] = a1d
a2/: Dt[a2, t] = a2d
a3/: Dt[a3, t] = a3d
a4/: Dt[a4, t] = a4d
a1d/: Dt[a1d, t] = a1dd
a2d/: Dt[a2d, t] = a2dd
a3d/: Dt[a3d, t] = a3dd
a4d/: Dt[a4d, t] = a4dd

(*Define link lengths.*)

s1 = {0,-l1}
s2 = {0,-l2}
s3 = {0,-l3}
s4 = {0,-l4}

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

p1 = {0, 0}
p2 = r01 . s1 + p1
p3 = r01 . r12 . s2 + p2
p4 = r01 . r12 . r23 . s3 + p3

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

cm1 = r01 . {0,-l1cm}
cm2 = r01 . r12 . {0,-l2cm}
cm3 = r01 . r12 . r23 . {0,-l3cm}
cm4 = r01 . r12 . r23 . r34 . {0,-l4cm}

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

q1 = p1 + cm1
q2 = p2 + cm2
q3 = p3 + cm3
q4 = p4 + cm4

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

qd1 = Dt[q1,t]
qd2 = Dt[q2,t]
qd3 = Dt[q3,t]
qd4 = Dt[q4,t]

qdd1 = Dt[qd1,t]
qdd2 = Dt[qd2,t]
qdd3 = Dt[qd3,t]
qdd4 = Dt[qd4,t]

xq1 = Simplify[Expand[Simplify[Coefficient[qdd4,a1dd]]]]
xq2 = Simplify[Expand[Simplify[Coefficient[qdd4,a2dd]]]]
xq3 = Simplify[Expand[Simplify[Coefficient[qdd4,a3dd]]]]
xq4 = Simplify[Expand[Simplify[Coefficient[qdd4,a4dd]]]]
CForm[xq1]
CForm[xq2]
CForm[xq3]
CForm[xq4]
CForm[Simplify[Expand[Simplify[Expand[qdd4 - a1dd*xq1 - a2dd*xq2 - a3dd*xq3 - a4dd*xq4]]]]]

(*Compute net link forces.*)

f1 = m1 * qdd1 - m1 * gravity
f2 = m2 * qdd2 - m2 * gravity
f3 = m3 * qdd3 - m3 * gravity
f4 = m4 * qdd4 - m4 * gravity

xq1 = Simplify[Expand[Simplify[Coefficient[f4,a1dd]]]]
xq2 = Simplify[Expand[Simplify[Coefficient[f4,a2dd]]]]
xq3 = Simplify[Expand[Simplify[Coefficient[f4,a3dd]]]]
xq4 = Simplify[Expand[Simplify[Coefficient[f4,a4dd]]]]
CForm[xq1]
CForm[xq2]
CForm[xq3]
CForm[xq4]
CForm[Simplify[Expand[Simplify[Expand[f4 - a1dd*xq1 - a2dd*xq2 - a3dd*xq3 - a4dd*xq4]]]]]

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

w1 = Dt[a1,t]
w2 = w1 + Dt[a2,t]
w3 = w2 + Dt[a3,t]
w4 = w3 + Dt[a4,t]

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

wd1 = Dt[w1,t]
wd2 = Dt[w2,t]
wd3 = Dt[w3,t]
wd4 = Dt[w4,t]

(*Compute net link torques.*)

n1 = I1*wd1
n2 = I2*wd2
n3 = I3*wd3
n4 = I4*wd4

(*Compute forces at the joints.*)

f34 = f4
f23 = f3 + f34
f12 = f2 + f23
f01 = f1 + f12

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

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

Simplify[Expand[Simplify[cm4]]]

(*Compute torques at the joints.*)

tau4 = n4 + CP[ cm4, f4 ]
tau3 = n3 + tau4 + CP[ cm3, f3 ] + CP[ p4-p3, f34 ]
tau2 = n2 + tau3 + CP[ cm2, f2 ] + CP[ p3-p2, f23 ]
tau1 = n1 + tau2 + CP[ cm1, f1 ] + CP[ p2-p1, f12 ]

tau1 = Simplify[Expand[Simplify[Expand[tau1, Trig->True]]]]
tau2 = Simplify[Expand[Simplify[Expand[tau2, Trig->True]]]]
tau3 = Simplify[Expand[Simplify[Expand[tau3, Trig->True]]]]
tau4 = Simplify[Expand[Simplify[Expand[tau4, Trig->True]]]]

a = Simplify[Expand[Simplify[Coefficient[tau1,a1dd]]]]
b = Simplify[Expand[Simplify[Coefficient[tau1,a2dd]]]]
c = Simplify[Expand[Simplify[Coefficient[tau1,a3dd]]]]
d = Simplify[Expand[Simplify[Coefficient[tau1,a4dd]]]]

CForm[a]
   I1 + I2 + I3 + I4 + Power(l1cm,2)*m1 + Power(l1,2)*m2 + Power(l2cm,2)*m2 + 
    Power(l1,2)*m3 + Power(l2,2)*m3 + Power(l3cm,2)*m3 + Power(l1,2)*m4 + 
    Power(l2,2)*m4 + Power(l3,2)*m4 + Power(l4cm,2)*m4 + 
    2*l1*(l2cm*m2 + l2*(m3 + m4))*Cos(a2) + 2*l2*(l3cm*m3 + l3*m4)*Cos(a3) + 
    2*l1*l3cm*m3*Cos(a2 + a3) + 2*l1*l3*m4*Cos(a2 + a3) + 
    2*l3*l4cm*m4*Cos(a4) + 2*l2*l4cm*m4*Cos(a3 + a4) + 
    2*l1*l4cm*m4*Cos(a2 + a3 + a4)

CForm[b]
   I2 + I3 + I4 + Power(l2cm,2)*m2 + Power(l2,2)*m3 + Power(l3cm,2)*m3 + 
    Power(l2,2)*m4 + Power(l3,2)*m4 + Power(l4cm,2)*m4 + 
    l1*(l2cm*m2 + l2*(m3 + m4))*Cos(a2) + 2*l2*(l3cm*m3 + l3*m4)*Cos(a3) + 
    l1*l3cm*m3*Cos(a2 + a3) + l1*l3*m4*Cos(a2 + a3) + 2*l3*l4cm*m4*Cos(a4) + 
    2*l2*l4cm*m4*Cos(a3 + a4) + l1*l4cm*m4*Cos(a2 + a3 + a4)

CForm[c]
   I3 + I4 + Power(l3cm,2)*m3 + Power(l3,2)*m4 + Power(l4cm,2)*m4 + 
    l2*(l3cm*m3 + l3*m4)*Cos(a3) + l1*(l3cm*m3 + l3*m4)*Cos(a2 + a3) + 
    2*l3*l4cm*m4*Cos(a4) + l2*l4cm*m4*Cos(a3 + a4) + 
    l1*l4cm*m4*Cos(a2 + a3 + a4)

CForm[d]
   I4 + Power(l4cm,2)*m4 + l3*l4cm*m4*Cos(a4) + l2*l4cm*m4*Cos(a3 + a4) + 
    l1*l4cm*m4*Cos(a2 + a3 + a4)

CForm[Simplify[Expand[Simplify[Expand[tau1 - a1dd*a - a2dd*b - a3dd*c - a4dd*d]]]]]
   G*(l1cm*m1 + l1*(m2 + m3 + m4))*Sin(a1) - 
    a2d*(2*a1d + a2d)*l1*(l2cm*m2 + l2*(m3 + m4))*Sin(a2) + 
    G*l2cm*m2*Sin(a1 + a2) + G*l2*m3*Sin(a1 + a2) + G*l2*m4*Sin(a1 + a2) - 
    2*a1d*a3d*l2*l3cm*m3*Sin(a3) - 2*a2d*a3d*l2*l3cm*m3*Sin(a3) - 
    Power(a3d,2)*l2*l3cm*m3*Sin(a3) - 2*a1d*a3d*l2*l3*m4*Sin(a3) - 
    2*a2d*a3d*l2*l3*m4*Sin(a3) - Power(a3d,2)*l2*l3*m4*Sin(a3) - 
    2*a1d*a2d*l1*l3cm*m3*Sin(a2 + a3) - 
    Power(a2d,2)*l1*l3cm*m3*Sin(a2 + a3) - 
    2*a1d*a3d*l1*l3cm*m3*Sin(a2 + a3) - 2*a2d*a3d*l1*l3cm*m3*Sin(a2 + a3) - 
    Power(a3d,2)*l1*l3cm*m3*Sin(a2 + a3) - 2*a1d*a2d*l1*l3*m4*Sin(a2 + a3) - 
    Power(a2d,2)*l1*l3*m4*Sin(a2 + a3) - 2*a1d*a3d*l1*l3*m4*Sin(a2 + a3) - 
    2*a2d*a3d*l1*l3*m4*Sin(a2 + a3) - Power(a3d,2)*l1*l3*m4*Sin(a2 + a3) + 
    G*l3cm*m3*Sin(a1 + a2 + a3) + G*l3*m4*Sin(a1 + a2 + a3) - 
    2*a1d*a4d*l3*l4cm*m4*Sin(a4) - 2*a2d*a4d*l3*l4cm*m4*Sin(a4) - 
    2*a3d*a4d*l3*l4cm*m4*Sin(a4) - Power(a4d,2)*l3*l4cm*m4*Sin(a4) - 
    2*a1d*a3d*l2*l4cm*m4*Sin(a3 + a4) - 2*a2d*a3d*l2*l4cm*m4*Sin(a3 + a4) - 
    Power(a3d,2)*l2*l4cm*m4*Sin(a3 + a4) - 
    2*a1d*a4d*l2*l4cm*m4*Sin(a3 + a4) - 2*a2d*a4d*l2*l4cm*m4*Sin(a3 + a4) - 
    2*a3d*a4d*l2*l4cm*m4*Sin(a3 + a4) - 
    Power(a4d,2)*l2*l4cm*m4*Sin(a3 + a4) - 
    2*a1d*a2d*l1*l4cm*m4*Sin(a2 + a3 + a4) - 
    Power(a2d,2)*l1*l4cm*m4*Sin(a2 + a3 + a4) - 
    2*a1d*a3d*l1*l4cm*m4*Sin(a2 + a3 + a4) - 
    2*a2d*a3d*l1*l4cm*m4*Sin(a2 + a3 + a4) - 
    Power(a3d,2)*l1*l4cm*m4*Sin(a2 + a3 + a4) - 
    2*a1d*a4d*l1*l4cm*m4*Sin(a2 + a3 + a4) - 
    2*a2d*a4d*l1*l4cm*m4*Sin(a2 + a3 + a4) - 
    2*a3d*a4d*l1*l4cm*m4*Sin(a2 + a3 + a4) - 
    Power(a4d,2)*l1*l4cm*m4*Sin(a2 + a3 + a4) + 
    G*l4cm*m4*Sin(a1 + a2 + a3 + a4)

e = Simplify[Expand[Simplify[Coefficient[tau2,a1dd]]]]
f = Simplify[Expand[Simplify[Coefficient[tau2,a2dd]]]]
g = Simplify[Expand[Simplify[Coefficient[tau2,a3dd]]]]
h = Simplify[Expand[Simplify[Coefficient[tau2,a4dd]]]]

CForm[e]
   I2 + I3 + I4 + Power(l2cm,2)*m2 + Power(l2,2)*m3 + Power(l3cm,2)*m3 + 
    Power(l2,2)*m4 + Power(l3,2)*m4 + Power(l4cm,2)*m4 + 
    l1*(l2cm*m2 + l2*(m3 + m4))*Cos(a2) + 2*l2*(l3cm*m3 + l3*m4)*Cos(a3) + 
    l1*l3cm*m3*Cos(a2 + a3) + l1*l3*m4*Cos(a2 + a3) + 2*l3*l4cm*m4*Cos(a4) + 
    2*l2*l4cm*m4*Cos(a3 + a4) + l1*l4cm*m4*Cos(a2 + a3 + a4)

CForm[f]
   I2 + I3 + I4 + Power(l2cm,2)*m2 + Power(l2,2)*m3 + Power(l3cm,2)*m3 + 
    Power(l2,2)*m4 + Power(l3,2)*m4 + Power(l4cm,2)*m4 + 
    2*l2*(l3cm*m3 + l3*m4)*Cos(a3) + 2*l3*l4cm*m4*Cos(a4) + 
    2*l2*l4cm*m4*Cos(a3 + a4)

CForm[g]
   I3 + I4 + Power(l3cm,2)*m3 + Power(l3,2)*m4 + Power(l4cm,2)*m4 + 
    l2*(l3cm*m3 + l3*m4)*Cos(a3) + 2*l3*l4cm*m4*Cos(a4) + 
    l2*l4cm*m4*Cos(a3 + a4)

CForm[h]
   I4 + Power(l4cm,2)*m4 + l3*l4cm*m4*Cos(a4) + l2*l4cm*m4*Cos(a3 + a4)

CForm[Simplify[Expand[Simplify[Expand[tau2 - a1dd*e - a2dd*f - a3dd*g - a4dd*h]]]]]
   Power(a1d,2)*l1*(l2cm*m2 + l2*(m3 + m4))*Sin(a2) + 
    G*(l2cm*m2 + l2*(m3 + m4))*Sin(a1 + a2) - 2*a1d*a3d*l2*l3cm*m3*Sin(a3) - 
    2*a2d*a3d*l2*l3cm*m3*Sin(a3) - Power(a3d,2)*l2*l3cm*m3*Sin(a3) - 
    2*a1d*a3d*l2*l3*m4*Sin(a3) - 2*a2d*a3d*l2*l3*m4*Sin(a3) - 
    Power(a3d,2)*l2*l3*m4*Sin(a3) + Power(a1d,2)*l1*l3cm*m3*Sin(a2 + a3) + 
    Power(a1d,2)*l1*l3*m4*Sin(a2 + a3) + G*l3cm*m3*Sin(a1 + a2 + a3) + 
    G*l3*m4*Sin(a1 + a2 + a3) - 2*a1d*a4d*l3*l4cm*m4*Sin(a4) - 
    2*a2d*a4d*l3*l4cm*m4*Sin(a4) - 2*a3d*a4d*l3*l4cm*m4*Sin(a4) - 
    Power(a4d,2)*l3*l4cm*m4*Sin(a4) - 2*a1d*a3d*l2*l4cm*m4*Sin(a3 + a4) - 
    2*a2d*a3d*l2*l4cm*m4*Sin(a3 + a4) - 
    Power(a3d,2)*l2*l4cm*m4*Sin(a3 + a4) - 
    2*a1d*a4d*l2*l4cm*m4*Sin(a3 + a4) - 2*a2d*a4d*l2*l4cm*m4*Sin(a3 + a4) - 
    2*a3d*a4d*l2*l4cm*m4*Sin(a3 + a4) - 
    Power(a4d,2)*l2*l4cm*m4*Sin(a3 + a4) + 
    Power(a1d,2)*l1*l4cm*m4*Sin(a2 + a3 + a4) + 
    G*l4cm*m4*Sin(a1 + a2 + a3 + a4)


i = Simplify[Expand[Simplify[Coefficient[tau3,a1dd]]]]
j = Simplify[Expand[Simplify[Coefficient[tau3,a2dd]]]]
k = Simplify[Expand[Simplify[Coefficient[tau3,a3dd]]]]
l = Simplify[Expand[Simplify[Coefficient[tau3,a4dd]]]]

CForm[i]
   I3 + I4 + Power(l3cm,2)*m3 + Power(l3,2)*m4 + Power(l4cm,2)*m4 + 
    l2*(l3cm*m3 + l3*m4)*Cos(a3) + l1*(l3cm*m3 + l3*m4)*Cos(a2 + a3) + 
    2*l3*l4cm*m4*Cos(a4) + l2*l4cm*m4*Cos(a3 + a4) + 
    l1*l4cm*m4*Cos(a2 + a3 + a4)

CForm[j]
   I3 + I4 + Power(l3cm,2)*m3 + Power(l3,2)*m4 + Power(l4cm,2)*m4 + 
    l2*(l3cm*m3 + l3*m4)*Cos(a3) + 2*l3*l4cm*m4*Cos(a4) + 
    l2*l4cm*m4*Cos(a3 + a4)

CForm[k]
   I3 + I4 + Power(l3cm,2)*m3 + Power(l3,2)*m4 + Power(l4cm,2)*m4 + 
    2*l3*l4cm*m4*Cos(a4)

CForm[l]
I4 + Power(l4cm,2)*m4 + l3*l4cm*m4*Cos(a4)

CForm[Simplify[Expand[Simplify[Expand[tau3 - a1dd*i - a2dd*j - a3dd*k - a4dd*l]]]]]
   Power(a1d + a2d,2)*l2*(l3cm*m3 + l3*m4)*Sin(a3) + 
    Power(a1d,2)*l1*(l3cm*m3 + l3*m4)*Sin(a2 + a3) + 
    G*l3cm*m3*Sin(a1 + a2 + a3) + G*l3*m4*Sin(a1 + a2 + a3) - 
    2*a1d*a4d*l3*l4cm*m4*Sin(a4) - 2*a2d*a4d*l3*l4cm*m4*Sin(a4) - 
    2*a3d*a4d*l3*l4cm*m4*Sin(a4) - Power(a4d,2)*l3*l4cm*m4*Sin(a4) + 
    Power(a1d,2)*l2*l4cm*m4*Sin(a3 + a4) + 
    2*a1d*a2d*l2*l4cm*m4*Sin(a3 + a4) + 
    Power(a2d,2)*l2*l4cm*m4*Sin(a3 + a4) + 
    Power(a1d,2)*l1*l4cm*m4*Sin(a2 + a3 + a4) + 
    G*l4cm*m4*Sin(a1 + a2 + a3 + a4)

m = Simplify[Expand[Simplify[Coefficient[tau4,a1dd]]]]
n = Simplify[Expand[Simplify[Coefficient[tau4,a2dd]]]]
o = Simplify[Expand[Simplify[Coefficient[tau4,a3dd]]]]
p = Simplify[Expand[Simplify[Coefficient[tau4,a4dd]]]]

CForm[m]
   I4 + Power(l4cm,2)*m4 + l3*l4cm*m4*Cos(a4) + l2*l4cm*m4*Cos(a3 + a4) + 
    l1*l4cm*m4*Cos(a2 + a3 + a4)
CForm[n]
   I4 + Power(l4cm,2)*m4 + l3*l4cm*m4*Cos(a4) + l2*l4cm*m4*Cos(a3 + a4)

CForm[o]
I4 + Power(l4cm,2)*m4 + l3*l4cm*m4*Cos(a4)

CForm[p]
I4 + Power(l4cm,2)*m4

CForm[Simplify[Expand[Simplify[Expand[tau4 - a1dd*m - a2dd*n - a3dd*o - a4dd*p]]]]]
   l4cm*m4*(Power(a1d + a2d + a3d,2)*l3*Sin(a4) + 
      Power(a1d + a2d,2)*l2*Sin(a3 + a4) + 
      Power(a1d,2)*l1*Sin(a2 + a3 + a4) + G*Sin(a1 + a2 + a3 + a4))

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