%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear format short %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% VARIABLES %joint variables syms q1 q2 q3 assume(q1,'real') assume(q2,'real') assume(q3,'real') %link lengths syms a2 a3 assume(a2,'real') assume(a3,'real') %COM syms com1x com1y com1z syms com2x com2y com2z syms com3x com3y com3z assume(com1x,'real') assume(com1y,'real') assume(com1z,'real') assume(com2x,'real') assume(com2y,'real') assume(com2z,'real') assume(com3x,'real') assume(com3y,'real') assume(com3z,'real') %masses syms m1 m2 m3 assume(m1,'real') assume(m2,'real') assume(m3,'real') %links inertia syms Ixx1 Iyy1 Izz1 syms Ixy1 Ixz1 Iyz1 assume(Ixx1,'real') assume(Iyy1,'real') assume(Izz1,'real') assume(Ixy1,'real') assume(Ixz1,'real') assume(Iyz1,'real') syms Ixx2 Iyy2 Izz2 syms Ixy2 Ixz2 Iyz2 assume(Ixx2,'real') assume(Iyy2,'real') assume(Izz2,'real') assume(Ixy2,'real') assume(Ixz2,'real') assume(Iyz2,'real') syms Ixx3 Iyy3 Izz3 syms Ixy3 Ixz3 Iyz3 assume(Ixx3,'real') assume(Iyy3,'real') assume(Izz3,'real') assume(Ixy3,'real') assume(Ixz3,'real') assume(Iyz3,'real') Il11 = [Ixx1 -Ixy1 -Ixz1; -Ixy1 Iyy1 -Iyz1; -Ixz1 -Iyz1 Izz1] Il22 = [Ixx2 -Ixy2 -Ixz2; -Ixy2 Iyy2 -Iyz2; -Ixz2 -Iyz2 Izz2] Il33 = [Ixx3 -Ixy3 -Ixz3; -Ixy3 Iyy3 -Iyz3; -Ixz3 -Iyz3 Izz3] %Inertia matrix syms b11 b12 b13 syms b21 b22 b23 syms b31 b32 b33 assume(b11,'real') assume(b12,'real') assume(b13,'real') assume(b21,'real') assume(b22,'real') assume(b23,'real') assume(b31,'real') assume(b32,'real') assume(b33,'real') %joint velocities and accelerations syms qdot1 syms qdot2 syms qdot3 syms qdotdot1 syms qdotdot2 syms qdotdot3 assume(qdot1,'real') assume(qdot2,'real') assume(qdot3,'real') assume(qdotdot1,'real') assume(qdotdot2,'real') assume(qdotdot3,'real') %gravity syms g assume(g, 'real') %friction syms Bm assume(Bm, 'real') %gear ratios syms Gear assume(Gear, 'real') %torques syms tau1 syms tau2 syms tau3 assume(tau1,'real') assume(tau2,'real') assume(tau3,'real') %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% KINEMATICS %A matrices A01 = [cos(q1) 0 sin(q1) 0;... sin(q1) 0 -cos(q1) 0;... 0 1 0 0;... 0 0 0 1] A12 = [cos(q2) -sin(q2) 0 a2*cos(q2);... sin(q2) cos(q2) 0 a2*sin(q2);... 0 0 1 0;... 0 0 0 1] A23 = [cos(q3) -sin(q3) 0 a3*cos(q3);... sin(q3) cos(q3) 0 a3*sin(q3);... 0 0 1 0;... 0 0 0 1] A03 = A01*A12*A23 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% DIFFERENTIAL KINEMATICS p0 = [0 0 0 1]' p1 = A01*p0 p2 = A01*A12*p0 p3= A01*A12*A23*p0 z0 = [0 0 1]'; z1 = A01(1:3,1:3)*z0 z2 = A01(1:3,1:3)*A12(1:3,1:3)*z0 J = [ cross(z0,p3(1:3)-p0(1:3)), cross(z1,p3(1:3)-p1(1:3)), cross(z2,p3(1:3)-p2(1:3));... z0 z1 z2] J = simplify(J) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% INERTIA MATRIX pl1 = A01*[com1x, com1y, com1z, 1]'; pl2 = A01*A12*[com2x, com2y, com2z, 1]'; pl3 = A01*A12*A23*[com3x, com3y, com3z, 1]'; JP_l1 = simplify([cross(z0,pl1(1:3)-p0(1:3)), [0 0 0]', [0 0 0]']); JP_l2 = simplify([cross(z0,pl2(1:3)-p0(1:3)), cross(z1,pl2(1:3)-p1(1:3)), [0 0 0]']); JP_l3 = simplify([cross(z0,pl3(1:3)-p0(1:3)), cross(z1,pl3(1:3)-p1(1:3)), ... cross(z2,pl3(1:3)-p2(1:3))]); Jtheta_l1 = [z0, [0 0 0]', [0 0 0]']; Jtheta_l2 = [z0, z1, [0 0 0]']; Jtheta_l3 = [z0, z1, z2]; B1P = simplify(m1*JP_l1'*JP_l1); B2P = simplify(m2*JP_l2'*JP_l2); B3P = simplify(m3*JP_l3'*JP_l3); R1 = A01(1:3,1:3); Il1 = simplify(R1*Il11*R1'); R2 = A01(1:3,1:3)*A12(1:3,1:3); Il2 = simplify(R2*Il22*R2'); R3 = A01(1:3,1:3)*A12(1:3,1:3)*A23(1:3,1:3); Il3 = simplify(R3*Il33*R3'); B1T = simplify(Jtheta_l1'*Il1*Jtheta_l1); B2T = simplify(Jtheta_l2'*Il2*Jtheta_l2); B3T = simplify(Jtheta_l3'*Il3*Jtheta_l3); B = simplify(B1P+B2P+B3P+B1T+B2T+B3T) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% CORIOLIS MATRIX b11 = B(1,1) b12 = B(1,2) b13 = B(1,3) b21 = B(2,1) b22 = B(2,2) b23 = B(2,3) b31 = B(3,1) b32 = B(3,2) b33 = B(3,3) %Function cijk(bij, bik, bjk, qi, qj, qk) computes: %cijk = simplify(0.5*(diff(bij,qk) + diff(bik,qj)-diff(bjk,qi))) %cijk = cikj %c111 = simplify(0.5*(diff(b11,q1) + diff(b11,q1)-diff(b11,q1))) c111 = cijk(b11, b11, b11, q1, q1, q1) c112 = cijk(b11, b12, b12, q1, q1, q2) c113 = cijk(b11, b13, b13, q1, q1, q3) c121 = c112 c122 = cijk(b12, b12, b22, q1, q2, q2) c123 = cijk(b12, b13, b23, q1, q2, q3) c131 = c113 c132 = c123 c133 = cijk(b13, b13, b33, q1, q3, q3) c211 = cijk(b21, b21, b11, q2, q1, q1) c212 = cijk(b21, b22, b12, q2, q1, q2) c213 = cijk(b21, b23, b13, q2, q1, q3) c221 = c212 c222 = cijk(b22, b22, b22, q2, q2, q2) c223 = cijk(b22, b23, b23, q2, q2, q3) c231 = c213 c232 = c223 c233 = cijk(b23, b23, b33, q2, q3, q3) c311 = cijk(b31, b31, b11, q3, q1, q1) c312 = cijk(b31, b32, b12, q3, q1, q2) c313 = cijk(b31, b33, b13, q3, q1, q3) c321 = c312 c322 = cijk(b32, b32, b22, q3, q2, q2) c323 = cijk(b32, b33, b23, q3, q2, q3) c331 = c313 c332 = c323 c333 = cijk(b33, b33, b33, q3, q3, q3) c11 = c111*qdot1+c112*qdot2+c113*qdot3 c12 = c121*qdot1+c122*qdot2+c123*qdot3 c13 = c131*qdot1+c132*qdot2+c133*qdot3 c21 = c211*qdot1+c212*qdot2+c213*qdot3 c22 = c221*qdot1+c222*qdot2+c223*qdot3 c23 = c231*qdot1+c232*qdot2+c233*qdot3 c31 = c311*qdot1+c312*qdot2+c313*qdot3 c32 = c321*qdot1+c322*qdot2+c323*qdot3 c33 = c331*qdot1+c332*qdot2+c333*qdot3 C = [c11 c12 c13; c21 c22 c23; c31 c32 c33] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% GRAVITATIONAL TERM g0 = [0 0 -g]' g1 = simplify(-m1*g0'*JP_l1(:,1) - m2*g0'*JP_l2(:,1) - m3*g0'*JP_l3(:,1)) g2 = simplify(-m1*g0'*JP_l1(:,2) - m2*g0'*JP_l2(:,2) - m3*g0'*JP_l3(:,2)) g3 = simplify(-m1*g0'*JP_l1(:,3) - m2*g0'*JP_l2(:,3) - m3*g0'*JP_l3(:,3)) G = [g1 g2 g3]' %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% FRICTION % no Coulomb friction is considered for this robot F = [Bm Bm Bm].*[Gear^2 Gear^2 Gear^2] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% VERIFICATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% THE MODEL %link lengths va1 = 0.5; va2 = 1.0; va3 = 0.5; %masses vm1 = 10; vm2 = 5; vm3 = 1; %center of masses vcom1x = 0; vcom1y = 0; vcom1z = 0; vcom2x = -va2/2; vcom2y = 0; vcom2z = 0; vcom3x = -va3/2; vcom3y = 0; vcom3z = 0; %link radius vr1 = 0.10; vr2 = 0.05; vr3 = 0.05; %friction vF = 0.001 %Gear ratios vGear = 50 %link inertia (cylindrical links assumed) %http://scienceworld.wolfram.com/physics/MomentofInertiaCylinder.html vIxx1 = (1/12)*vm1*(3*vr1^2+va1^2); vIyy1 = (1/2)*vm1*vr1^2; vIzz1 = (1/12)*vm1*(3*vr1^2+va1^2); vIxy1 = 0; vIxz1 = 0; vIyz1 = 0; vIxx2 = (1/2)*vm2*vr2^2; vIyy2 = (1/12)*vm2*(3*vr2^2+va2^2); vIzz2 = (1/12)*vm2*(3*vr2^2+va2^2); vIxy2 = 0; vIxz2 = 0; vIyz2 = 0; vIxx3 = (1/2)*vm3*vr3^2; vIyy3 = (1/12)*vm3*(3*vr3^2+va3^2); vIzz3 = (1/12)*vm3*(3*vr3^2+va3^2); vIxy3 = 0; vIxz3 = 0; vIyz3 = 0; %the links L(1) = Link([0 0 0 pi/2]); L(2) = Link([0 0 a2 0]); L(3) = Link([0 0 a3 0]); %the manipultor: rob = SerialLink(L, 'name', 'anthropomorphic arm') %dynmic data of the links rob.links(1).m = vm1; rob.links(2).m = vm2; rob.links(3).m = vm3; rob.links(1).I = [vIxx1 0 0; 0 vIyy1 0;0 0 vIzz1] rob.links(2).I = [vIxx2 0 0; 0 vIyy2 0;0 0 vIzz2] rob.links(3).I = [vIxx3 0 0; 0 vIyy3 0;0 0 vIzz3] rob.links(1).G = vGear; rob.links(2).G = vGear; rob.links(3).G = vGear; rob.links(1).B = vF; rob.links(2).B = vF; rob.links(3).B = vF; rob.links(1).r = [vcom1x vcom1y vcom1z]; rob.links(2).r = [vcom2x vcom2y vcom2z]; rob.links(3).r = [vcom3x vcom2y vcom3z]; rob.links(1).Jm = 0.0; rob.links(2).Jm = 0.0; rob.links(3).Jm = 0.0; %gravity vg = rob.gravity(3) %test configurations and test velocities qz = [0 0 0] qa = [pi/4 pi/4 pi/4] qb = [-pi/2 -pi/4 pi/2] q_dot = [0.1, 0.1 0.1] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% direct kinematics verification rob.links(2).a = va2; rob.links(3).a = va3; A03 = subs(A03,{a2 a3},{va2 va3}) T=rob.fkine(qz) verifyFK = T.inv*SE3(subs(A03,{q1 q2 q3},{qz})) double(verifyFK.T) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% inertia matrix verification vB = subs(B,{Ixx1,Iyy1,Izz1,Ixy1,Ixz1,Iyz1, ... Ixx2,Iyy2,Izz2,Ixy2,Ixz2,Iyz2, ... Ixx3,Iyy3,Izz3,Ixy3,Ixz3,Iyz3, ... com1x, com1y,com1z, ... com2x, com2y,com2z, ... com3x, com3y,com3z, ... m1, m2, m3, ... a2, a3}, ... {vIxx1,vIyy1,vIzz1,vIxy1,vIxz1,vIyz1, ... vIxx2,vIyy2,vIzz2,vIxy2,vIxz2,vIyz2, ... vIxx3,vIyy3,vIzz3,vIxy3,vIxz3,vIyz3, ... vcom1x, vcom1y, vcom1z, ... vcom2x, vcom2y, vcom2z, ... vcom3x, vcom3y, vcom3z, ... vm1, vm2, vm3, ... va2, va3}) B1z = rob.inertia(qz) B2z = double(subs(vB,{q1, q2, q3}, {qz(1) qz(2) qz(3)})) B1a = rob.inertia(qa) B2a = double(subs(vB,{q1, q2, q3}, {qa(1) qa(2) qa(3)})) B1b = rob.inertia(qb) B2b = double(subs(vB,{q1, q2, q3}, {qb(1) qb(2) qb(3)})) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Coriolis matrix verification vC = subs(C,{Ixx1,Iyy1,Izz1,Ixy1,Ixz1,Iyz1, ... Ixx2,Iyy2,Izz2,Ixy2,Ixz2,Iyz2, ... Ixx3,Iyy3,Izz3,Ixy3,Ixz3,Iyz3, ... com1x, com1y,com1z, ... com2x, com2y,com2z, ... com3x, com3y,com3z, ... m1, m2, m3, ... a2, a3}, ... {vIxx1,vIyy1,vIzz1,vIxy1,vIxz1,vIyz1, ... vIxx2,vIyy2,vIzz2,vIxy2,vIxz2,vIyz2, ... vIxx3,vIyy3,vIzz3,vIxy3,vIxz3,vIyz3, ... vcom1x, vcom1y, vcom1z, ... vcom2x, vcom2y, vcom2z, ... vcom3x, vcom3y, vcom3z, ... vm1, vm2, vm3, ... va2, va3}) C1z = rob.coriolis(qz, q_dot) C2z = double(subs(vC,{q1, q2, q3, qdot1, qdot2, qdot3}, ... {qz(1), qz(2), qz(3), q_dot(1), q_dot(2), q_dot(3)})) C1a = rob.coriolis(qa, q_dot) C2a = double(subs(vC,{q1, q2, q3, qdot1, qdot2, qdot3}, ... {qa(1), qa(2), qa(3), q_dot(1), q_dot(2), q_dot(3)})) C1b = rob.coriolis(qb, q_dot) C2b = double(subs(vC,{q1, q2, q3, qdot1, qdot2, qdot3}, ... {qb(1), qb(2), qb(3), q_dot(1), q_dot(2), q_dot(3)})) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Gravitational term verification vG = subs(G, {com1x, com1y,com1z, ... com2x, com2y,com2z, ... com3x, com3y,com3z, ... m1, m2, m3, ... a2, a3, g}, ... {vcom1x, vcom1y, vcom1z, ... vcom2x, vcom2y, vcom2z, ... vcom3x, vcom3y, vcom3z, ... vm1, vm2, vm3, ... va2, va3, vg}) G1z = rob.gravload(qz)' G2z = double(subs(vG, {q1 q2 q3},qz)) G1a = rob.gravload(qa)' G2a = double(subs(vG, {q1 q2 q3},qa)) G1b = rob.gravload(qb)' G2b = double(subs(vG, {q1 q2 q3},qb)) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Test the friction terms F1 = rob.friction(q_dot) F2 = double(-subs(F,{Bm,Gear},{vF,vGear}).*q_dot)