%______________________________________________________________ % 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('servos_position.txt'); FANUC_data = csvread('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)'; 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 rP_arrow = [1 0 0 1]'; % Arrow position in Sr if FANUC_flag == 1, fPptu = [fPptu ; ones(1,DIM)]; %fanuc para a seta rTf = [ 1 0 0 -614.086*1e-3%X 0 1 0 -5.95e-3%Y 0 0 1 5.07e-3%ZZ 0 0 0 1 ]; rPo = rTf*fPptu; rPo = rPo(1:3,:); else Pi = [-2 2 0.5]'; Pf = [-2 -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 %______________________________________________________________ Qd = zeros(2,DIM); for ii = 1:DIM, % Inverse kinematics _________________________ rTo = [eye(3) rPo(:,ii); 0 0 0 1]; 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) 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); 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 figure(1), clf hold on H = plot3(P_ptu(1,:),P_ptu(2,:),P_ptu(3,:),'ro-', 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.'); 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'), ylabel('YY'), zlabel('ZZ') axis equal, axis([-0.5 1.5 -0.5 0.5 -1 2]), view(EL,AZ), %grid on % Pan-tilt joint angles figure(2) subplot(2,1,1), plot(t(1:ii),Q(1,1:ii)*180/pi,'r.'), xlim([0 Texe]) xlabel('Time (s)'), ylabel('PAN: Q1 (deg)') subplot(2,1,2), plot(t(1:ii),Q(2,1:ii)*180/pi,'r.'), xlim([0 Texe]) xlabel('Time (s)'), ylabel('TILT: Q2 (deg)') if ii == 1, pause, [EL,AZ] = view; end pause(0.0001) end %______________________________________________________________ % Performance measure %______________________________________________________________ figure(3) subplot(2,1,1), plot(t,Qd(1,:)*180/pi,'r:', t,Q(1,:)*180/pi,'k', tr,(-Qr(1,:)+1450)/10,'b') xlabel('Time (s)'), ylabel('PAN: Q1 (deg)'), legend('q1d','q1','q1r') xlim([13 21]) subplot(2,1,2), plot(t,Qd(2,:)*180/pi,'r:', t,Q(2,:)*180/pi,'k', tr,(-Qr(2,:)+1900)/10+33.675,'b') xlabel('Time (s)'), ylabel('TILT: Q2 (deg)'), legend('q2d','q2','q2r') xlim([13 21]) figure(4) subplot(2,1,1), plot(t,(Qd(1,:)-Q(1,:))*180/pi,'r') xlabel('Time (s)'), ylabel('PAN err (deg)') subplot(2,1,2), plot(t,(Qd(2,:)-Q(2,:))*180/pi,'r') xlabel('Time (s)'), ylabel('TILT err (deg)') figure(5) subplot(2,1,1), plot(t,ERR_img(1,:),'r') xlabel('Time (s)'), ylabel('XX-err (m)'), title('Arrow Centroid Projected on the Image Plane') subplot(2,1,2), plot(t,ERR_img(2,:),'r') xlabel('Time (s)'), ylabel('YY-err (m)') %______________________________________________________________