%______________________________________________________________ % File: Sim_ArrowTracking_PTU % Date: 20-Oct-2014 % Purpose: Simulate the tracking of the "arrow" cue with % the pan-tilt unit (PTU) mounted on the FANUC %______________________________________________________________ clear all close all clc %______________________________________________________________ % Parameters and initializations %______________________________________________________________ %L = [36.4 35.5 60.0 1]*1e-3; % Link lengths L = [60 35.5 30 1]*1e-3; % Link lengths FANUC_flag = 1; if FANUC_flag == 1, % Read joint angles and FANUC's end-effector coordinates JOINT_data = csvread('data/servos_position.txt'); FANUC_data = csvread('data/positions.txt'); % Synchronized time vector if JOINT_data(1,1) > FANUC_data(1,1), dtime = JOINT_data(1,1); else dtime = FANUC_data(1,1); end % PTU joint angles tr = JOINT_data(:,1)-dtime; Qr = JOINT_data(:,2:3)'; ILog = Qr == 0; Qr(ILog) = NaN; Qr = Qr([2 1],:); % FANUC's end-effector coordinates FANUC_data = FANUC_data(1:2:end,:); fPptu = 1e-3*FANUC_data(:,2:4)'; %Calculate the experiment max velocity maxVel = calculateSpeed(FANUC_data(:,[1 3])) fPptu(2,:) = fPptu(2,:)-260*1e-3; % Gripper's offset (YY-axis) t = FANUC_data(:,1)-dtime; Texe = t(end); else Ts = 5e-3; % Sampling rate Texe = 2; % Simulation time t = 0 :Ts:Texe; end DIM = length(t); %______________________________________________________________ % Static arrow / Moving PTU %______________________________________________________________ %posi�l�o da seta em rela��o � ref (1-> 1metro) % 1000+614 = distacia fanuc seta (Eixo X) % tem de estar alinhado com a seta % descobrir onde esta a origem do fanuc e conseguitr realacionar rP_arrow = [1 0 0 1]'; % Arrow position in Sr if FANUC_flag == 1, fPptu = [fPptu ; ones(1,DIM)]; %fanuc para a seta %origem do fanuc no sistema de referencia rTf = [ 1 0 0 -885.086*1e-3%X 0 1 0 -5.95e-3%Y com val Positivo é pior! 0 0 1 5.07e-3%ZZ 0 0 0 1 ]; rPo = rTf*fPptu; %Posição do end-efector ao longo do movimento. rPo = rPo(1:3,:); else %Simula um movimento do fanuc com planeamento de trajetoria Pi = [0 2 0.5]'; Pf = [0 -2 0.5]'; % Initial/final camera position S0 rPo = zeros(3,DIM); [ rPo(1,:),dP,ddP,counter ] = PolyTrajGenerator( t,Pi(1),Pf(1),5 ); [ rPo(2,:),dP,ddP,counter ] = PolyTrajGenerator( t,Pi(2),Pf(2),5 ); [ rPo(3,:),dP,ddP,counter ] = PolyTrajGenerator( t,Pi(3),Pf(3),5 ); end %______________________________________________________________ % Desired PTU's tracking: reference joint angles %___________________________________________rTo___________________ Qd = zeros(2,DIM); for ii = 1:DIM, % Inverse kinematics _________________________ rTo = [eye(3) rPo(:,ii); 0 0 0 1];% assume que não ha rotacao do gripper SoP_arrow = rTo\rP_arrow; Qd(:,ii) = InverseKinematics( SoP_arrow,L ); end %______________________________________________________________ % Animate motion using a PTU's proportional controller %______________________________________________________________ H=figure(1); set(H,'Color',[1 1 1]), EL = 0; AZ = 90; figure(2) Q = zeros(2,DIM); ERR_img = zeros(2,DIM); P_ptu = zeros(3,4); P_cam = zeros(3,4); for ii = 1:DIM, if ii == 1, % Initial alignment of the camera with the arrow % Inverse kinematics _____________________________________ rTo = [eye(3) rPo(:,1); 0 0 0 1]; SoP_arrow = rTo\rP_arrow; Q(:,ii) = InverseKinematics( SoP_arrow,L ); else % Proportional control based on the image error La = P_ptu(:,4); Lb = rP_arrow(1:3); P0 = P_cam(:,1); P1 = P_cam(:,2); P2 = P_cam(:,3); A = [La-Lb P1-P0 P2-P0]; b = La-P0; S = A\b; P_img = La+(Lb-La)*S(1); q1 = Q(1,ii-1); q2 = Q(2,ii-1); s1 = sin(q1); c1 = cos(q1); s2 = sin(q2); c2 = cos(q2); l1 = L(1); l2 = L(2); l3 = L(3)-0.01*L(3); T = [ c1*c2 -c1*s2 s1 (l3*c1*c2-l2*c1*s2)+rPo(1,ii-1)%Usa a posição do fanuc? s1*c2 -s1*s2 -c1 (l3*s1*c2-l2*s1*s2)+rPo(2,ii-1) s2 c2 0 (l3*s2+l2*c2+l1)+rPo(3,ii-1) 0 0 0 1 ]; P_img = T\[P_img; 1]; ERR_img(:,ii) = [P_img(3) P_img(2)]'; Q(1,ii) = 400*P_img(3)+Q(1,ii-1); %O que representa o 400? Q(2,ii) = -400*P_img(2)+Q(2,ii-1); Q_ptu = Q(:,1:ii)*180/pi; %pause end % Direct kinematics ______________________________________ [P_ptu P_cam] = DirectKinematics( Q(:,ii),L ); P_ptu = P_ptu + repmat(rPo(:,ii),1,4); P_cam = P_cam + repmat(rPo(:,ii),1,4); % % Animate motion % FontSize = 16; % figure(1), clf % get(0,'Factory');set(0,'defaultfigurecolor',[1 1 1]); % hold on % H = plot3(P_ptu(1,:),P_ptu(2,:),P_ptu(3,:),'gp-', rP_arrow(1),rP_arrow(2),rP_arrow(3),'bx',... % [P_ptu(1,end) rP_arrow(1)],[P_ptu(2,end) rP_arrow(2)],[P_ptu(3,end) rP_arrow(3)],'b-.',... % rPo(1,1:ii),rPo(2,1:ii),rPo(3,1:ii),'r.','markers',FontSize ); % % fill3([P_cam(1,:) P_cam(1,1)],[P_cam(2,:) P_cam(2,1)],[P_cam(3,:) P_cam(3,1)],'k') % xlabel('XX','FontSize', FontSize ), ylabel('YY','FontSize', FontSize ), zlabel('ZZ','FontSize', FontSize ) % set(gca,'FontSize',FontSize ), % % axis equal, axis([-0.5 1.5 -0.5 0.5 -1 2]), view(EL,AZ), %grid on % axis equal, axis([-0.5 1.5 -0.5 2.5 -1 2]), view(EL,AZ), %grid on % % Pan-tilt joint angles % figure(2); % get(0,'Factory');set(0,'defaultfigurecolor',[1 1 1]); % % subplot(2,1,1), plot(t(1:ii),Q(1,1:ii)*180/pi,'r.'), xlim([0 Texe]) % xlabel('Time (s)','FontSize', FontSize ), ylabel('PAN: Q1 (deg)','FontSize', FontSize ) % set(gca,'FontSize',FontSize ); % subplot(2,1,2), plot(t(1:ii),Q(2,1:ii)*180/pi,'r.'), xlim([0 Texe]) % xlabel('Time (s)','FontSize', FontSize ), ylabel('TILT: Q2 (deg)','FontSize', FontSize ) % set(gca,'FontSize',FontSize ); if ii == 1, pause, [EL,AZ] = view; end pause(0.0001) if ii== 210, pause;end end %______________________________________________________________ % Performance measure %______________________________________________________________ if FANUC_flag == 1, % figure('Name', 'Q','Color',[1 1 1]) % subplot(2,1,1), plot(t,Qd(1,:)*180/pi,'r:', t,Q(1,:)*180/pi,'k', tr,(-Qr(1,:)+1454)/10,'b') % xlabel('Time (s)','FontSize', 12), ylabel('PAN: Q1 (deg)','FontSize', 12), legend('q1d','q1','q1r') % xlim([17 35]),ylim([-30 30])%xlim([13 21])%xlim([11 24]) % set(gca,'FontSize',12); % subplot(2,1,2), plot(t,Qd(2,:)*180/pi,'r:', t,Q(2,:)*180/pi,'k', tr,(-Qr(2,:)+1862)/10,'b')%1896 % xlabel('Time (s)','FontSize', 12), ylabel('TILT: Q2 (deg)','FontSize', 12), legend('q2d','q2','q2r') % xlim([17 35]),ylim([-30 30])%xlim([13 21])%xlim([11 24]) % set(gca,'FontSize',12); %XLim = [14 26]; %YLim = [-45 45];%xlim([13 21])% XLim = [11 24]; YLim = [-30 30]; % t = t - XLim(1); % tr = tr - XLim(1); % XLim = XLim - XLim(1); FontSize = 16; figure('Name', 'Q','Color',[1 1 1]) subplot(2,1,1), plot(t,Qd(1,:)*180/pi,'r', tr,(-Qr(1,:)+1454)/10,'b','LineWidth',2,'MarkerSize',4) xlabel('Time (s)','FontSize', FontSize), ylabel('PAN: Q1 (deg)','FontSize', FontSize ), legend('q1d','q1r') xlim(XLim),ylim(YLim) set(gca,'FontSize',FontSize ); subplot(2,1,2), plot(t,Qd(2,:)*180/pi,'r',tr,(-Qr(2,:)+1862)/10,'b','LineWidth',2,'MarkerSize',2)%1896 xlabel('Time (s)','FontSize', FontSize ), ylabel('TILT: Q2 (deg)','FontSize', FontSize ), legend('q2d','q2r') xlim(XLim),ylim(YLim) set(gca,'FontSize',FontSize ); end figure('Name', 'XX/YY - ERROR','Color',[1 1 1])%,'FontSize', 12) subplot(2,1,1), plot(t,ERR_img(1,:),'r') xlabel('Time (s)','FontSize', 12), ylabel('XX-err (m)','FontSize', 12), title('Arrow Centroid Projected on the Image Plane','FontSize', 12) xlim([17 35]),set(gca,'FontSize',12); subplot(2,1,2), plot(t,ERR_img(2,:),'r') xlabel('Time (s)','FontSize', 12), ylabel('YY-err (m)','FontSize', 12) xlim([17 35]),set(gca,'FontSize',12); figure('Name', 'PAN/TILT - ERROR','Color',[1 1 1]) subplot(2,1,1), plot(t,(Qd(1,:)-Q(1,:))*180/pi,'r') xlabel('Time (s)','FontSize', 12), ylabel('PAN err (deg)','FontSize', 12) set(gca,'FontSize',12),xlim([17 35]); subplot(2,1,2), plot(t,(Qd(2,:)-Q(2,:))*180/pi,'r') xlabel('Time (s)','FontSize', 12), ylabel('TILT err (deg)','FontSize', 12) set(gca,'FontSize',12),xlim([17 35]); %______________________________________________________________ % MSE calculos %Time valTime = [11.64 21.19];% => timeExe = t(414)-t(28); % Time of execution % Error real trajectory % Pan panErrorRealTrajA = (Q(1,1:1154)*180/pi);% Setpoint panErrorRealTrajB = ((-Qr(1,:)+1454)/10);% Real traj panErrorRealTrajBinterpol = interp1(tr,panErrorRealTrajB,t(1:1154))'; panErrorRealTraj = panErrorRealTrajA - panErrorRealTrajBinterpol; % Tilt tiltErrorRealTrajA = (Q(2,1:1154)*180/pi);% Setpoint tiltErrorRealTrajB = ((-Qr(2,:)+1862)/10);% Real traj tiltErrorRealTrajBinterpol = interp1(tr,tiltErrorRealTrajB,t(1:1154))'; tiltErrorRealTraj = tiltErrorRealTrajA - tiltErrorRealTrajBinterpol; panErrorRealTrajMse = sum((panErrorRealTraj(28:414)').^2)/timeExe tiltErrorRealTrajMse = sum((tiltErrorRealTraj(28:414)').^2)/timeExe % Arrow Centroid Projected on the Image Plane xxError = ERR_img(1,28:414);% Get valid data yyError = ERR_img(2,28:414); xxErrorMse = sum(xxError.^2)/timeExe yyErrorMse = sum(yyError.^2)/timeExe % angle error of PTU panError = Qd(1,:)*180/pi - Q(1,:)*180/pi;% Qd(1,:)-Q(1,:); panError = panError(1,28:414); % Get valid data tiltError = Qd(2,:)*180/pi - Q(2,:)*180/pi; tiltError = tiltError(1,28:414); % Get valid data panMseError = sum(panError.^2)/timeExe tiltMseError = sum(tiltError.^2)/timeExe %______________________________________________________________ % Plot the difference between ideal, real setpoint, and real trajectory figure; get(0,'Factory');set(0,'defaultfigurecolor',[1 1 1]);%Para que é que isto serve?!! subplot(2,1,1), plot(t(1:1154),panErrorRealTraj,[0 t(28:414)' 30],[0 panError 0]), xlim([11 24]) xlabel('Time (s)','FontSize', 12), ylabel('PAN (deg)','FontSize', 12), title('Difference between Ideal Set Point, Calculated Set Point and Real Trajectory','FontSize', 12), legend('Diff Calc Set Point vs Real Trajectory','Diff Idial Set Point vs Calc Set Point'), set(gca,'FontSize',12),xlim([17 35]); subplot(2,1,2), plot(t(1:1154),tiltErrorRealTraj,[0 t(28:414)' 30],[0 tiltError 0]), xlim([11 24]),ylim([-10 10]) xlabel('Time (s)','FontSize', 12), ylabel('TILT (deg)','FontSize', 12), legend('Diff Calc Set Point vs Real Trajectory','Diff Idial Set Point vs Calc Set Point'), set(gca,'FontSize',12),xlim([17 35]); % Plot Pand and Tilt diference between real trajectory and calculated with % kinematics figure; plot(t(1:1154),panErrorRealTraj,t(1:1154),tiltErrorRealTraj),xlim([11 24])