% 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,' Error Types | &theta | <% mm Cube
% fprintf(fidHtml,'
Fixed Link Angle play | %3.6f | %3.6f | | | %3.6f | %3.6f |
', theta_angle_error, alpha_angle_error,Rzerr,Ryerr);
% fprintf(fidHtml,' Link Distances | | | %3.6f | %3.6f | | |
', ustep, base_Dd);
% fprintf(fidHtml,'
');
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,'\n TEST CONTROL BLOCK FOR TAYLOR POINTS WITH NOISE
' );
% fprintf(fidHtml,'\n t1,t2,t3 = %6.4f %6.4f %6.4f
',t1,t2,t3 );
% fprintf(fidHtml,' t1err,t2err,t3err = %3.8f %3.8f %3.8f
',t1err,t2err,t3err );
% fprintf(fidHtml,' Dt1,Dt2,Dt3 = %3.8f %3.8f %3.8f
',Dt1,Dt2,Dt3 );
% fprintf(fidHtml,'\n a1,a2,a3 = %6.4f %6.4f %6.4f
',a1,a2,a3 );
% fprintf(fidHtml,' a1err,a2err,a3err = %3.8f %3.8f %3.8f
',a1err,a2err,a3err );
% fprintf(fidHtml,' Da1,Da2,Da3 = %3.8f %3.8f %3.8f
',Da1,Da2,Da3 );
% fprintf(fidHtml,'\n d1,d2,d3 = %6.4f %6.4f %6.4f
',d1,d2,d3 );
% fprintf(fidHtml,' d1err,d2err,d3err = %3.8f %3.8f %3.8f
',d1err,d2err,d3err );
% fprintf(fidHtml,' Dd1,Dd2,Dd3 = %3.8f %3.8f %3.8f
',Dd1,Dd2,Dd3 );
% fprintf(fidHtml,'\n A1,A2,A3 = %6.4f %6.4f %6.4f
',A1,A2,A3 ) ;
% fprintf(fidHtml,' A1err,A2err,A3err = %3.8f %3.8f %3.8f
',A1err,A2err,A3err );
% fprintf(fidHtml,' DA1,DA2,DA3 = %3.8f %3.8f %3.8f
',DA1,DA2,DA3 );
% fprintf(fidHtml,'',DRz,DRy );
% fprintf(fidHtml,'\n %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
' );
% fprintf(fidHtml,'\n %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
' );
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,'\n TEST CONTROL BLOCK FOR TAYLOR POINTS WITH NOISE
' );
% fprintf(fidHtml,'\n t1,t2,t3 = %6.4f %6.4f %6.4f
',t1,t2,t3 );
% fprintf(fidHtml,' t1err,t2err,t3err = %3.8f %3.8f %3.8f
',t1err,t2err,t3err );
% fprintf(fidHtml,' Dt1,Dt2,Dt3 = %3.8f %3.8f %3.8f
',Dt1,Dt2,Dt3 );
% fprintf(fidHtml,'\n a1,a2,a3 = %6.4f %6.4f %6.4f
',a1,a2,a3 );
% fprintf(fidHtml,' a1err,a2err,a3err = %3.8f %3.8f %3.8f
',a1err,a2err,a3err );
% fprintf(fidHtml,' Da1,Da2,Da3 = %3.8f %3.8f %3.8f
',Da1,Da2,Da3 );
% fprintf(fidHtml,'\n d1,d2,d3 = %6.4f %6.4f %6.4f
',d1,d2,d3 );
% fprintf(fidHtml,' d1err,d2err,d3err = %3.8f %3.8f %3.8f
',d1err,d2err,d3err );
% fprintf(fidHtml,' Dd1,Dd2,Dd3 = %3.8f %3.8f %3.8f
',Dd1,Dd2,Dd3 );
% fprintf(fidHtml,'\n A1,A2,A3 = %6.4f %6.4f %6.4f
',A1,A2,A3 ) ;
% fprintf(fidHtml,' A1err,A2err,A3err = %3.8f %3.8f %3.8f
',A1err,A2err,A3err );
% fprintf(fidHtml,' DA1,DA2,DA3 = %3.8f %3.8f %3.8f
',DA1,DA2,DA3 );
% fprintf(fidHtml,'',DRz,DRy );
% fprintf(fidHtml,'\n %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
' );
% fprintf(fidHtml,'\n %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
' );
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, '\n P0(1) = %6.4f P0(2) = %6.4f P0(3) = %6.4f \n
', P0(1), P0(2), P0(3) );
% fprintf(fidHtml, '\n TT(numberIterations) = %6.4f %6.4f %6.4f\n
', 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
% %