PROPAGATE A GAUSSIAN ERROR DISTRIBUTION OF EACH JOINT TO AN ERROR IN END EFFECTORS' POSITION AN ELLIPSE IS DRAWN TO REPRESENT THE ERROR IN POSITION
0001 % PROPAGATE A GAUSSIAN ERROR DISTRIBUTION OF EACH JOINT TO AN ERROR IN 0002 % END EFFECTORS' POSITION 0003 % AN ELLIPSE IS DRAWN TO REPRESENT THE ERROR IN POSITION 0004 0005 % Copyright (C) 2012, by Arturo Gil Aparicio 0006 % 0007 % This file is part of ARTE (A Robotics Toolbox for Education). 0008 % 0009 % ARTE is free software: you can redistribute it and/or modify 0010 % it under the terms of the GNU Lesser General Public License as published by 0011 % the Free Software Foundation, either version 3 of the License, or 0012 % (at your option) any later version. 0013 % 0014 % ARTE is distributed in the hope that it will be useful, 0015 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0016 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0017 % GNU Lesser General Public License for more details. 0018 % 0019 % You should have received a copy of the GNU Leser General Public License 0020 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0021 0022 %load arm parameters 0023 robot= load_robot('example','scara') 0024 0025 %find errors around pose 0026 q=[pi/4 3*pi/4 0 0]; 0027 0028 %Matriz de errores en las articulaciones 0029 % sigmaq1=sigmaq2=0.0017 rad, sigmaq3=0.01 m. 0030 sigmaq1=0.017;%rad, 0.01 grados 0031 sigmaq2=0.017;%rad 0032 sigmaq3=0.01;% m 0033 Rq=[sigmaq1^2 0 0; 0034 0 sigmaq2^2 0; 0035 0 0 sigmaq3^2] 0036 0037 teta = eval(robot.DH.theta); 0038 d = eval(robot.DH.d); 0039 a = eval(robot.DH.a); 0040 alfa = eval(robot.DH.alpha); 0041 0042 Jq = eval(robot.J) 0043 0044 0045 Rp=Jq*Rq*Jq' 0046 0047 T=directkinematic(robot, q); 0048 0049 drawrobot3d(robot,q), hold on 0050 draw_ellipse([T(1,4),T(2,4)], Rp, 'r')