% 10nov2013_7PM -- seems problem with some of the error setups, % insensitive to Dd for instance and theta T1err etc. % 01nov2013 -- the original plan calculated the first TCP with FKin with % no noise. Then, for subsequent iterations, the TCP was calculated with % noise for both FKin and Taylor. % Don't want this for 3D Printer. I want FKinetics to compute a point once with % no noise and then Monte Carlo alot of Taylor noise points about the % "truth - FKin" point. Subsequent Fkin points can be chosen with the same % process. For one point, the norm is needed or the 3D neighborhood at the % 3 STD point. This will be interpreted as the +- tolerance. If a large % number of space points are chosen then the statistics for all can be % rolled up to one tolerance. % 25oct2013 % 3D Printer Arm Error Analysis Project % Robert E. Nee %y and z resultys are confuzed function [del_X, del_Y, del_Z,delR2,P0,delXYZ_variance,delXYZ_std,estimatedIterations] = MonteCarloForwardKinematics_3DP(numberIterations, ... logging_fid, ... x,y,z, ... a1,a2,a3, ... d1,d2,d3, ... A1,A2,A3, ... t1,t2,t3, ... t1err,t2err,t3err, ... a1err,a2err,a3err, ... A1err,A2err,A3err, ... d1err,d2err,d3err, ... L3DP,estimatedIterations) global theta_angle_error alpha_angle_error ustep base_Dd global fidHtml DRy DRz TT X2 Y2 Z2 global a1err a2err a3err global RzRxAngleError %global numberPoints numberIterations currentNumberPoints currentNumberIterations %global Da1 Da2 Da3 Dd1 Dd2 Dd3 DA1 DA2 DA3 Dt1 Dt2 Dt3 % X1 = []; % Y1 = []; % Z1 = []; X2 = []; Y2 = []; Z2 = []; del_X = []; del_Y = []; del_Z = []; delR2 = []; % distance between "exact" TCP , i.e. P0 and all other TT computed points temp2 = []; % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % %%%%%%%%% Calculate these once %%%%%%%%%%% % %%% for Taylor expandsion approach %%%%%%%%% Ct1 = cos(t1);Ct2 = cos(t2);Ct3 = cos(t3); St1 = sin(t1);St2 = sin(t2);St3 = sin(t3); CA1 = cos(A1);CA2 = cos(A2);CA3 = cos(A3); SA1 = sin(A1);SA2 = sin(A2);SA3 = sin(A3); L1 = [ Ct1, -St1*CA1, St1*SA1, Ct1*(a1); St1, Ct1*CA1, -Ct1*SA1, St1*(a1); 0, SA1, CA1, d1; 0, 0, 0, 1]; % L2 = [ Ct2, -St2*CA2, St2*SA2, Ct2*(a2) ; St2, Ct2*CA2, -Ct2*SA2, St2*(a2) ; 0, SA2, CA2, d2 ; 0, 0, 0, 1 ]; % % L3 = [ Ct3, -St3*CA3, St3*SA3, Ct3*(a3) ; St3, Ct3*CA3, -Ct3*SA3, St3*(a3) ; 0, SA3, CA3, d3 ; 0, 0, 0, 1 ]; % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 1 inch = 2.54 cm = 25.4 mm % assume length tolerance = +- 0.001 inch ==> 0.0254 mm => STD = 0.0254/3 % ---> TBD % x,y,z errors will be based on the stepper motors driving the 11-12 mm % diameter pulleys % i.e. 1 rev = pi * 11 mm the stepper motors can do 200 * 16 % microsteps/rev therefore 3200 usteps = 11*pi* mm ==> % 1 ustep = (11*pi/3200)mm % Assumption: +- 1 ustep %ustep = 11*pi/3200 == +- 0.0108 mm == +- 0.000425 inch % NOTE: 1mm about = .04 inch Da1_array = []; % this loop to randomize errors about the current truth point % ====================================================== % ====================================================== % ====================================================== for currentNumberIterations = 1:numberIterations % ustep = 11*pi/3200; %tolerance would be +- 3sigma % 1 std == 1 sigma, 68.3% between +- 1 sigma ; 95.4% +- 2 sigma ; % 99.75 between +-3sigma % ----> must account for all +- of errors % lets estimate A1err as 10mm in std at 20*12*25.4mm --- can try this with a % lazer pointer theta = (1/12)/(20) = 1/240 radians --> % (1/240)*(360/2*pi) % try 1 m with 0.001 inch non-parallelism %alpha_angle_error = (.001*25.4/1000); % radians % angle_error = 1.0/(20*12*25.4); % radians % a1err = ustep; % a2err = ustep; % a3err = ustep; % a2err = 0; % a3err = 0; % alpha A1err = alpha_angle_error; % assume sigma A2err = alpha_angle_error; % assume sigma A3err = alpha_angle_error; % assume sigma % theta t1err = theta_angle_error; t2err = theta_angle_error; t3err = theta_angle_error; DRzerr = RzRxAngleError; DRyerr = RzRxAngleError; % assume d1err is based on play between z1 and z2 % base_Dd = 0.001 * 25.4/1000; % sigma (mm) d1err = base_Dd * randn ; d2err = base_Dd * randn ; d3err = base_Dd * randn ; Dd1 = d1err * randn; Dd2 = d2err * randn; Dd3 = d3err * randn; Dt1 = t1err * randn; Dt2 = t2err * randn; Dt3 = t3err * randn; % % Da1 = a1err * randn; % Da2 = a2err * randn; % Da3 = a3err * randn; Da1 = a1err * randn; Da2 = a1err * randn; Da3 = a1err * randn; DA1 = A1err * randn; DA2 = A2err * randn; DA3 = A3err * randn; DRz = DRzerr * randn; DRy = DRyerr * randn; % ========================================================================= % ============= this block is used to tune out variables ================= % ustep = 11*pi/3200; % theta_angle_error = (.001*25.4/1000); % radians % alpha_angle_error = (.001*25.4/1000); % radians % base_Dd = 0.0005*25.4; % sigma % Rzerr = theta_angle_error; % Ryerr = theta_angle_error; % % fprintf(fidHtml,'

