prefix='delaware3_ss_walking1';
prefix2='delaware3';

%q1=read_motionFile(sprintf('Results/%s_ik_Kinematics_q.sto',prefix));
%q2=read_motionFile(sprintf('Results/%s_ik_rra_Kinematics_q.sto',prefix));
%q2=read_motionFile(sprintf('%s_ik.mot',prefix));
%q2=read_motionFile(sprintf('../test3/Results/checkPositions.sto'));

%q1=read_motionFile('Results/900045_15_ik_Actuation_force.sto');
%q2=read_motionFile('Results/900045_15_ik_rra_Actuation_force.sto');

%q1=read_motionFile(sprintf('Results/%s_ik_Kinematics_q.sto',prefix));
%q2=read_motionFile(sprintf('ResultsCMC/%s_Kinematics_q.sto',prefix2));

%q1=read_motionFile(sprintf('Results/%s_ik_rra_Kinematics_dudt.sto',prefix));
%q2=read_motionFile(sprintf('ResultsCMC/%s_Kinematics_dudt.sto',prefix2));

%q1=read_motionFile(sprintf('Results/%s_ik_Kinematics_q.sto',prefix));
%q2=read_motionFile(sprintf('ResultsForward/%s_Kinematics_q.sto',prefix2));

%q1=read_motionFile(sprintf('ResultsCMC/%s_Kinematics_q.sto',prefix2));
%q2=read_motionFile(sprintf('ResultsForward/%s_Kinematics_q.sto',prefix2));

%q1=read_motionFile(sprintf('ResultsCMC/%s_Kinematics_dudt.sto',prefix2));
%q2=read_motionFile(sprintf('../delaware3_test/ResultsCMC/%s_Kinematics_dudt.sto',prefix2));

%q1=read_motionFile(sprintf('Results/%s_ik_rra_Kinematics_u.sto',prefix));
%q2=read_motionFile(sprintf('../delaware3_test/Results/%s_ik_rra_Kinematics_u.sto',prefix));

%q1=read_motionFile(sprintf('Results/%s_ik_rra_Kinematics_q.sto',prefix));
%q2=read_motionFile(sprintf('../delaware3_test/Results/%s_ik_rra_Kinematics_q.sto',prefix));

%q1=read_motionFile(sprintf('ResultsCMC/%s_Kinematics_q.sto',prefix2));
%q2=read_motionFile(sprintf('ResultsCMC/tmp/%s_Kinematics_q.sto',prefix2));
%q2=read_motionFile(sprintf('../delaware3_test/ResultsCMC/%s_Kinematics_q.sto',prefix2));
%q2=read_motionFile(sprintf('../delaware3/ResultsCMC/%s_Kinematics_q.sto',prefix2));

%q1=read_motionFile(sprintf('Results/%s_ik_Actuation_force.sto',prefix));
%q2=read_motionFile(sprintf('linux_results/Results/%s_ik_Actuation_force.sto',prefix));

%q1=read_motionFile(sprintf('Results/%s_ik_Kinematics_q.sto',prefix));
%q2=read_motionFile(sprintf('linux_results/Results/%s_ik_Kinematics_q.sto',prefix));

q1=read_motionFile(sprintf('ResultsCMC/%s_controls.sto',prefix2));
%q2=read_motionFile(sprintf('ResultsCMC/tmp/%s_Actuation_force.sto',prefix2));
q2=read_motionFile(sprintf('../delaware3_testcontrolsnoextrapolate/ResultsCMC/%s_controls.sto',prefix2));

%q1=read_motionFile(sprintf('ResultsCMC/%s_Actuation_force.sto',prefix2));
%q2=read_motionFile(sprintf('ResultsCMC/tmp/%s_Actuation_force.sto',prefix2));
%q2=read_motionFile(sprintf('../delaware3_test/ResultsCMC/%s_Actuation_force.sto',prefix2));
%q2=read_motionFile(sprintf('../delaware3_testcontrolsnoextrapolate/ResultsCMC/%s_Actuation_force.sto',prefix2));
%q2=read_motionFile(sprintf('../delaware3/ResultsCMC/%s_Actuation_force.sto',prefix2));

%q1=read_motionFile(sprintf('ResultsCMC/%s_Actuation_force.sto',prefix2));
%q2=read_motionFile(sprintf('ResultsForward/%s_Actuation_force.sto',prefix2));

%q1=read_motionFile(sprintf('ResultsCMC/%s_Actuation_force.sto',prefix2));
%q2=read_motionFile(sprintf('output_success/ResultsCMC/%s_Actuation_force.sto',prefix2));

%q1=read_motionFile('controls_cmc.sto');
%q2=read_motionFile('controls_forward.sto');

%q1=read_motionFile('states_cmc.sto');
%q2=read_motionFile('states_forward.sto');

tmin=max(q1.data(1,1),q2.data(1,1));
tmax=min(q1.data(end,1),q2.data(end,1));
%tmax=0.6;
T=tmin:0.0001:tmax;

columns=2:size(q1.data,2);

d1=interp1(q1.data(:,1),q1.data(:,columns),T);
d2=interp1(q2.data(:,1),q2.data(:,columns),T);

D=max(abs(d1-d2));
for i=1:length(D)
	disp(sprintf('%s = %f',q1.labels{1+i},D(i)));
end

DDtr=max(abs(d1(:,1:3)-d2(:,1:3)),[],2);
DDan=max(abs(d1(:,4:end)-d2(:,4:end)),[],2);

plot(q1.data(:,1),q1.data(:,2:end));
hold on;
plot(q2.data(:,1),q2.data(:,2:end));
hold off

figure
plot(T,d1-d2);
legend(q1.labels(columns),'Interpreter','none');
