%%%%%%%%%%%%%%%%%%
%
% plot_trajectories.m
%
% Plots data series of .traj files, explained in more detail at:
%
% http://jks-folks.stanford.edu/haptic_data/
%
% Filenames should be defined in the 'globals' section below.
%
% Uses the 'read_traj_file' function, also available at the
% above URL
%
% Author: Dan Morris
%
%%%%%%%%%%%%%%%%%%
 

%%%%%%%%%%%%%%%%%%
% GLOBALS
%%%%%%%%%%%%%%%%%%

% Filenames from which we'll read all of the data

% the raw position data, converted to .traj format
probefile = 'probe.traj';

% out-trajectory (the raw data projected onto the surface) (traj format)
outfile = 'out.traj';

% in-trajectory (the optimal haptic device path given the out-trajectory) (traj format)
infile = 'in.traj';

% the surface contact point path generated by CHAI's proxy (traj format)
resultfile = 'proxy.traj';


%%%%%%%%%%%%%%%%%%%
% DATA INPUT
%%%%%%%%%%%%%%%%%%%

% Read and parse the traj files
probedata = read_traj_file(probefile);
outdata = read_traj_file(outfile);
indata = read_traj_file(infile);
resultdata = read_traj_file(resultfile);

% We're going to restrict our graphs to the range of time
% used in the outfile and line up the graphs on that
% interval
mintime = outdata.t(1);
maxtime = outdata.t(end);


%%%%%%%%%%%%%%%
% GRAPHS
%%%%%%%%%%%%%%%

%%% POSITION GRAPHS %%%

figure(1);

subplot(2,2,1);
plot(probedata.t,probedata.x,'r',probedata.t,probedata.y,'g',probedata.t,probedata.z,'b');
legend('x','y','z');
xlabel('Seconds');
ylabel('millimeters');
title('Raw trajectory position (probe.traj)');
a = axis;
axis([mintime maxtime a(3) a(4)]);

subplot(2,2,2);
plot(outdata.t,outdata.x,'r',outdata.t,outdata.y,'g',outdata.t,outdata.z,'b');
legend('x','y','z');
xlabel('Seconds');
ylabel('millimeters');
title('Out-trajectory position (out.traj)');
a = axis;
axis([mintime maxtime a(3) a(4)]);

subplot(2,2,3);
plot(indata.t,indata.x,'r',indata.t,indata.y,'g',indata.t,indata.z,'b');
legend('x','y','z');
xlabel('Seconds');
ylabel('millimeters');
title('In-trajectory position (in.traj)');
a = axis;
axis([mintime maxtime a(3) a(4)]);

subplot(2,2,4);
plot(resultdata.t,resultdata.x,'r',resultdata.t,resultdata.y,'g',resultdata.t,resultdata.z,'b');
legend('x','y','z');
xlabel('Seconds');
ylabel('millimeters');
title('CHAI surface contact position (result.traj)');
a = axis;
axis([mintime maxtime a(3) a(4)]);

zoom on;

%%% FORCE GRAPHS %%%

figure(2);

subplot(2,2,1);
plot(probedata.t,probedata.forcex,'r',probedata.t,probedata.forcey,'g',probedata.t,probedata.forcez,'b');
legend('x','y','z');
xlabel('N');
ylabel('N');
title('Raw trajectory forces (probe.traj)');
a = axis;
axis([mintime maxtime a(3) a(4)]);

subplot(2,2,2);
plot(outdata.t,outdata.forcex,'r',outdata.t,outdata.forcey,'g',outdata.t,outdata.forcez,'b');
legend('x','y','z');
xlabel('N');
ylabel('N');
title('Out-trajectory forces (out.traj)');
a = axis;
axis([mintime maxtime a(3) a(4)]);

subplot(2,2,3);
plot(indata.t,indata.forcex,'r',indata.t,indata.forcey,'g',indata.t,indata.forcez,'b');
legend('x','y','z');
xlabel('N');
ylabel('N');
title('In-trajectory forces (in.traj)');
a = axis;
axis([mintime maxtime a(3) a(4)]);

subplot(2,2,4);
plot(resultdata.t,resultdata.forcex,'r',resultdata.t,resultdata.forcey,'g',resultdata.t,resultdata.forcez,'b');
legend('x','y','z');
xlabel('Seconds');
ylabel('Newtons');
title('CHAI output forces (result.traj)');
a = axis;
axis([mintime maxtime a(3) a(4)]);


zoom on;