(* Inverse dynamics for a two joint mechanism using Newton Euler approach *)
(* Moment of inertia about center of mass of each link *)
(*Define the vector cross product.*)
CP[X_,Y_] := {X[[2]]*Y[[3]] - X[[3]]*Y[[2]],
-X[[1]]*Y[[3]] + X[[3]]*Y[[1]],
X[[1]]*Y[[2]] - X[[2]]*Y[[1]]}
(* the starting position is hanging straight down with z down, x to the side,
and y coming out towards the viewer. *)
gravity = {0, 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],0,Sin[a1]},{0,1,0},{-Sin[a1],0, Cos[a1]}}
r10=Transpose[r01]
r12={{Cos[a2],0,Sin[a2]},{0,1,0},{-Sin[a2],0, Cos[a2]}}
r21=Transpose[r12]
(*Deal with time dependence *)
SetAttributes [m1, Constant]
SetAttributes [m2, Constant]
SetAttributes [l1, Constant]
SetAttributes [l2, Constant]
SetAttributes [l1cm, Constant]
SetAttributes [l2cm, Constant]
SetAttributes [I1xx, Constant]
SetAttributes [I1yy, Constant]
SetAttributes [I1zz, Constant]
SetAttributes [I2xx, Constant]
SetAttributes [I2yy, Constant]
SetAttributes [I2zz, Constant]
a1/: Dt[a1, t] = a1d
a2/: Dt[a2, t] = a2d
a1d/: Dt[a1d, t] = a1dd
a2d/: Dt[a2d, t] = a2dd
(*Define link lengths.*)
s1 = {0, 0, l1}
s2 = {0, 0, l2}
(*Define the location of the proximal end of each link (in world coordinates) *)
p1 = {0, 0, 0}
p2 = r01 . s1
Out[25]= {l1 Sin[a1], 0, l1 Cos[a1]}
(*Define the location of the center of mass with respect
to the proximal end of each link.*)
cm1 = r01 . {0, 0, l1cm}
Out[26]= {l1cm Sin[a1], 0, l1cm Cos[a1]}
cm2 = r01 . r12 . {0, 0, l2cm}
Out[27]= {l2cm (Cos[a2] Sin[a1] + Cos[a1] Sin[a2]), 0,
> l2cm (Cos[a1] Cos[a2] - Sin[a1] Sin[a2])}
(*Compute the location of the center of mass of each link.*)
q1 = p1 + cm1
Out[28]= {l1cm Sin[a1], 0, l1cm Cos[a1]}
q2 = p2 + cm2
Out[29]= {l1 Sin[a1] + l2cm (Cos[a2] Sin[a1] + Cos[a1] Sin[a2]), 0,
> l1 Cos[a1] + l2cm (Cos[a1] Cos[a2] - Sin[a1] Sin[a2])}
(*Compute the linear velocity and acceleration of each link cm.*)
qd1 = Dt[q1,t]
Out[30]= {a1d l1cm Cos[a1], 0, -(a1d l1cm Sin[a1])}
qd2 = Dt[q2,t]
Out[31]= {a1d l1 Cos[a1] + l2cm
> (a1d Cos[a1] Cos[a2] + a2d Cos[a1] Cos[a2] - a1d Sin[a1] Sin[a2] -
> a2d Sin[a1] Sin[a2]), 0,
> -(a1d l1 Sin[a1]) + l2cm (-(a1d Cos[a2] Sin[a1]) - a2d Cos[a2] Sin[a1] -
> a1d Cos[a1] Sin[a2] - a2d Cos[a1] Sin[a2])}
qdd1 = Dt[qd1,t]
2
Out[32]= {a1dd l1cm Cos[a1] - a1d l1cm Sin[a1], 0,
2
> -(a1d l1cm Cos[a1]) - a1dd l1cm Sin[a1]}
qdd2 = Dt[qd2,t]
2
Out[33]= {a1dd l1 Cos[a1] - a1d l1 Sin[a1] +
> l2cm (a1dd Cos[a1] Cos[a2] + a2dd Cos[a1] Cos[a2] -
2
> a1d Cos[a2] Sin[a1] - 2 a1d a2d Cos[a2] Sin[a1] -
2 2
> a2d Cos[a2] Sin[a1] - a1d Cos[a1] Sin[a2] -
2
> 2 a1d a2d Cos[a1] Sin[a2] - a2d Cos[a1] Sin[a2] -
> a1dd Sin[a1] Sin[a2] - a2dd Sin[a1] Sin[a2]), 0,
2
> -(a1d l1 Cos[a1]) - a1dd l1 Sin[a1] +
2
> l2cm (-(a1d Cos[a1] Cos[a2]) - 2 a1d a2d Cos[a1] Cos[a2] -
2
> a2d Cos[a1] Cos[a2] - a1dd Cos[a2] Sin[a1] - a2dd Cos[a2] Sin[a1] -
2
> a1dd Cos[a1] Sin[a2] - a2dd Cos[a1] Sin[a2] + a1d Sin[a1] Sin[a2] +
2
> 2 a1d a2d Sin[a1] Sin[a2] + a2d Sin[a1] Sin[a2])}
(*Compute the angular velocity of each link.*)
w1 = {0,Dt[a1,t],0}
Out[36]= {0, a1d, 0}
w2 = w1 + {0,Dt[a2,t],0}
Out[37]= {0, a1d + a2d, 0}
(*Compute the angular acceleration of each link.*)
wd1 = Dt[w1,t]
Out[38]= {0, a1dd, 0}
wd2 = Dt[w2,t]
Out[39]= {0, a1dd + a2dd, 0}
(* Moment of inertia tensors. We will express them in link coordinates about
the center of mass *)
I1 = {{I1xx, 0, 0}, {0, I1yy, 0}, {0, 0, I1zz}}
I2 = {{I2xx, 0, 0}, {0, I2yy, 0}, {0, 0, I2zz}}
(*Compute net link forces.*)
f1 = m1 * qdd1 + m1 * gravity
2
Out[44]= {m1 (a1dd l1cm Cos[a1] - a1d l1cm Sin[a1]), 0,
2
> -(G m1) + m1 (-(a1d l1cm Cos[a1]) - a1dd l1cm Sin[a1])}
f2 = m2 * qdd2 + m2 * gravity
2
Out[45]= {m2 (a1dd l1 Cos[a1] - a1d l1 Sin[a1] +
> l2cm (a1dd Cos[a1] Cos[a2] + a2dd Cos[a1] Cos[a2] -
2
> a1d Cos[a2] Sin[a1] - 2 a1d a2d Cos[a2] Sin[a1] -
2 2
> a2d Cos[a2] Sin[a1] - a1d Cos[a1] Sin[a2] -
2
> 2 a1d a2d Cos[a1] Sin[a2] - a2d Cos[a1] Sin[a2] -
> a1dd Sin[a1] Sin[a2] - a2dd Sin[a1] Sin[a2])), 0,
2
> -(G m2) + m2 (-(a1d l1 Cos[a1]) - a1dd l1 Sin[a1] +
2
> l2cm (-(a1d Cos[a1] Cos[a2]) - 2 a1d a2d Cos[a1] Cos[a2] -
2
> a2d Cos[a1] Cos[a2] - a1dd Cos[a2] Sin[a1] -
> a2dd Cos[a2] Sin[a1] - a1dd Cos[a1] Sin[a2] -
2
> a2dd Cos[a1] Sin[a2] + a1d Sin[a1] Sin[a2] +
2
> 2 a1d a2d Sin[a1] Sin[a2] + a2d Sin[a1] Sin[a2]))}
(*Compute net link torques.*)
n1 = I1 . wd1 + CP[w1,I1.w1]
Out[46]= {0, a1dd I1yy, 0}
n2 = I2 . wd2 + CP[w2,I2.w2]
Out[47]= {0, (a1dd + a2dd) I2yy, 0}
(*Compute forces at the joints.*)
f12 = f2
2
Out[48]= {m2 (a1dd l1 Cos[a1] - a1d l1 Sin[a1] +
> l2cm (a1dd Cos[a1] Cos[a2] + a2dd Cos[a1] Cos[a2] -
2
> a1d Cos[a2] Sin[a1] - 2 a1d a2d Cos[a2] Sin[a1] -
2 2
> a2d Cos[a2] Sin[a1] - a1d Cos[a1] Sin[a2] -
2
> 2 a1d a2d Cos[a1] Sin[a2] - a2d Cos[a1] Sin[a2] -
> a1dd Sin[a1] Sin[a2] - a2dd Sin[a1] Sin[a2])), 0,
2
> -(G m2) + m2 (-(a1d l1 Cos[a1]) - a1dd l1 Sin[a1] +
2
> l2cm (-(a1d Cos[a1] Cos[a2]) - 2 a1d a2d Cos[a1] Cos[a2] -
2
> a2d Cos[a1] Cos[a2] - a1dd Cos[a2] Sin[a1] -
> a2dd Cos[a2] Sin[a1] - a1dd Cos[a1] Sin[a2] -
2
> a2dd Cos[a1] Sin[a2] + a1d Sin[a1] Sin[a2] +
2
> 2 a1d a2d Sin[a1] Sin[a2] + a2d Sin[a1] Sin[a2]))}
f01 = f1 + f12
2
Out[49]= {m1 (a1dd l1cm Cos[a1] - a1d l1cm Sin[a1]) +
2
> m2 (a1dd l1 Cos[a1] - a1d l1 Sin[a1] +
> l2cm (a1dd Cos[a1] Cos[a2] + a2dd Cos[a1] Cos[a2] -
2
> a1d Cos[a2] Sin[a1] - 2 a1d a2d Cos[a2] Sin[a1] -
2 2
> a2d Cos[a2] Sin[a1] - a1d Cos[a1] Sin[a2] -
2
> 2 a1d a2d Cos[a1] Sin[a2] - a2d Cos[a1] Sin[a2] -
> a1dd Sin[a1] Sin[a2] - a2dd Sin[a1] Sin[a2])), 0,
2
> -(G m1) - G m2 + m1 (-(a1d l1cm Cos[a1]) - a1dd l1cm Sin[a1]) +
2
> m2 (-(a1d l1 Cos[a1]) - a1dd l1 Sin[a1] +
2
> l2cm (-(a1d Cos[a1] Cos[a2]) - 2 a1d a2d Cos[a1] Cos[a2] -
2
> a2d Cos[a1] Cos[a2] - a1dd Cos[a2] Sin[a1] -
> a2dd Cos[a2] Sin[a1] - a1dd Cos[a1] Sin[a2] -
2
> a2dd Cos[a1] Sin[a2] + a1d Sin[a1] Sin[a2] +
2
> 2 a1d a2d Sin[a1] Sin[a2] + a2d Sin[a1] Sin[a2]))}
(*Compute torques at the joints.*)
n12 = n2 + CP [ cm2, f2 ]
Out[50]= {0, (a1dd + a2dd) I2yy +
> l2cm m2 (Cos[a1] Cos[a2] - Sin[a1] Sin[a2])
2
> (a1dd l1 Cos[a1] - a1d l1 Sin[a1] +
> l2cm (a1dd Cos[a1] Cos[a2] + a2dd Cos[a1] Cos[a2] -
2
> a1d Cos[a2] Sin[a1] - 2 a1d a2d Cos[a2] Sin[a1] -
2 2
> a2d Cos[a2] Sin[a1] - a1d Cos[a1] Sin[a2] -
2
> 2 a1d a2d Cos[a1] Sin[a2] - a2d Cos[a1] Sin[a2] -
> a1dd Sin[a1] Sin[a2] - a2dd Sin[a1] Sin[a2])) -
> l2cm (Cos[a2] Sin[a1] + Cos[a1] Sin[a2])
2
> (-(G m2) + m2 (-(a1d l1 Cos[a1]) - a1dd l1 Sin[a1] +
2
> l2cm (-(a1d Cos[a1] Cos[a2]) - 2 a1d a2d Cos[a1] Cos[a2] -
2
> a2d Cos[a1] Cos[a2] - a1dd Cos[a2] Sin[a1] -
> a2dd Cos[a2] Sin[a1] - a1dd Cos[a1] Sin[a2] -
2
> a2dd Cos[a1] Sin[a2] + a1d Sin[a1] Sin[a2] +
2
> 2 a1d a2d Sin[a1] Sin[a2] + a2d Sin[a1] Sin[a2]))), 0}
n01 = n1 + n12 + CP [ cm1, f1 ] + CP[ p2-p1, f12 ]
Out[51]= {0, a1dd I1yy + (a1dd + a2dd) I2yy +
2
> l1cm m1 Cos[a1] (a1dd l1cm Cos[a1] - a1d l1cm Sin[a1]) -
> l1cm Sin[a1] (-(G m1) +
2
> m1 (-(a1d l1cm Cos[a1]) - a1dd l1cm Sin[a1])) +
2
> l1 m2 Cos[a1] (a1dd l1 Cos[a1] - a1d l1 Sin[a1] +
> l2cm (a1dd Cos[a1] Cos[a2] + a2dd Cos[a1] Cos[a2] -
2
> a1d Cos[a2] Sin[a1] - 2 a1d a2d Cos[a2] Sin[a1] -
2 2
> a2d Cos[a2] Sin[a1] - a1d Cos[a1] Sin[a2] -
2
> 2 a1d a2d Cos[a1] Sin[a2] - a2d Cos[a1] Sin[a2] -
> a1dd Sin[a1] Sin[a2] - a2dd Sin[a1] Sin[a2])) +
> l2cm m2 (Cos[a1] Cos[a2] - Sin[a1] Sin[a2])
2
> (a1dd l1 Cos[a1] - a1d l1 Sin[a1] +
> l2cm (a1dd Cos[a1] Cos[a2] + a2dd Cos[a1] Cos[a2] -
2
> a1d Cos[a2] Sin[a1] - 2 a1d a2d Cos[a2] Sin[a1] -
2 2
> a2d Cos[a2] Sin[a1] - a1d Cos[a1] Sin[a2] -
2
> 2 a1d a2d Cos[a1] Sin[a2] - a2d Cos[a1] Sin[a2] -
> a1dd Sin[a1] Sin[a2] - a2dd Sin[a1] Sin[a2])) -
2
> l1 Sin[a1] (-(G m2) + m2 (-(a1d l1 Cos[a1]) - a1dd l1 Sin[a1] +
2
> l2cm (-(a1d Cos[a1] Cos[a2]) - 2 a1d a2d Cos[a1] Cos[a2] -
2
> a2d Cos[a1] Cos[a2] - a1dd Cos[a2] Sin[a1] -
> a2dd Cos[a2] Sin[a1] - a1dd Cos[a1] Sin[a2] -
2
> a2dd Cos[a1] Sin[a2] + a1d Sin[a1] Sin[a2] +
2
> 2 a1d a2d Sin[a1] Sin[a2] + a2d Sin[a1] Sin[a2]))) -
> l2cm (Cos[a2] Sin[a1] + Cos[a1] Sin[a2])
2
> (-(G m2) + m2 (-(a1d l1 Cos[a1]) - a1dd l1 Sin[a1] +
2
> l2cm (-(a1d Cos[a1] Cos[a2]) - 2 a1d a2d Cos[a1] Cos[a2] -
2
> a2d Cos[a1] Cos[a2] - a1dd Cos[a2] Sin[a1] -
> a2dd Cos[a2] Sin[a1] - a1dd Cos[a1] Sin[a2] -
2
> a2dd Cos[a1] Sin[a2] + a1d Sin[a1] Sin[a2] +
2
> 2 a1d a2d Sin[a1] Sin[a2] + a2d Sin[a1] Sin[a2]))), 0}
(*Pull out just joint torques.*)
tau1 = Expand[n01[[2]], Trig->True]
2 2
Out[52]= a1dd I1yy + a1dd I2yy + a2dd I2yy + a1dd l1cm m1 + a1dd l1 m2 +
2 2
> a1dd l2cm m2 + a2dd l2cm m2 + 2 a1dd l1 l2cm m2 Cos[a2] +
> a2dd l1 l2cm m2 Cos[a2] + G l1cm m1 Sin[a1] + G l1 m2 Sin[a1] +
> G l2cm m2 Cos[a2] Sin[a1] - 2 a1d a2d l1 l2cm m2 Sin[a2] -
2
> a2d l1 l2cm m2 Sin[a2] + G l2cm m2 Cos[a1] Sin[a2]
tau2 = Expand[n12[[2]], Trig->True]
2 2
Out[53]= a1dd I2yy + a2dd I2yy + a1dd l2cm m2 + a2dd l2cm m2 +
> a1dd l1 l2cm m2 Cos[a2] + G l2cm m2 Cos[a2] Sin[a1] +
2
> a1d l1 l2cm m2 Sin[a2] + G l2cm m2 Cos[a1] Sin[a2]
(* To get C code use
CForm[tau1]
Out[54]//CForm=
a1dd*I1yy + a1dd*I2yy + a2dd*I2yy + a1dd*Power(l1cm,2)*m1 +
a1dd*Power(l1,2)*m2 + a1dd*Power(l2cm,2)*m2 + a2dd*Power(l2cm,2)*m2 +
2*a1dd*l1*l2cm*m2*Cos(a2) + a2dd*l1*l2cm*m2*Cos(a2) + G*l1cm*m1*Sin(a1) +
G*l1*m2*Sin(a1) + G*l2cm*m2*Cos(a2)*Sin(a1) -
2*a1d*a2d*l1*l2cm*m2*Sin(a2) - Power(a2d,2)*l1*l2cm*m2*Sin(a2) +
G*l2cm*m2*Cos(a1)*Sin(a2)
CForm[tau2]
Out[55]//CForm=
a1dd*I2yy + a2dd*I2yy + a1dd*Power(l2cm,2)*m2 + a2dd*Power(l2cm,2)*m2 +
a1dd*l1*l2cm*m2*Cos(a2) + G*l2cm*m2*Cos(a2)*Sin(a1) +
Power(a1d,2)*l1*l2cm*m2*Sin(a2) + G*l2cm*m2*Cos(a1)*Sin(a2)
*)