(* Inverse dynamics for a three joint mechanism 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] (*Deal with time dependence *) SetAttributes [m1, Constant] SetAttributes [m2, Constant] SetAttributes [m3, Constant] SetAttributes [l1, Constant] SetAttributes [l2, Constant] SetAttributes [l3, Constant] SetAttributes [l1cm, Constant] SetAttributes [l2cm, Constant] SetAttributes [l3cm, Constant] SetAttributes [I1, Constant] SetAttributes [I2, Constant] SetAttributes [I3, Constant] a1/: Dt[a1, t] = a1d a2/: Dt[a2, t] = a2d a3/: Dt[a3, t] = a3d a1d/: Dt[a1d, t] = a1dd a2d/: Dt[a2d, t] = a2dd a3d/: Dt[a3d, t] = a3dd (*Define link lengths.*) s1 = {0,-l1} s2 = {0,-l2} s3 = {0,-l3} (*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 (*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} (*Compute the location of the center of mass of each link.*) q1 = p1 + cm1 q2 = p2 + cm2 q3 = p3 + cm3 (*Compute the linear velocity and acceleration of each link cm.*) qd1 = Dt[q1,t] qd2 = Dt[q2,t] qd3 = Dt[q3,t] qdd1 = Dt[qd1,t] qdd2 = Dt[qd2,t] qdd3 = Dt[qd3,t] (*Compute net link forces.*) f1 = m1 * qdd1 - m1 * gravity f2 = m2 * qdd2 - m2 * gravity f3 = m3 * qdd3 - m3 * gravity (*Compute the angular velocity of each link.*) w1 = Dt[a1,t] w2 = w1 + Dt[a2,t] w3 = w2 + Dt[a3,t] (*Compute the angular acceleration of each link.*) wd1 = Dt[w1,t] wd2 = Dt[w2,t] wd3 = Dt[w3,t] (*Compute net link torques.*) n1 = I1*wd1 n2 = I2*wd2 n3 = I3*wd3 (*Compute forces at the joints.*) f23 = f3 f12 = f2 + f23 f01 = f1 + f12 (*Define the 2D vector cross product.*) CP[X_,Y_] := X[[1]]*Y[[2]] - X[[2]]*Y[[1]] (*Compute torques at the joints.*) tau3 = n3 + CP[ cm3, f3 ] tau2 = n2 + tau3 + CP[ cm2, f2 ] + CP[ p3-p2, f23 ] tau1 = n1 + tau2 + CP[ cm1, f1 ] + CP[ p2-p1, f12 ] tau1 = Expand[tau1, Trig->True] tau2 = Expand[tau2, Trig->True] tau3 = Expand[tau3, Trig->True] a = Coefficient[tau1,a1dd] b = Coefficient[tau1,a2dd] c = Coefficient[tau1,a3dd] (**************************************************************) (* To get C code use *) CForm[tau1] a1dd*I1 + a1dd*I2 + a2dd*I2 + a1dd*I3 + a2dd*I3 + a3dd*I3 + a1dd*Power(l1cm,2)*m1 + a1dd*Power(l1,2)*m2 + a1dd*Power(l2cm,2)*m2 + a2dd*Power(l2cm,2)*m2 + a1dd*Power(l1,2)*m3 + a2dd*Power(l2,2)*m3 + a1dd*Power(l3cm,2)*m3 + a2dd*Power(l3cm,2)*m3 + a3dd*Power(l3cm,2)*m3 + 2*a1dd*l1*l2cm*m2*Cos(a2) + a2dd*l1*l2cm*m2*Cos(a2) + a1dd*l1*l2*m3*Cos(a1)*Cos(a2) + a2dd*l1*l2*m3*Cos(a1)*Cos(a2) + a1dd*l2*l3cm*m3*Cos(a1)*Cos(a3) + 2*a2dd*l2*l3cm*m3*Cos(a1)*Cos(a3) + a3dd*l2*l3cm*m3*Cos(a1)*Cos(a3) + 2*a1dd*l1*l3cm*m3*Cos(a2)*Cos(a3) + a2dd*l1*l3cm*m3*Cos(a2)*Cos(a3) + a3dd*l1*l3cm*m3*Cos(a2)*Cos(a3) + G*l1cm*m1*Sin(a1) + G*l1*m2*Sin(a1) + G*l1*m3*Sin(a1) + G*l2cm*m2*Cos(a2)*Sin(a1) - Power(a1d,2)*l1*l2*m3*Cos(a2)*Sin(a1) + Power(a2d,2)*l1*l2*m3*Cos(a2)*Sin(a1) - Power(a1d,2)*l2*l3cm*m3*Cos(a3)*Sin(a1) - 2*a1d*a2d*l2*l3cm*m3*Cos(a3)*Sin(a1) - 2*a1d*a3d*l2*l3cm*m3*Cos(a3)*Sin(a1) - 2*a2d*a3d*l2*l3cm*m3*Cos(a3)*Sin(a1) - Power(a3d,2)*l2*l3cm*m3*Cos(a3)*Sin(a1) + G*l3cm*m3*Cos(a2)*Cos(a3)*Sin(a1) - 2*a1d*a2d*l1*l2cm*m2*Sin(a2) - Power(a2d,2)*l1*l2cm*m2*Sin(a2) + G*l2*m3*Sin(a2) + G*l2cm*m2*Cos(a1)*Sin(a2) + Power(a1d,2)*l1*l2*m3*Cos(a1)*Sin(a2) - Power(a2d,2)*l1*l2*m3*Cos(a1)*Sin(a2) - 2*a1d*a2d*l1*l3cm*m3*Cos(a3)*Sin(a2) - Power(a2d,2)*l1*l3cm*m3*Cos(a3)*Sin(a2) - 2*a1d*a3d*l1*l3cm*m3*Cos(a3)*Sin(a2) - 2*a2d*a3d*l1*l3cm*m3*Cos(a3)*Sin(a2) - Power(a3d,2)*l1*l3cm*m3*Cos(a3)*Sin(a2) + G*l3cm*m3*Cos(a1)*Cos(a3)*Sin(a2) + a1dd*l1*l2*m3*Sin(a1)*Sin(a2) + a2dd*l1*l2*m3*Sin(a1)*Sin(a2) - Power(a1d,2)*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a1d*a2d*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a1d*a3d*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a2d*a3d*l2*l3cm*m3*Cos(a1)*Sin(a3) - Power(a3d,2)*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a1d*a2d*l1*l3cm*m3*Cos(a2)*Sin(a3) - Power(a2d,2)*l1*l3cm*m3*Cos(a2)*Sin(a3) - 2*a1d*a3d*l1*l3cm*m3*Cos(a2)*Sin(a3) - 2*a2d*a3d*l1*l3cm*m3*Cos(a2)*Sin(a3) - Power(a3d,2)*l1*l3cm*m3*Cos(a2)*Sin(a3) + G*l3cm*m3*Cos(a1)*Cos(a2)*Sin(a3) - a1dd*l2*l3cm*m3*Sin(a1)*Sin(a3) - 2*a2dd*l2*l3cm*m3*Sin(a1)*Sin(a3) - a3dd*l2*l3cm*m3*Sin(a1)*Sin(a3) - 2*a1dd*l1*l3cm*m3*Sin(a2)*Sin(a3) - a2dd*l1*l3cm*m3*Sin(a2)*Sin(a3) - a3dd*l1*l3cm*m3*Sin(a2)*Sin(a3) - G*l3cm*m3*Sin(a1)*Sin(a2)*Sin(a3) CForm[tau2] a1dd*I2 + a2dd*I2 + a1dd*I3 + a2dd*I3 + a3dd*I3 + a1dd*Power(l2cm,2)*m2 + a2dd*Power(l2cm,2)*m2 + a2dd*Power(l2,2)*m3 + a1dd*Power(l3cm,2)*m3 + a2dd*Power(l3cm,2)*m3 + a3dd*Power(l3cm,2)*m3 + a1dd*l1*l2cm*m2*Cos(a2) + a1dd*l1*l2*m3*Cos(a1)*Cos(a2) + a1dd*l2*l3cm*m3*Cos(a1)*Cos(a3) + 2*a2dd*l2*l3cm*m3*Cos(a1)*Cos(a3) + a3dd*l2*l3cm*m3*Cos(a1)*Cos(a3) + a1dd*l1*l3cm*m3*Cos(a2)*Cos(a3) + G*l2cm*m2*Cos(a2)*Sin(a1) - Power(a1d,2)*l1*l2*m3*Cos(a2)*Sin(a1) - Power(a1d,2)*l2*l3cm*m3*Cos(a3)*Sin(a1) - 2*a1d*a2d*l2*l3cm*m3*Cos(a3)*Sin(a1) - 2*a1d*a3d*l2*l3cm*m3*Cos(a3)*Sin(a1) - 2*a2d*a3d*l2*l3cm*m3*Cos(a3)*Sin(a1) - Power(a3d,2)*l2*l3cm*m3*Cos(a3)*Sin(a1) + G*l3cm*m3*Cos(a2)*Cos(a3)*Sin(a1) + Power(a1d,2)*l1*l2cm*m2*Sin(a2) + G*l2*m3*Sin(a2) + G*l2cm*m2*Cos(a1)*Sin(a2) + Power(a1d,2)*l1*l2*m3*Cos(a1)*Sin(a2) + Power(a1d,2)*l1*l3cm*m3*Cos(a3)*Sin(a2) + G*l3cm*m3*Cos(a1)*Cos(a3)*Sin(a2) + a1dd*l1*l2*m3*Sin(a1)*Sin(a2) - Power(a1d,2)*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a1d*a2d*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a1d*a3d*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a2d*a3d*l2*l3cm*m3*Cos(a1)*Sin(a3) - Power(a3d,2)*l2*l3cm*m3*Cos(a1)*Sin(a3) + Power(a1d,2)*l1*l3cm*m3*Cos(a2)*Sin(a3) + G*l3cm*m3*Cos(a1)*Cos(a2)*Sin(a3) - a1dd*l2*l3cm*m3*Sin(a1)*Sin(a3) - 2*a2dd*l2*l3cm*m3*Sin(a1)*Sin(a3) - a3dd*l2*l3cm*m3*Sin(a1)*Sin(a3) - a1dd*l1*l3cm*m3*Sin(a2)*Sin(a3) - G*l3cm*m3*Sin(a1)*Sin(a2)*Sin(a3) CForm[tau3] a1dd*I3 + a2dd*I3 + a3dd*I3 + a1dd*Power(l3cm,2)*m3 + a2dd*Power(l3cm,2)*m3 + a3dd*Power(l3cm,2)*m3 + a2dd*l2*l3cm*m3*Cos(a1)*Cos(a3) + a1dd*l1*l3cm*m3*Cos(a2)*Cos(a3) + Power(a2d,2)*l2*l3cm*m3*Cos(a3)*Sin(a1) + G*l3cm*m3*Cos(a2)*Cos(a3)*Sin(a1) + Power(a1d,2)*l1*l3cm*m3*Cos(a3)*Sin(a2) + G*l3cm*m3*Cos(a1)*Cos(a3)*Sin(a2) + Power(a2d,2)*l2*l3cm*m3*Cos(a1)*Sin(a3) + Power(a1d,2)*l1*l3cm*m3*Cos(a2)*Sin(a3) + G*l3cm*m3*Cos(a1)*Cos(a2)*Sin(a3) - a2dd*l2*l3cm*m3*Sin(a1)*Sin(a3) - a1dd*l1*l3cm*m3*Sin(a2)*Sin(a3) - G*l3cm*m3*Sin(a1)*Sin(a2)*Sin(a3) (**************************************************************) CForm[a] I1 + I2 + I3 + Power(l1cm,2)*m1 + Power(l1,2)*m2 + Power(l2cm,2)*m2 + Power(l1,2)*m3 + Power(l3cm,2)*m3 + 2*l1*l2cm*m2*Cos(a2) + l1*l2*m3*Cos(a1)*Cos(a2) + l2*l3cm*m3*Cos(a1)*Cos(a3) + 2*l1*l3cm*m3*Cos(a2)*Cos(a3) + l1*l2*m3*Sin(a1)*Sin(a2) - l2*l3cm*m3*Sin(a1)*Sin(a3) - 2*l1*l3cm*m3*Sin(a2)*Sin(a3) CForm[b] I2 + I3 + Power(l2cm,2)*m2 + Power(l2,2)*m3 + Power(l3cm,2)*m3 + l1*l2cm*m2*Cos(a2) + l1*l2*m3*Cos(a1)*Cos(a2) + 2*l2*l3cm*m3*Cos(a1)*Cos(a3) + l1*l3cm*m3*Cos(a2)*Cos(a3) + l1*l2*m3*Sin(a1)*Sin(a2) - 2*l2*l3cm*m3*Sin(a1)*Sin(a3) - l1*l3cm*m3*Sin(a2)*Sin(a3) CForm[c] I3 + Power(l3cm,2)*m3 + l2*l3cm*m3*Cos(a1)*Cos(a3) + l1*l3cm*m3*Cos(a2)*Cos(a3) - l2*l3cm*m3*Sin(a1)*Sin(a3) - l1*l3cm*m3*Sin(a2)*Sin(a3) CForm[Expand[tau1 - a1dd*a - a2dd*b - a3dd*c]] G*l1cm*m1*Sin(a1) + G*l1*m2*Sin(a1) + G*l1*m3*Sin(a1) + G*l2cm*m2*Cos(a2)*Sin(a1) - Power(a1d,2)*l1*l2*m3*Cos(a2)*Sin(a1) + Power(a2d,2)*l1*l2*m3*Cos(a2)*Sin(a1) - Power(a1d,2)*l2*l3cm*m3*Cos(a3)*Sin(a1) - 2*a1d*a2d*l2*l3cm*m3*Cos(a3)*Sin(a1) - 2*a1d*a3d*l2*l3cm*m3*Cos(a3)*Sin(a1) - 2*a2d*a3d*l2*l3cm*m3*Cos(a3)*Sin(a1) - Power(a3d,2)*l2*l3cm*m3*Cos(a3)*Sin(a1) + G*l3cm*m3*Cos(a2)*Cos(a3)*Sin(a1) - 2*a1d*a2d*l1*l2cm*m2*Sin(a2) - Power(a2d,2)*l1*l2cm*m2*Sin(a2) + G*l2*m3*Sin(a2) + G*l2cm*m2*Cos(a1)*Sin(a2) + Power(a1d,2)*l1*l2*m3*Cos(a1)*Sin(a2) - Power(a2d,2)*l1*l2*m3*Cos(a1)*Sin(a2) - 2*a1d*a2d*l1*l3cm*m3*Cos(a3)*Sin(a2) - Power(a2d,2)*l1*l3cm*m3*Cos(a3)*Sin(a2) - 2*a1d*a3d*l1*l3cm*m3*Cos(a3)*Sin(a2) - 2*a2d*a3d*l1*l3cm*m3*Cos(a3)*Sin(a2) - Power(a3d,2)*l1*l3cm*m3*Cos(a3)*Sin(a2) + G*l3cm*m3*Cos(a1)*Cos(a3)*Sin(a2) - Power(a1d,2)*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a1d*a2d*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a1d*a3d*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a2d*a3d*l2*l3cm*m3*Cos(a1)*Sin(a3) - Power(a3d,2)*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a1d*a2d*l1*l3cm*m3*Cos(a2)*Sin(a3) - Power(a2d,2)*l1*l3cm*m3*Cos(a2)*Sin(a3) - 2*a1d*a3d*l1*l3cm*m3*Cos(a2)*Sin(a3) - 2*a2d*a3d*l1*l3cm*m3*Cos(a2)*Sin(a3) - Power(a3d,2)*l1*l3cm*m3*Cos(a2)*Sin(a3) + G*l3cm*m3*Cos(a1)*Cos(a2)*Sin(a3) - G*l3cm*m3*Sin(a1)*Sin(a2)*Sin(a3) d = Coefficient[tau2,a1dd] e = Coefficient[tau2,a2dd] f = Coefficient[tau2,a3dd] CForm[d] I2 + I3 + Power(l2cm,2)*m2 + Power(l3cm,2)*m3 + l1*l2cm*m2*Cos(a2) + l1*l2*m3*Cos(a1)*Cos(a2) + l2*l3cm*m3*Cos(a1)*Cos(a3) + l1*l3cm*m3*Cos(a2)*Cos(a3) + l1*l2*m3*Sin(a1)*Sin(a2) - l2*l3cm*m3*Sin(a1)*Sin(a3) - l1*l3cm*m3*Sin(a2)*Sin(a3) CForm[e] I2 + I3 + Power(l2cm,2)*m2 + Power(l2,2)*m3 + Power(l3cm,2)*m3 + 2*l2*l3cm*m3*Cos(a1)*Cos(a3) - 2*l2*l3cm*m3*Sin(a1)*Sin(a3) CForm[f] I3 + Power(l3cm,2)*m3 + l2*l3cm*m3*Cos(a1)*Cos(a3) - l2*l3cm*m3*Sin(a1)*Sin(a3) CForm[Expand[tau2 - a1dd*d - a2dd*e - a3dd*f]] G*l2cm*m2*Cos(a2)*Sin(a1) - Power(a1d,2)*l1*l2*m3*Cos(a2)*Sin(a1) - Power(a1d,2)*l2*l3cm*m3*Cos(a3)*Sin(a1) - 2*a1d*a2d*l2*l3cm*m3*Cos(a3)*Sin(a1) - 2*a1d*a3d*l2*l3cm*m3*Cos(a3)*Sin(a1) - 2*a2d*a3d*l2*l3cm*m3*Cos(a3)*Sin(a1) - Power(a3d,2)*l2*l3cm*m3*Cos(a3)*Sin(a1) + G*l3cm*m3*Cos(a2)*Cos(a3)*Sin(a1) + Power(a1d,2)*l1*l2cm*m2*Sin(a2) + G*l2*m3*Sin(a2) + G*l2cm*m2*Cos(a1)*Sin(a2) + Power(a1d,2)*l1*l2*m3*Cos(a1)*Sin(a2) + Power(a1d,2)*l1*l3cm*m3*Cos(a3)*Sin(a2) + G*l3cm*m3*Cos(a1)*Cos(a3)*Sin(a2) - Power(a1d,2)*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a1d*a2d*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a1d*a3d*l2*l3cm*m3*Cos(a1)*Sin(a3) - 2*a2d*a3d*l2*l3cm*m3*Cos(a1)*Sin(a3) - Power(a3d,2)*l2*l3cm*m3*Cos(a1)*Sin(a3) + Power(a1d,2)*l1*l3cm*m3*Cos(a2)*Sin(a3) + G*l3cm*m3*Cos(a1)*Cos(a2)*Sin(a3) - G*l3cm*m3*Sin(a1)*Sin(a2)*Sin(a3) g = Coefficient[tau3,a1dd] h = Coefficient[tau3,a2dd] i = Coefficient[tau3,a3dd] CForm[g] I3 + Power(l3cm,2)*m3 + l1*l3cm*m3*Cos(a2)*Cos(a3) - l1*l3cm*m3*Sin(a2)*Sin(a3) CForm[h] I3 + Power(l3cm,2)*m3 + l2*l3cm*m3*Cos(a1)*Cos(a3) - l2*l3cm*m3*Sin(a1)*Sin(a3) CForm[i] I3 + Power(l3cm,2)*m3 CForm[Expand[tau3 - a1dd*g - a2dd*h - a3dd*i]] Power(a2d,2)*l2*l3cm*m3*Cos(a3)*Sin(a1) + G*l3cm*m3*Cos(a2)*Cos(a3)*Sin(a1) + Power(a1d,2)*l1*l3cm*m3*Cos(a3)*Sin(a2) + G*l3cm*m3*Cos(a1)*Cos(a3)*Sin(a2) + Power(a2d,2)*l2*l3cm*m3*Cos(a1)*Sin(a3) + Power(a1d,2)*l1*l3cm*m3*Cos(a2)*Sin(a3) + G*l3cm*m3*Cos(a1)*Cos(a2)*Sin(a3) - G*l3cm*m3*Sin(a1)*Sin(a2)*Sin(a3)