Error Estimation Basis

'); % % fprintf(fidHtml,''); % fprintf(fidHtml,' <% mm Cube % fprintf(fidHtml,'', theta_angle_error, alpha_angle_error,Rzerr,Ryerr); % fprintf(fidHtml,'', ustep, base_Dd); % fprintf(fidHtml,'
Error Types &theta
Fixed Link Angle play %3.6f %3.6f %3.6f %3.6f
Link Distances %3.6f %3.6f
'); beginTime1 = cputime; if (currentNumberIterations == 1) % compute TCP, no noise T=L3DP_CalculateEndCoordinate(t1 ,t2 ,t3 , ... a1 ,a2 ,a3 , ... A1 ,A2 ,A3 , ... d1 ,d2 ,d3 , ... x,y,z); P0 = [T(1) T(2) T(3)]; end endTime1 = cputime; beginTime2 = cputime; if ( currentNumberIterations == numberIterations) % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % fprintf(fidHtml,' ' ); % fprintf(fidHtml,'',t1,t2,t3 ); % fprintf(fidHtml,'',t1err,t2err,t3err ); % fprintf(fidHtml,'',Dt1,Dt2,Dt3 ); % fprintf(fidHtml,'',a1,a2,a3 ); % fprintf(fidHtml,'',a1err,a2err,a3err ); % fprintf(fidHtml,'',Da1,Da2,Da3 ); % fprintf(fidHtml,'',d1,d2,d3 ); % fprintf(fidHtml,'',d1err,d2err,d3err ); % fprintf(fidHtml,'',Dd1,Dd2,Dd3 ); % fprintf(fidHtml,'',A1,A2,A3 ) ; % fprintf(fidHtml,'',A1err,A2err,A3err ); % fprintf(fidHtml,'',DA1,DA2,DA3 ); % fprintf(fidHtml,'',DRz,DRy ); % fprintf(fidHtml,' ' ); % fprintf(fidHtml,' ' ); end % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% TT= TaylorExpandsionEndCoordinate(a1,a2,a3, ... L1,L2,L3, .... Ct1,Ct2,Ct3, ... St1,St2,St3, ... CA1,CA2,CA3, ... SA1,SA2,SA3, ... Dt1,Dt2,Dt3, ... Da1,Da2,Da3, ... DA1,DA2,DA3, ... Dd1,Dd2,Dd3, ... x,y,z); endTime2 = cputime; % fprintf('\n OKAY HOW does FIRST POINT COMPARES '); % fprintf('\n P from Forward Kinematics = %6.4f \n', P0); % fprintf('\n TT from Taylor = %6.4f \n', TT); % fprintf(fid1, '\n P0 = %6.2f \n', P0); beginCollectCoordTime = cputime; temp2 = sqrt( (P0(1)-TT(1))^2 + (P0(2)-TT(2))^2 + (P0(3)-TT(3))^2 ); del_X = [del_X ( P0(1)-TT(1) ) ]; del_Y = [del_Y ( P0(2)-TT(2) ) ]; del_Z = [del_Z ( P0(3)-TT(3) ) ]; delR2 = [delR2 temp2]; X2 = [X2 TT(1)] ; Y2 = [Y2 TT(2)] ; Z2 = [Z2 TT(3)] ; if currentNumberIterations == numberIterations delXYZ_variance = std(del_X)^2 + std(del_Y)^2 + std(del_Z)^2; % alpha = significane level e.g. 0.01 1% alpha = 0.01; % --> Za/2 = Z.005 --> Z = 2.58 ??? eta = .01 ; % mm N = delXYZ_variance/(eta^2 * alpha) % fprintf(logging_fid,'\n number of iterations for eta= %2.4f, a = %2.5f, N = %8.2f \n\n',eta,a,N); % fprintf( '\n number of iterations for eta= %2.4f, a = %2.5f, N = %8.2f \n\n',eta,a,N); estimatedIterations = double(N); end end %% for i=1:numberIterations % ====================================================== % ====================================================== % ====================================================== for currentNumberIterations = numberIterations+1 : estimatedIterations % ustep = 11*pi/3200; %tolerance would be +- 3sigma % 1 std == 1 sigma, 68.3% between +- 1 sigma ; 95.4% +- 2 sigma ; % 99.75 between +-3sigma % ----> must account for all +- of errors % lets estimate A1err as 10mm in std at 20*12*25.4mm --- can try this with a % lazer pointer theta = (1/12)/(20) = 1/240 radians --> % (1/240)*(360/2*pi) % try 1 m with 0.001 inch non-parallelism %alpha_angle_error = (.001*25.4/1000); % radians % angle_error = 1.0/(20*12*25.4); % radians % a1err = ustep; % a2err = ustep; % a3err = ustep; % a2err = 0; % a3err = 0; % alpha A1err = alpha_angle_error; % assume sigma A2err = alpha_angle_error; % assume sigma A3err = alpha_angle_error; % assume sigma % theta t1err = theta_angle_error; t2err = theta_angle_error; t3err = theta_angle_error; DRzerr = RzRxAngleError; DRyerr = RzRxAngleError; % assume d1err is based on play between z1 and z2 % base_Dd = 0.001 * 25.4/1000; % sigma (mm) d1err = base_Dd * randn ; d2err = base_Dd * randn ; d3err = base_Dd * randn ; Dd1 = d1err * randn; Dd2 = d2err * randn; Dd3 = d3err * randn; Dt1 = t1err * randn; Dt2 = t2err * randn; Dt3 = t3err * randn; Da1 = a1err * randn; Da2 = a1err * randn; Da3 = a1err * randn; DA1 = A1err * randn; DA2 = A2err * randn; DA3 = A3err * randn; DRz = DRzerr * randn; DRy = DRyerr * randn; beginTime2 = cputime; if ( currentNumberIterations == estimatedIterations) % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % fprintf(fidHtml,' ' ); % fprintf(fidHtml,'',t1,t2,t3 ); % fprintf(fidHtml,'',t1err,t2err,t3err ); % fprintf(fidHtml,'',Dt1,Dt2,Dt3 ); % fprintf(fidHtml,'',a1,a2,a3 ); % fprintf(fidHtml,'',a1err,a2err,a3err ); % fprintf(fidHtml,'',Da1,Da2,Da3 ); % fprintf(fidHtml,'',d1,d2,d3 ); % fprintf(fidHtml,'',d1err,d2err,d3err ); % fprintf(fidHtml,'',Dd1,Dd2,Dd3 ); % fprintf(fidHtml,'',A1,A2,A3 ) ; % fprintf(fidHtml,'',A1err,A2err,A3err ); % fprintf(fidHtml,'',DA1,DA2,DA3 ); % fprintf(fidHtml,'',DRz,DRy ); % fprintf(fidHtml,' ' ); % fprintf(fidHtml,' ' ); end % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% TT= TaylorExpandsionEndCoordinate(a1,a2,a3, ... L1,L2,L3, .... Ct1,Ct2,Ct3, ... St1,St2,St3, ... CA1,CA2,CA3, ... SA1,SA2,SA3, ... Dt1,Dt2,Dt3, ... Da1,Da2,Da3, ... DA1,DA2,DA3, ... Dd1,Dd2,Dd3, ... x,y,z); endTime2 = cputime; beginCollectCoordTime = cputime; temp2 = sqrt( (P0(1)-TT(1))^2 + (P0(2)-TT(2))^2 + (P0(3)-TT(3))^2 ); del_X = [del_X ( P0(1)-TT(1) ) ]; del_Y = [del_Y ( P0(2)-TT(2) ) ]; del_Z = [del_Z ( P0(3)-TT(3) ) ]; delR2 = [delR2 temp2]; % this is array of delta R2s X2 = [X2 TT(1)] ; Y2 = [Y2 TT(2)] ; Z2 = [Z2 TT(3)] ; end %% for i=numberIterations+1 : estimatedIterations % fprintf(fidHtml, '', P0(1), P0(2), P0(3) ); % fprintf(fidHtml, '', TT(1) , TT(2), TT(3) ); % mean_del_X_std = mean(del_X); % mean_del_Y_std = mean(del_Y); % mean_del_Z_std = mean(del_Z); delXYZ_variance = std(del_X)^2 + std(del_Y)^2 + std(del_Z)^2; delXYZ_std = delXYZ_variance^0.5; R2variance = std(X2)^2 + std(Y2)^2 + std(Z2)^2 ; X2_std = std(X2); X2_var = X2_std^2; R2std = ( std(X2)^2 + std(Y2)^2 + std(Z2)^2 )^0.5 ; delR2_mean = mean(delR2); delR2_std = std(delR2); delR2_variance = delR2_std^2; fprintf( '\n estimatedIterations = %8.8f ', estimatedIterations); fprintf(logging_fid, '\n estimatedIterationsd = %8.8f ', estimatedIterations); end % OKAY BELOW % OKAY BELOW function T=L3DP_CalculateEndCoordinate(t1,t2,t3, ... a1,a2,a3, ... A1,A2,A3, ... d1,d2,d3, ... x,y,z) % % Li = % % [ cos(ti), -sin(ti)*cos(Ai), sin(ti)*sin(Ai), cos(ti)*ai] % [ sin(ti), cos(ti)*cos(Ai), -cos(ti)*sin(Ai), sin(ti)*ai] % [ 0, sin(Ai), cos(Ai), di] % [ 0, 0, 0, 1] % % global Da1 Da2 Da3 Dd1 Dd2 Dd3 DA1 DA2 DA3 Dt1 Dt2 Dt3 DRz DRy % substitutions/simplifications: Ct1 = cos(t1);Ct2 = cos(t2);Ct3 = cos(t3); St1 = sin(t1);St2 = sin(t2);St3 = sin(t3); CA1 = cos(A1);CA2 = cos(A2);CA3 = cos(A3); SA1 = sin(A1);SA2 = sin(A2);SA3 = sin(A3); Cmpi_2 = cos(-pi/2); Spi_2 = sin(pi/2); Smpi_2 = sin(-pi/2); Cpi_2 = cos(pi/2); % Rz_minus = [ Cmpi_2, -Smpi_2, 0, 0; Smpi_2, Cmpi_2, 0, 0; 0, 0, 1, 0; 0, 0, 0, 1 ]; % % convienent pure rotation % this is the rotation of link2 to link1 - real errors possible ,i.e. not exactly 90 deg % Rz_plusErr = [ C(pi_2+DRz), -S(pi_2+DRz), 0, 0; % S(pi_2+DRz), C(pi_2+DRz), 0, 0; % 0, 0, 1, 0; % 0, 0, 0, 1 ]; Rz_plus = [ Cpi_2, -Spi_2, 0, 0; Spi_2, Cpi_2, 0, 0; 0, 0, 1, 0; 0, 0, 0, 1 ]; Ry_minus = [ Cmpi_2, 0, Smpi_2, 0; 0, 1 0 , 0; -Smpi_2, 0, Cmpi_2, 0; 0, 0, 0, 1 ]; % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% L1 = [ Ct1, -St1*CA1, St1*SA1, Ct1*a1; St1, Ct1*CA1, -Ct1*SA1, St1*a1; 0, SA1, CA1, d1; 0, 0, 0, 1]; L2 = [ Ct2, -St2*CA2, St2*SA2, Ct2*a2; St2, Ct2*CA2, -Ct2*SA2, St2*a2; 0, SA2, CA2, d2; 0, 0, 0, 1]; L3 = [ Ct3, -St3*CA3, St3*SA3, Ct3*a3; St3, Ct3*CA3, -Ct3*SA3, St3*a3; 0, SA3, CA3, d3; 0, 0, 0, 1]; T = L1 * Rz_plus *L2 * Ry_minus * L3 * Rz_minus * [x y z 1]' ; end % OKAY BELOW % OKAY BELOW function T= TaylorExpandsionEndCoordinate(a1,a2,a3, ... L1,L2,L3, .... Ct1,Ct2,Ct3, ... St1,St2,St3, ... CA1,CA2,CA3, ... SA1,SA2,SA3, ... Dt1,Dt2,Dt3, ... Da1,Da2,Da3, ... DA1,DA2,DA3, ... Dd1,Dd2,Dd3, ... x,y,z) %global Da1 Da2 Da3 Dd1 Dd2 Dd3 DA1 DA2 DA3 Dt1 Dt2 Dt3 global DRz DRy % Taylor's expandsion % f(x+h) = f(x) + h*f'(x) + (f''(h)*(x-h)^2)/2! + ..... % % Li = % % [ cos(ti+Dti), -sin(ti+Dti)*cos(Ai+DAi), sin(ti+Dti)*sin(Ai+DAi), cos(ti+Dti)*(ai+Dai)] % [ sin(ti+Dti), cos(ti+Dti)*cos(Ai+DAi), -cos(ti+Dti)*sin(Ai+DAi), sin(ti+Dti)*(ai+Dai)] % [ 0, sin(Ai+DAi), cos(Ai+DAi), di+Ddi] % [ 0, 0, 0, 1] % % % cos(ti+Dti) = cos(ti) - Dti*sin(ti); % sin(ti+Dti) = sin(ti) + Dti*cos(ti); % % cos(Ai+DAi) = cos(Ai) - DAi*sin(Ai); % sin(Ai+DAi) = sin(Ai) + DAi*cos(Ai); % sin(ti+Dti)*cos(Ai+DAi) = sin(ti)*cos(Ai) - sin(ti)*sin(Ai)*DAi % +cos(ti)*cos(Ai)*Dti OK % sin(ti+Dti)*sin(Ai+DAi) = sin(ti)*sin(Ai) + sin(ti)*cos(Ai)*DAi % +cos(ti)*sin(Ai)*Dti OK % % cos(ti+Dti)*(ai+Da1) = ai*cos(ti) + cos(ti)*Dai -ai*sin(ti)*Dti % cos(ti+Dti)*cos(Ai+DAi) = cos(ti)*cos(Ai) - cos(ti)*sin(Ai)*DAi -sin(ti)*cos(Ai)*Dti % % % cos(ti+Dti)*sin(Ai+DAi) = cos(ti)*sin(Ai) + cos(ti)*cos(Ai)*DAi -sin(ti)*sin(Ai)*Dti % sin(ti+Dti)*(ai+Dai) = ai*sin(ti) + sin(ti)*Dai + ai*cos(ti)*Dti % Li => % % [ cos(ti+Dti), cos(ti) - Dti*sin(ti); % -sin(ti+Dti)*cos(Ai+DAi), - sin(ti)*cos(Ai) + sin(ti)*sin(Ai)*DAi - cos(ti)*cos(Ai)*Dti % sin(ti+Dti)*sin(Ai+DAi), sin(ti)*sin(Ai) + sin(ti)*cos(Ai)*DAi + cos(ti)*sin(Ai)*Dti % cos(ti+Dti)*(ai+Dai)] ai*cos(ti) + cos(ti)*Dai -ai*sin(ti)*Dti % [ sin(ti+Dti), sin(ti) + Dti*cos(ti) % cos(ti+Dti)*cos(Ai+DAi), cos(ti)*cos(Ai) - cos(ti)*sin(Ai)*DAi -sin(ti)*cos(Ai)*Dti % -cos(ti+Dti)*sin(Ai+DAi), - cos(ti)*sin(Ai) - cos(ti)*cos(Ai)*DAi +sin(ti)*sin(Ai)*Dti % sin(ti+Dti)*(ai+Dai)] ai*sin(ti) + sin(ti)*Dai + ai*cos(ti)*Dti % [ 0, % sin(Ai+DAi), sin(Ai) + DAi*cos(Ai) % cos(Ai+DAi), cos(Ai) - DAi*sin(Ai) % di+Ddi] di +Ddi % [ 0, 0, 0, 1] % % % Li = % % [ cos(ti) - Dti*sin(ti), % - sin(ti)*cos(Ai) + sin(ti)*sin(Ai)*DAi - cos(ti)*cos(Ai)*Dti, % sin(ti)*sin(Ai) + sin(ti)*cos(Ai)*DAi + cos(ti)*sin(Ai)*Dti, % ai*cos(ti) + cos(ti)*Dai -ai*sin(ti)*Dti ] % [ sin(ti) + Dti*cos(ti), % cos(ti)*cos(Ai) - cos(ti)*sin(Ai)*DAi -sin(ti)*cos(Ai)*Dti, % - cos(ti)*sin(Ai) - cos(ti)*cos(Ai)*DAi +sin(ti)*sin(Ai)*Dti, % ai*sin(ti) + sin(ti)*Dai + ai*cos(ti)*Dti ] % [ 0, % sin(Ai) + DAi*cos(Ai), % cos(Ai) - DAi*sin(Ai), % di + Ddi ] % [ 0 , 0 0, 1] % % (Li + DLi) ==> % Li = % % [ cos(ti), -sin(ti)*cos(Ai), sin(ti)*sin(Ai), cos(ti)*(ai)] % [ sin(ti), cos(ti)*cos(Ai), -cos(ti)*sin(Ai), sin(ti)*(ai)] % [ 0, sin(Ai), cos(Ai), di] % [ 0, 0, 0, 1] % % % DLi = % % [- Dti*sin(ti), + sin(ti)*sin(Ai)*DAi - cos(ti)*cos(Ai)*Dti, % + sin(ti)*cos(Ai)*DAi + cos(ti)*sin(Ai)*Dti, % + cos(ti)*Dai -ai*sin(ti)*Dti ] % [+ Dti*cos(ti), - cos(ti)*sin(Ai)*DAi -sin(ti)*cos(Ai)*Dti, % - cos(ti)*cos(Ai)*DAi +sin(ti)*sin(Ai)*Dti, % + sin(ti)*Dai + ai*cos(ti)*Dti ] % [ 0, + DAi*cos(Ai), - DAi*sin(Ai), + Ddi ] % [ 0 0, 0, 1 ] % % Ct1 = cos(t1);Ct2 = cos(t2);Ct3 = cos(t3);Ct4 = cos(t4);Ct5 = cos(t5);Ct6 = cos(t6); % St1 = sin(t1);St2 = sin(t2);St3 = sin(t3);St4 = sin(t4);St5 = sin(t5);St6 = sin(t6); % CA1 = cos(A1);CA2 = cos(A2);CA3 = cos(A3);CA4 = cos(A4);CA5 = cos(A5);CA6 = cos(A6); % SA1 = sin(A1);SA2 = sin(A2);SA3 = sin(A3);SA4 = sin(A4);SA5 = sin(A5);SA6 = sin(A6); % % (L1 + DL1) ==> % L1 = [ Ct1, -St1*CA1, St1*SA1, Ct1*(a1); % St1, Ct1*CA1, -Ct1*SA1, St1*(a1); % 0, SA1, CA1, d1; % 0, 0, 0, 1]; % % Cmpi_2 = cos(-pi/2); Spi_2 = sin(pi/2); Smpi_2 = sin(-pi/2); Cpi_2 = cos(pi/2); Rz_minus = [ Cmpi_2, -Smpi_2, 0, 0; Smpi_2, Cmpi_2, 0, 0; 0, 0, 1, 0; 0, 0, 0, 1 ]; Rz_plus = [ Cpi_2, -Spi_2, 0, 0; Spi_2, Cpi_2, 0, 0; 0, 0, 1, 0; 0, 0, 0, 1 ]; Rx_minus = [ 1, 0, 0, 0; 0, Cmpi_2, -Smpi_2, 0; 0, Smpi_2, Cmpi_2, 0; 0, 0, 0, 1 ]; Rx_plus = [ 1, 0, 0, 0; 0, Cpi_2, -Spi_2, 0; 0, Spi_2, Cpi_2, 0; 0, 0, 0, 1 ]; Ry_minus = [ Cmpi_2, 0, Smpi_2, 0; 0, 1, 0 , 0; -Smpi_2, 0, Cmpi_2, 0; 0, 0, 0, 1 ]; % substitutions/simplifications: % Ct1 = cos(t1);Ct2 = cos(t2);Ct3 = cos(t3); % St1 = sin(t1);St2 = sin(t2);St3 = sin(t3); % CA1 = cos(A1);CA2 = cos(A2);CA3 = cos(A3); % SA1 = sin(A1);SA2 = sin(A2);SA3 = sin(A3); Cmpi_2 = cos(-pi/2); Spi_2 = sin(pi/2); Smpi_2 = sin(-pi/2); Cpi_2 = cos(pi/2); % convienent pure rotation Rz_minus = [ Cmpi_2, -Smpi_2, 0, 0; Smpi_2, Cmpi_2, 0, 0; 0, 0, 1, 0; 0, 0, 0, 1 ]; % this is the rotation of link2 to link1 - real errors possible ,i.e. not exactly 90 deg % Rz_plusErr = [ C(pi_2+DRz), -S(pi_2+DRz), 0, 0; % S(pi_2+DRz), C(pi_2+DRz), 0, 0; % 0, 0, 1, 0; % 0, 0, 0, 1 ]; Rz_plusErr = [ (Cpi_2 -DRz*Spi_2), (-Spi_2 +DRz*Cpi_2), 0, 0; (Spi_2 +DRz*Cpi_2), (Cpi_2 -DRz*Spi_2), 0, 0; 0, 0, 1, 0; 0, 0, 0, 1]; % this is the rotation of link3 to link2 - real errors possible ,i.e. not exactly 90 deg % Ry_minus = [ Cmpi_2), 0, Smpi_2, 0; % 0, 1 0 , 0; % -Smpi_2, 0, Cmpi_2, 0; % 0, 0, 0, 1 ]; % Ry_minusErr = [ C(mpi_2+DRy), 0, S(mpi_2+DRy), 0; % 0, 1 0 , 0; % -S(mpi_2+DRy), 0, C(mpi_2+DRY), 0; % 0, 0, 0, 1 ]; Ry_minusErr = [ (Cmpi_2 -DRy*Smpi_2), 0, ( Smpi_2 -DRy*Cmpi_2), 0; 0, 1, 0, 0; (-Smpi_2 -DRy*Cmpi_2), 0, ( Cmpi_2 -DRy*Smpi_2), 0; 0, 0, 0, 1]; DL1 = [ (- Dt1*St1), (+ St1*SA1*DA1 - Ct1*CA1*Dt1), (+ St1*CA1*DA1 + Ct1*SA1*Dt1), (+ Ct1*Da1 - a1*St1*Dt1) ; (+ Dt1*Ct1), (- Ct1*SA1*DA1 - St1*CA1*Dt1), (- Ct1*CA1*DA1 + St1*SA1*Dt1), (+ St1*Da1 + a1*Ct1*Dt1) ; 0 , (+ DA1*CA1) , (- DA1*SA1) , + Dd1 ; 0 , 0, 0, 0 ]; % % L2 = [ Ct2, -St2*CA2, St2*SA2, Ct2*(a2) ; % St2, Ct2*CA2, -Ct2*SA2, St2*(a2) ; % 0, SA2, CA2, d2 ; % 0, 0, 0, 1 ]; % % % DL2 = [ (- Dt2*St2), (+ St2*SA2*DA2 - Ct2*CA2*Dt2), (+ St2*CA2*DA2 + Ct2*SA2*Dt2), (+ Ct2*Da2 - a2*St2*Dt2) ; (+ Dt2*Ct2), (- Ct2*SA2*DA2 - St2*CA2*Dt2), (- Ct2*CA2*DA2 + St2*SA2*Dt2), (+ St2*Da2 + a2*Ct2*Dt2) ; ( 0 ), (+ DA2*CA2 ), (- DA2*SA2 ), (+ Dd2 ) ; ( 0 ), ( 0 ), ( 0 ), (0 ) ]; % % L3 = [ Ct3, -St3*CA3, St3*SA3, Ct3*(a3) ; % St3, Ct3*CA3, -Ct3*SA3, St3*(a3) ; % 0, SA3, CA3, d3 ; % 0, 0, 0, 1 ]; % % DL3 = [ (- Dt3*St3), (+ St3*SA3*DA3 - Ct3*CA3*Dt3), (+ St3*CA3*DA3 + Ct3*SA3*Dt3), (+ Ct3*Da3 - a3*St3*Dt3) ; (+ Dt3*Ct3), (- Ct3*SA3*DA3 - St3*CA3*Dt3), (- Ct3*CA3*DA3 + St3*SA3*Dt3), (+ St3*Da3 + a3*Ct3*Dt3) ; (0 ), (+ DA3*CA3 ), (- DA3*SA3 ), (+ Dd3 ) ; (0 ), ( 0 ), (0 ), (0 ) ]; % %T = (L1+DL1)*(L2+DL2)*(L3+DL3)*(L4+DL4)*(L5+DL5)*(L6+DL6)*[x y z 1]' ; % T = L1 * Rz_plus *L2 * Ry_minus * L3 * Rz_minus * [x y z 1]' ; T = (L1+DL1)* Rz_plusErr *(L2+DL2)* Ry_minusErr *(L3+DL3)* Rz_minus *[x y z 1]' ; %T = Rx_prep * Rz_prep *(L1)*(L2)*(L3)*[x y z 1]' ; % fprintf('\n T = %8.6f \n', T); % % % substitutions/simplifications: % Ct1 = cos(t1);Ct2 = cos(t2);Ct3 = cos(t3);Ct4 = cos(t4);Ct5 = cos(t5);Ct6 = cos(t6); % St1 = sin(t1);St2 = sin(t2);St3 = sin(t3);St4 = sin(t4);St5 = sin(t5);St6 = sin(t6); % CA1 = cos(A1);CA2 = cos(A2);CA3 = cos(A3);CA4 = cos(A4);CA5 = cos(A5);CA6 = cos(A6); % SA1 = sin(A1);SA2 = sin(A2);SA3 = sin(A3);SA4 = sin(A4);SA5 = sin(A5);SA6 = sin(A6); % % L1 = [ Ct1, -St1*CA1, St1*SA1, Ct1*a1; % St1, Ct1*CA1, -Ct1*SA1, St1*a1; % 0, SA1, CA1, d1; % 0, 0, 0, 1]; % % % % L2 = [ Ct2, -St2*CA2, St2*SA2, Ct2*a2; % St2, Ct2*CA2, -Ct2*SA2, St2*a2; % 0, SA2, CA2, d2; % 0, 0, 0, 1]; % % % % L3 = [ Ct3, -St3*CA3, St3*SA3, Ct3*a3; % St3, Ct3*CA3, -Ct3*SA3, St3*a3; % 0, SA3, CA3, d3; % 0, 0, 0, 1]; % % % end % plot(del_X ); % plot(del_Y); % plot(del_Z ); % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % %%%%% Change in direction. Code below OBE. %%%%%%%%%%%%%% % %%%%% Used in checking out coding of equations %%%%%%%%%%% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % function newValueDel = randomdelta(varSTD,numberSamples,nominalValue) % % % this function computes random samples about a nominal value % % % varSTD is used to get a gaussian random offset % % %varDel = varSTD * randn(numberSamples,1); % % % x = -4*varSTD:0.1:4*varSTD; % % %y = randn(numberSamples,1);*(L4+DL4)*(L5+DL5)*(L6+DL6) % % newValueDel = varSTD * randn(numberSamples,1) + nominalValue; % % hist(newValueDel); % % % % %fprintf('\nwhat the\n'); % % end % % % % % % % % TT = ARM1*ARM2 % % % % % % test code % % % TT = [ nx sx ax px % % % ny sy ay py % % % nz sz az pz % % % 0 0 0 1 ] % % T = TT * [x y z 1]' % % % % % end test code % % % T = [ nx sx ax px % % ny sy ay py TESTVARIABLESCOPE = 100; % % nz sz az pz % % 0 0 0 1 ] * [x y z 1]' % T = ARM1*ARM2 * [x y z 1]' ; % % % varDel = randomdelta(.1,10000,200.5); % % hist(varDel); % end % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% TESTVARIABLESCOPE = 100; % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % %%%%%%% CODE BELOW WAS USED DURING DEBUG/DEVELOPMENT %% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % % % function ForwardKinematics % % % Forward Kinematics % % % Robert E. Nee 10/13/05 % % % % % % Puma 560 Forward Kinematics % % % p 45 Robotics: Control,Densing,Vision, and Intelligence % % % K.D.Fu , R.C. Gonzalez, C.S.G. LeeT = Rz_prep * Rx_prep* L1 * L2 * L3 * [x y z 1]' ; % % % % % % % t1 = pi/2; t2 = 0; t3 = pi/2; t4 = 0; t5 = 0; t6 = 0; % % % Cij == cos(theta i + theta j) % % % Sij == sin(theta i + theta j) % % % C1 = cos(t1); C23 = cos(t2+t3); C4 = cos(t4); C5 = cos(t5); C6 = cos(t6); % % % S4 = sin(t4); S6 = sin(t6); S23 = sin(t2+t3); S5 = sin(t5); % % % S1 = sin(t1); % % % C2 = cos(t2); % % % S2 = sin(t2); % % % % % NOTE: must redo equations with all d parameters i.e. book formulas % % % set d1,d3,d5 = indentically zero % % % same with alpha & a-parameters % % % Must obtain transformation matrix using all parameters % % % USE SYMbolic toolbox on XP or get this fixed or do it from MAPLE % % % =====>>>>>>>> DHequations.m symbolically generated ARM1, ARM2 % % a1 = 0; a2 = 431.8; a3 = -20.32; a4 = 0; a5 =0; a6 =0; % % d1 = 0; d2 = 149.09; d3 = 0; d4 = 433.07; d5 =0; d6 = 56.25; % % A1= -pi/2; A2=0; A3=pi/2; A4=-pi/2; A5=pi/2; A6=0; % % % % % % % test case 1: % % t1 = pi/2; t2 = 0.0; t3 = pi/2; t4 = 0.0; t5 = 0.0; t6 = 0.0; % % x = 0.0; y = 0.0; z = 0.0; % % % should produce: % % % TT = % % % % % % 0.0000 -1.0000 0.0000 -149.0900 % % % 0.0000 0 1.0000 921.1200 % % % -1.0000 0 0.0000 20.3200 % % % 0 0 0 1.0000 % % % % % % T = % % % % % % -149.0900 % % % 921.1200 % % % 20.3200 % % % 1.0000 % % % % % % end test case 1 % % % % % % % % T=L3DP_CalculateEndCoordinate(t1,t2,t3,t4,t5,t6, ... % % a1,a2,a3,a4,a5,a6, ... % % A1,A2,A3,A4,A5,A6, ... % % d1,d2,d3,d4,d5,d6, ... % % x,y,z) % % % % end % %