Mechanical Engineering Matlab Exercises

Craig Robotics Text Matlab Exercises - Solutions Outline 2а) Orthonormal rotation matrices 2Ь) Homogeneous transformation matrices 3) Forward pose kinematics - Planar ЗR 4) lnverse pose k:inematics - Planar ЗR 5) JacoЬian, determinant, resolved-rate, inverse statics - Planar ЗR ба) Jnverse dynamics simulation - Planar 2R 6Ь) Inverse dynamics snapshot - Planar ЗR 6с) Forward dynamics trajectory- Planar ЗR 7) Trajectory generation - З'', З"' \V/ via, 5 th 8) Resolved-rate with kinematic redundancy - Planar 4R, particular only 9) Single joint control Simulink simulation All Matlab solutions were developed оп а РС running WindO\VS 98 and Matlab R12, and the Ьеtа version 7 of the Corke RoЬotics ТооlЬох . However, these exercises and solutions should Ье general for other platforms, operating systcms, and soft,vare version numЬers (these versions or ne,ver). Matlab Exercise 2А а) i) Giveп а= 10'. /3= 20', and j r = 30° ; j 0.9254 0.0180 0.3785 :R= [ 0.1632 0.8826 -0.44!0] -0.3420 0.4698 0.8138 Column-wise orthonormal demonstration (row-wise is similar): 0.9254 0.0180 0.3785 A -0.4410 = )= Z 8 лх. 8 х У8 = 0.1632 ] х 1 0.8826] 0.8138 -0.3420 0.4698 А jAxвll= j 0.9254 0.1632 ) =1 -0.3420 (3 scalar coпstraints) 0.0180 0.8826 ) =1 1 II fв l= 0.4698 A l 0.3785 -0.4410) =1 � IIAzвll= 0.8138 (3 scalar constraints) 0.9254 0.1632 -0.3420 fR=inv(;R)= [0.0180 0.8826 0.4698 ]=:R т 0.3785 -0.4410 0.8138 ii) Ь) i) Giveп а= 30', /3= 90° , and -0.9%2 0.0872 0.0872 0.9962] r = -55 ° : о 0.9254 0.0180 0.3785 Giveп :R= [ 0.1632 0.8826 -0.4410] ., -0.3420 0.4698 0.8138 Solution 1 2 а fJ 10' ° -170 20' ° 160 о r 30' -150 ° The secoпd solutioп is not really needed -just included as an exercise. Plug the second solution row into the а. code and it yields the same rotation matrix (circular check). ii) Given ЙR=[� -1 -0.9%2 0.0872 0.0872 0.9962 ] Solution 1 2 о о о а у ° зо· ° -150 ° -55 ° 125 90 ° 90 Again, plug the second solution ro,v into the а. code and it yields the same rotation matrix (circular ° check). This case is singular (artificial singularity) since /J = 90 and the inverse solution must divide Ьу cos /J. Therefore, one should implement the alternate artificial singularity solution, equation (2.67). However, in my solution, this was not necessary - 1 guess the cos /J terrns ,vere enough different from О to allow the Matlab atan function to work properly. с) Given а=о•, /3=20 ° ,and r=o· also 8Р={1 О 1У; 20 l 2 l7 ; 1п J i 1 ] а а.9397 1 0.5977 � Sketch: О.з уА.В 0.5977 Л l ZA d) Check alJ results with the Corke Matlab RoЬotics ТооlЬох. % Exercise 2А using the Corke Matlab Aobotics Toolbox % Dr. ВоЬ Williams, Ohio University, 2/2002 clear; clc; DA = pi/180; angs alp Ьеt gam = input('enter alpha, beta, gamma (deg): '); = angs(1)*DA; = angs(2)*DA; = angs(3)*DA; Tz Ту Тх = rotz(alp); = roty(bet); = rotx(gam); Az = Tz(1 :3, 1 :3); Ау= Ту(1 :3, 1 :3); Ах= Тх(1:3,1:3); % Parta Amult = Az•Ay•Ax; Trpy = rpy2tr(alp,bet,gam) Rrpy = Trpy(1 :3, 1 :3) % Z-Y-X a-b-g Euler angles a'la Craig % Same result - rpy corresponds to Z-Y-X a-b-g Euler % Extract 3х3 rotation matrix from 4х4 % Partb = tr2rpy(Тrpy); ang ang/DR % lnverse solution - гру corresponds to Z-Y-X a-b-g Euler % Display inverse sol ution results % Partc = input('enter Ьеtа (deg): '); beta РЬ = input('enter [РЬ] (column-wise): '); = roty(beta·DR); Ryc = Тус(1 :3, 1 :3); Туе Ра = Ryc'Pb; Using this Matlab program, the answers check for part а. The function rpy2tr() corresponds to the Z-Y-X a-�r Euler convention; i.e. roll-pitch-yaw is the same convention. The result returned is а 4х4 homogeneous transformation matrix \vith zeros in the translation vector column, so we extract the 3х3. Also using this program, the inverse results check for part Ь. OnJy one solution is given, the second possibllity is ignored. Also, case ii is а singular case, so the result in this case is: Solution а fJ 90 ° r -85 ° Upon plugging this singular solution into the for\vard function rpy2tr(), tbe correct rotation matrix results. The result for part с also agrees with the answer given аЬоvе. Matlab Exercise 2В а) i) Given а= 10•, /3 = 20•, and у= зо• and л Р8 = {1 2 3у: 0.9254 0.0180 0.3785 А т- l 0.1632 0.8826 -0.4410 8 - -0.3420 0.4698 0.8138 ii) о о о ° Given /3=20° (a=y=0 ),and Арв ={з О l}т : О 9397 1: О 0.3420 3 �Т= l -0�420 � 0.9�97 О о 1 О 9397 о о 1 1 �11� О 0.3420 0 1 0 • = А р=А Т ВР в - 0.3420 О 0.9397 1 о о о 1 4 2i 171 • l = l.5977 1 l 3 Interpretations for Homogeneous Transfonnatio11 Matrices: 1} Description of а frame 1т descriЬes the pose (position and orientation) of moving Cartesian coordinate frame {В}, ,v.r.t. to а reference frame, {А}. {А Р8 } is the position vector giving the location of the origin of {В} lй j w.r.t. the origin оГ {А}, expressed i11 the basis (coordinates) of {А}. R is the rotatio11 matrix giving the orientation of {В} w.r.t. {Л}; colurnns are the ХУ2 unit vectors of {В} projected onto the ХУ2 {А} u11it directions. Figure for first interpretation: 3 2) Transfonn Mapping: 8 Matrix �Т maps Р➔ АР. DescriЬes а vector known in one Cartesian coordinate frame {В} in another frame {А}. There is Ьoth position and orientation (Ьasis) difference in general. The л Р=�Т 8Р math has already Ьееn done аЬоvе immediately following Ь. Figure for second interpretatioп: Zв - 00 � .._,: х, У, з Z, 1.5977 3) Transform Operator: Т operates оп А Pi to yield А Р2. Same Cartesian coordinate frame {А}, there is по {В} for this interpretation. The original vector А Р1 is translated and rotated to ne\V vector А Pz via Т. Order of translation and rotation doesn't rnatter if we assume rotations al\vays occur aЬout the tail of vectors Let . ususethesamegivennumЬers,i.e. /3=20' (a=y=0'),and Р={3 О 1Y,and Ар1 ={! О !}г : А А Р2 =Т Р1 = r 0.9397 0 -0.3420 о о 1 о о 0.3420 о 0.9397 о Figure for third interpretation: 7 . 2i 1 4 1 11�1 1 �1 1 Translated and Rotated У ector 3 1.5977 =1 1.5977 1 ] с) d) i) Both methods (symЬolic formula, Matlab function inv) yield: ii) Both methods (symЬolic formula, Matlab function inv) yield: i) Calculate 0.9254 0.1632 -0.3420 -0.2257 А Т -\= [0.0180 0.8826 0.4698 -3.19271 8 0.3785 -0.4410 0.8138 -1.9380 о 1 о о 0.9397 О -0.3420 -2.�771 0 1 А Т -\ - [ 0 1 8 0.3420 О 0.9397 -1.9658 1 о о о &' and show the relationship via а transform graph. �j[ Ditto for �Т . 0.9254 0.0180 0.3785 0.9i 97 о 0.3420 1 о 0.1632 0.8826 -0.4410 �Т=1Т�Т= -0.3420 0.4698 0.8138 3 -О 3420 о 0.9397 1 о 1 О о о о 1 о 1 0 40 0.0180 0.6722 4.1548 7 0.8826 -0.3586 2.0486 0.3042 1 l . = -О.�997 0.4698 0.6477 2.7877 о о 1 0.7401 0.3042 -0.5997 -2.0263 0.8826 0.4698 -3.1927 0.0180 -1 А 1 В САу= СВ Т ВАТ= С Т Ву = [ 1 0.6722 -0.3586 0.6477 -3.8641 о о о 1 Transform graphs: в в с с А А ii) Л А С А В 8Т=сТ 8Т=сТ сТ iii) l Given tJ and �Т from d)i; assume you don't know �Т - calculate it and compare with the ans\ver you know. -l 0.9254 0.0180 0.3785 1 0.1632 0.8826 -0.4410 : = -0.3420 0.4698 0.8138 1] о о о l Given jr and �Т from d)i; assume you don't kno\v �Т - calculate it and compare with the answer you know. В В А -l А сТ=лТtг=вТ сТ= 0.9397 Q -0.3420 о е) Check all results with the Corke Matlab RoЬotics Тоо!Ьох. % Exercise 2В using the Corke Matlab Robotics Toolbox % Dr. ВоЬ Williams, Ohio University, 2/2002 clear; clc; DR = pi/180; % Parta aпgs = input('enter alpha, beta, gamma (deg): '); = aпgs(1 )'DR; alp bet = angs(2)'DR; gam = angs(З)"DR; РЬа = input('enter РЬа (column-wise): '); Trpy Ttrп ТЬа = rpy2tr(alp,bet,gam); = transl(Pba); = Ttrn'Тrpy; % Rotation matrix 4х4, Z-Y-X a-b-g Euler convention % Translation matrix 4х4 with РЬа % 4х4 Homogeneous transformation matrix ТЬа % Part Ь РЬ = input('enter РЬ (column-wise): '); = ТЬа'(РЬ;1]; Ра % Partc Tba_inv= inv(Tba); еуе(4) - Tba'Tba_inv; еуе(4) - Tba_inv'Tba; % lnverse homogeneous transformation matrix - Matlab function % Check - should Ье zero % Check - should Ье zero % Partd angs2 = input('enter alpha2, beta2, gamma2 (deg): '); alp2 = angs2(1)'DR; Ьеt2 = angs2(2)'DA; gam2 = angs2(3)'DR; РсЬ = input('enter РсЬ (column-wise): '); Trpy2 = rpy2tr(alp2,bet2,gam2); Ttm2 = transl(Pcb); = Ttm2'Тrpy2; ТсЬ % Rotation matrix 4х4, Z-Y-X a-b-g Euler convention % Translation matrix 4х4 with РсЬ % 4х4 Homogeneous transformation matrix ТсЬ Тса Тае Тас2 =ТЬа • ТсЬ = inv(Тcb) • inv(Tba) = inv(Tca) % Parld.i ТЬа2 = Тса • inv(Tcb) % Partd.ii ТсЬ2 = inv(Tba) • Тса % Partd.iii % Check - should Ье same as Тае Using t.his Matlab program, the answers check for all parts. We use functions rpy2tr(.) and transl() to build the overaJI homogeneous transformation matrices. There is по special toolЬox function to invert homogeneous transformation matrices so \Ve use the Matlab function inv(.). Matlab Exercise 3 а) ОН parameters: i ан о о о 1 2 з Ь) 0; d,- а;-1 о о о о L1 L2 01 02 03 NeighЬoring homogeneous transformation matrices; Also �т and Дт: � �1 -S3 С3 о о 1 О О 1 о о 1 О О 1 о о с) Forward pose kinematics transformation: [зт]= [�т(01 )l1т(02)l�т(0з)]= rCl23 [Ят]= [�т(01 , 02, 0з)lдт(Lз)]= - S12 3 С123 5 3 о о 1i i r C l 23 5 3 - S12 3 С123 о о о о о о о 1 о L1 c 1 + L2 c 12 L1s1 +L2s12 о 1 1 Lici +L2c12 +Lзст L1 s 1 +L2 s 2 +Lзs12з where: s123 = sin(01 + 02 +03), с123 = cos(01 + 02 + 03), etc. � 1 i) о Т= н Sketch to verify: l о l � Хз.н From this dra\ving, the аЬоvе values сап Ье derived Ьу inspection (position and orientation). ii) 0=�о· Г 20· зо· 0.5 -0.866 О 6.5373 о 2.1946� 0.6 0.866 0 зТ= [ о 1 о о о о 1 0 нТ= Sketch to verify: 5 . -0.866 0 0.866 0.6 l о о о о о о 7.5373 3.92661 о 1 < уо From this dra\ving, the аЬоvе values сап Ье derived Ьу inspection (position and orientation). iii) 0=fю' 90 ° 90' }т о gт= 1 о о о о о 1 о о о -1 Sketch to verify: �j L2 Lз 1 о о нТ= � о о о о о l l Уз Ун -r1 о Уо L1 Хо Хз,н From this dra\ving, the аЬоvе values сап Ье derived Ьу inspection (position and orientation). d) Check all results with the Corke Matlab RoЬotics ТооlЬох. % Exercise З using the Corke Matlab Robotics Toolbox % Dr. ВоЬ Williams, Ohio University, 2/2002 clc; clear; % Conslants DR = pi/1B0; L 1 = 4; L2 = З; LЗ = 2; % ОН parameters alp(1) = О; а(1) = О; d(1) = О; th(1) = О; alp(2} = О; а(2) = L1; d(2) = О; th(2) = О; аlр(З) = О; а(З) = L2; d(З) = О; th(З) = О; L{ 1} = link([alp(1 ),а(1 ),th(1 ),d(1),0),'mod'); L{2} = link([alp(2),a(2),th(2},d(2),0],'mod'}; L{З} = link([alp(З),a(З),th(З),d(З),0],'mod'); ThreeR = roЬot(L, 'PlanЗR'); % R joints and Craig DH convention % Create ЗR robot oЬject q1 = [О О 0]*DR; q2 = [10 20 З0]*DR; qЗ = [90 90 90]*DR; % Forward Pose Кinematics тзо_1 = fkine(ТhreeR,q1 ); Т30_2 = fkine(ThreeR,q2); ТЗО_З = fkine(ThreeR,qЗ); % тзо ТНЗ = [1 О О LЗ;О 1 О О;О О 1 О;О О О 1]; ТНО_1 = ТЗО_1 • ТНЗ; ТНО_2 = ТЗО_2 • ТНЗ; тно_з = тзо_з • тнз; % ТНО Using this Matlab program, the answers check for all parts. We use function link() to assign the Craig-convention ('mod' for modi.fied, or Craig-convention) DH parameters for each link. Then we build the Planar 3R roЬot object using function robot(). Finally, the basic forward pose transformation �Т is found with function fkine(). There is no special toolЬox fuпction to find manner as Ьefore. �т so we do it in the same Matlab Exercise 4 а) The Ьооk prese11ts three methods to solve this ргоЫеm; I favor the tangent-half angle algebraic approach, but all three will yield identical results. There аге t\VO solution sets for {01 02 03}, elЬo\v down and elЬow up configurations in the plane of motion. Ь) i) l о о 1 оТ= о � о н о о r �j 1 о First calculate: Solution 01 Elbow Down EIЬow Uo ii) 5 -0.866 0. 0.6 0.866 0 нТ= О о о о r Solution EIЬow Down EIЬow Up o 1 оТ= -1 о О н � о r 02 о о о о 03 = о о о 7.5373 О 3.9266 1 I О ] 5 о -0.866 о 6.5373 0. о 2.1946 0.6 о о -1 0.866 1 3Т=нТ Н}r = о о о 1 о о 1 о First calculate: iii) �Т=ЛТ н3т-� о о 7 о l о о о о о о о о О о 1 о Solution EIЬow Down EIЬow Up r 01 02 10 ° 27.1' 20' ° -20 03 30' 52.9 ° First calculate: 01 02 90' 163.7 ° 90' - 90 ° 03 90' -163.7' 1 о О о О о 1 о iv) In this case, all joint angle solutions are imaginary; this means the solution does not exist. The commanded position vector is outside ot· the reachaЬ!e workspace of the roЬot. For cases i-iii, the Е!Ьоw Do\vn solution is the same as the Forward Pose Кinematics input in Exercise ЗА cases i-iii, \vhich validates the inverse pose solurion. The EIЬo\v Up solution checks out when plugged into the foнvard pose program, i.e. the commaпded �Т апd flт result. Note the Е!Ьоw Dowп and EIЬow Up cases are identical for case i, which means this case lies оп the solution Ьoundary Ьetween multiple solutions; this is the e!Ьow-straight singular case, i.e. the reachaЬ!e workspace Ьoundary. d) Check all results ,vith the Corke Matlab Rolюtics Toollюx. % Exercise 4 using the Corke Matlab Robotics Тоо!Ьох % Dr. ВоЬ Williams, Ohio University, 2/2002 clc; clear; Constants % DR = pi/180; L 1 = 4; L2 = 3; L3 = 2; % ОН parameters alp(1) = О; а(1) = О; d(1) = О; th(1) = О; alp(2) = О; а(2) = L 1; d(2) = О; th(2) = О; alp(3) = О; а(3) = L2; d(З) = О; th(3) = О; L{1} = link([alp(1),a(1),th(1),d(1),0],'mod'); L{2} = link([alp(2),a(2),th(2),d(2),0],'mod'); ЦЗ} = link([alp(З),a(З),th(З),d(3),0),'mod'); % R joints and Craig ОН convention ТhreeR = robot(L, 'Plan3R'); % Create 3R robot oЬject ТН3 = (1 О О L3;0 1 О О;О О 1 О;О О О1]; тно_1 = [1 О О 9;0 1 О О;О О 1 О;О О О 1 ]; Т30_ 1 = тно_1 • inv(ТH3); ТНО_2 = [0.5 -0.866 О 7.5373;0.866 0.5 О 3.9266;0 О 1 О;О О О 1]; Т30_2 = ТНО_2 • inv(TH3); тно_з = [О 1 о -3;-1 о о 2;0 о 1 о;о о о 1 J; Т30_3 = ТНО_3 • inv(THЗ); ТНО_4 = [0.866 0.5 0-3.1245;-0.5 0.866 О 9.1674;0 О 1 О;О О О 1]; тзо_4 = тно_4 • iпv(ТНЗ); % lnverse Pose Kinematics М=(11 0001]; % Mask for planar motion %Guess1 = (О О О]'ОА; Guess1 = (90 -90 90]'DR; 01 = ikine(ТhreeR, ТЗО_ 1, Guess1, М); 010 =O1/ОА %Guess2 = [О О О]'ОА; Guess2 = (20 -10 50]'0R; 02 = ikine(ТhreeR, ТЗО_2, Guess2, М); O2D =O2/ОА % For Elbow Oown % For Elbow Up %Guess3 = [70 60 80]*0R; GuessЗ = [150 -80 -150]'0А; аз = ikine(ТhreeR, ТЗО_З, GuessЗ, М); O3D = O3/DR % For Elbow Oown % For Elbow Up Guess4 = [О О 0]'OR; 04 = ikine(ТhreeR, тзо_4, Guess4, М); O4D=O4/DR Using this Matlab program, the answers check for all parts. We нsе the mask М to indicate planar motion only (.х апd у translation, rotation aЬout z). An initial guess is required for iterative function ikine(). For case i, the solution converges to the same result for Ьoth initial guesses shO\Vll, as e)(pected. For cases ii and iii, different initiaJ guesses were used to find the EIЬow Do\vn and EIЬo\v Up solutions, as shown. For case iv, there was no convergence to а solution. This is e)(pected since the commanded position is out of reach for the roЬot. The Mailab error message is displayed Ьelow: ??? Error using ==> ikine Solution wouldn1 converge Error in ==> C:\mydocs\matfiles\Corke\Bob4A.m Оп line 55 ==> 04 = ikine(ThreeR, ТЗО_4, Guess4, М);