(* Inverse dynamics for a two joint mechanism using Newton Euler approach *)
(* Moment of inertia about center of mass of each link *)
(* 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]
(*Deal with time dependence *)
SetAttributes [m1, Constant]
SetAttributes [m2, Constant]
SetAttributes [l1, Constant]
SetAttributes [l2, Constant]
SetAttributes [l1cm, Constant]
SetAttributes [l2cm, Constant]
SetAttributes [I1, Constant]
SetAttributes [I2, 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 = {l1,0}
s2 = {l2,0}
(*Define the location of the proximal end of each link (in world coordinates) *)
p1 = {0, 0}
p2 = r01 . s1
(* {l1 Cos[a1], l1 Sin[a1]} *)
(*Define the location of the center of mass with respect
to the proximal end of each link.*)
cm1 = r01 . {l1cm,0}
(* {l1cm Cos[a1], l1cm Sin[a1]} *)
cm2 = r01 . r12 . {l2cm,0}
(* {l2cm (Cos[a1] Cos[a2] - Sin[a1] Sin[a2]),
l2cm (Cos[a2] Sin[a1] + Cos[a1] Sin[a2])} *)
(*Compute the location of the center of mass of each link.*)
q1 = p1 + cm1
(* {l1cm Cos[a1], l1cm Sin[a1]} *)
q2 = p2 + cm2
(* {l1 Cos[a1] + l2cm (Cos[a1] Cos[a2] - Sin[a1] Sin[a2]),
l1 Sin[a1] + l2cm (Cos[a2] Sin[a1] + Cos[a1] Sin[a2])} *)
(*Compute the linear velocity and acceleration of each link cm.*)
qd1 = Dt[q1,t]
(* {-(a1d l1cm Sin[a1]), a1d l1cm Cos[a1]} *)
qd2 = Dt[q2,t]
(* {-(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]), 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])} *)
qdd1 = Dt[qd1,t]
(*
2
{-(a1d l1cm Cos[a1]) - a1dd l1cm Sin[a1],
2
> a1dd l1cm Cos[a1] - a1d l1cm Sin[a1]}
*)
qdd2 = Dt[qd2,t]
(*
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]),
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])}
*)
(*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])}
*)
f2 = m2 * qdd2 - m2 * gravity
(*
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])),
2
> G m2 + 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]))}
*)
(*Compute the angular velocity of each link.*)
w1 = Dt[a1,t]
(* a1d *)
w2 = w1 + Dt[a2,t]
(* a1d + a2d *)
(*Compute the angular acceleration of each link.*)
wd1 = Dt[w1,t]
(* a1dd *)
wd2 = Dt[w2,t]
(* a1dd + a2dd *)
(*Compute net link torques.*)
n1 = I1*wd1
(* a1dd I1 *)
n2 = I2*wd2
(* (a1dd + a2dd) I2 *)
(*Compute forces at the joints.*)
f12 = f2
(* Huge mess *)
f01 = f1 + f12
(* Huge mess *)
(*Define the 2D vector cross product.*)
CP[X_,Y_] := X[[1]]*Y[[2]] - X[[2]]*Y[[1]]
(*Compute torques at the joints.*)
tau2 = n2 + CP [ cm2, f2 ]
(* Huge mess *)
tau1 = n1 + tau2 + CP [ cm1, f1 ] + CP[ p2-p1, f12 ]
(* Huge mess *)
(*Clean it up*)
tau1 = Expand[tau1, Trig->True]
(*
2 2
a1dd I1 + a1dd I2 + a2dd I2 + a1dd l1cm m1 + a1dd l1 m2 +
2 2
> a1dd l2cm m2 + a2dd l2cm m2 + G l1cm m1 Cos[a1] + G l1 m2 Cos[a1] +
> 2 a1dd l1 l2cm m2 Cos[a2] + a2dd l1 l2cm m2 Cos[a2] +
> G l2cm m2 Cos[a1] Cos[a2] - 2 a1d a2d l1 l2cm m2 Sin[a2] -
2
> a2d l1 l2cm m2 Sin[a2] - G l2cm m2 Sin[a1] Sin[a2]
*)
tau2 = Expand[tau2, Trig->True]
(*
2 2
a1dd I2 + a2dd I2 + a1dd l2cm m2 + a2dd l2cm m2 +
> a1dd l1 l2cm m2 Cos[a2] + G l2cm m2 Cos[a1] Cos[a2] +
2
> a1d l1 l2cm m2 Sin[a2] - G l2cm m2 Sin[a1] Sin[a2]
*)
(* To get C code use *)
CForm[tau1]
(*
a1dd*I1 + a1dd*I2 + a2dd*I2 + a1dd*Power(l1cm,2)*m1 +
a1dd*Power(l1,2)*m2 + a1dd*Power(l2cm,2)*m2 + a2dd*Power(l2cm,2)*m2 +
G*l1cm*m1*Cos(a1) + G*l1*m2*Cos(a1) + 2*a1dd*l1*l2cm*m2*Cos(a2) +
a2dd*l1*l2cm*m2*Cos(a2) + G*l2cm*m2*Cos(a1)*Cos(a2) -
2*a1d*a2d*l1*l2cm*m2*Sin(a2) - Power(a2d,2)*l1*l2cm*m2*Sin(a2) -
G*l2cm*m2*Sin(a1)*Sin(a2)
*)
CForm[tau2]
(*
a1dd*I2 + a2dd*I2 + a1dd*Power(l2cm,2)*m2 + a2dd*Power(l2cm,2)*m2 +
a1dd*l1*l2cm*m2*Cos(a2) + G*l2cm*m2*Cos(a1)*Cos(a2) +
Power(a1d,2)*l1*l2cm*m2*Sin(a2) - G*l2cm*m2*Sin(a1)*Sin(a2)
*)