%VARLOG plot the registered simulation state variables
% VARLOG var_type reads the simulation state variables during the whole
% simulation from the simulation log file and plot them
% VARLOG var_type t_ini and t_end only reads the registers from
% t_ini to t_end
% var_type is the variables (file columns) which must be plotted.
% var_type must be: ti, in, st, to, tt, ou, er, le, ma
% ti: Plot consumed computation-time information
% (only one trajectory excution is plotted)
% in: Plot the desired error posistion, velocity and
% acceleration (one execution)
% st: Plot the desired error versus the actual error posistion,
% ou: Plot the the corrective cerebellar output torque
% er: Plot the robot's performed error per joint
% le: Plot the learning signal computed from the robot's error
% ma: Plot the mean average error per trajectory execution
% gain: Plot the VOR gain per trajectory execution
% phase: Plot the VOR phase per trajectory execution
% fft: Plot the desired fft error versus actual fft error
% example to plot the robot state in the time interval 0 1: varlog_reduced_VOR st 0 1
%
% Copyright (C) 2014 by Richard R. Carrillo and Niceto R. Luque
% $Revision: 1.1 $ $Date: 31/11/2015 $
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later versio
function varlog_reduced_VOR(varargin)
LOG_FILE='vars.dat'; % log file name
single_exec_dur=1; % trajectoy duration in seconds
num_joints=1; % number of robot's joints
sim_slot_time=2e-3; % simulation slot length in seconds
nargs=nargin;
if nargin > 0
v=varargin{1};
if ~isequal(v,'ti') && ~isequal(v,'in') && ~isequal(v,'st') && ~isequal(v,'ou') && ~isequal(v,'er') && ~isequal(v,'le') && ~isequal(v,'ma') && ~isequal(v,'gain') && ~isequal(v,'phase') && ~isequal(v,'fft')
display('Incorrect value of the first argument');
nargs=0;
end
end
switch(nargs)
case 1
disp(['loading log file: ' LOG_FILE ' completely']);
disp(' 100%');
vars=load(LOG_FILE);
case 3
start_time=str2num(varargin{2});
end_time=str2num(varargin{3});
disp(['Partially loading log file: ' LOG_FILE ' time from ' num2str(start_time) ' to ' num2str(end_time)]);
fid = fopen(LOG_FILE,'rt');
if fid ~= -1
fseek(fid,-1,'eof');
filelength=ftell(fid); % get log file size
findline_backwards(fid); % set file reading pointer to the line start
last_line_txt=fgetl(fid); % load last register
last_line=str2num(last_line_txt);
last_file_time=last_line(1); % last register time
tot_num_cols=length(last_line); % total number of columns in the file
fseek(fid,fix(filelength*(start_time/last_file_time)),'bof');
findline_backwards(fid);
vars=read_file_part(fid,start_time,end_time);
fclose(fid);
if last_file_time < start_time
disp('Error: The specified start_time is no included in the file')
return
end
else
disp(['Cannot open log file: ' LOG_FILE]);
end
otherwise
disp('You must specify 1 or 3 arguments');
disp('varlog var_type [start_time end_time]');
disp('type: help varlog for more help');
return
end
num_input_vars=3*num_joints;
num_state_vars=3*num_joints;
num_torque_vars=num_joints;
num_output_vars=2*num_joints;
num_learning_vars=2*num_joints;
num_error_vars=num_joints;
num_fft_vars=3*num_joints;
time_col=1;
consum_col=time_col+1;
spikes_col=consum_col+1;
input_col=spikes_col+1;
state_col=input_col+num_input_vars;
torque_col=state_col+num_state_vars;
output_col=torque_col+num_torque_vars;
learning_col=output_col+num_output_vars;
error_col=learning_col+num_learning_vars;
fft_col=input_col+num_fft_vars;
tot_num_cols=error_col+num_error_vars;
reg_times=vars(:,time_col);
last_1exec_reg=find(reg_times > reg_times(1)+single_exec_dur,1);
if isempty(last_1exec_reg)
last_1exec_reg=length(reg_times);
end
switch(v)
case 'ti'
subplot(2,1,1),stairs(reg_times,vars(:,consum_col)*1e3)
ylabel('consumed time (ms)')
subplot(2,1,2),stairs(reg_times,vars(:,spikes_col))
ylabel('generated spikes')
xlabel('time (s)')
case 'in'
title('Input vars')
for ivar=1:num_input_vars,
subplot(3,num_input_vars/3,ivar),plot(reg_times(1:last_1exec_reg),vars(1:last_1exec_reg,input_col+ivar-1))
cur_ax=axis;
axis([reg_times(1) reg_times(last_1exec_reg) cur_ax(3) cur_ax(4)]);
ylabel(['in:' num2str(ivar)])
end
xlabel('time (s)')
case 'st'
title('State vars')
for ivar=1:num_state_vars,
subplot(3,num_state_vars/3,ivar),plot(reg_times,vars(:,input_col+ivar-1),'b',reg_times,vars(:,state_col+ivar-1),'r')
ylabel(['st:' num2str(ivar)])
end
xlabel('time (s)')
case 'ou'
title('Output vars')
for ivar=1:num_output_vars/2,
subplot(num_output_vars/2,2,(ivar-1)*2+1),plot(reg_times,vars(:,output_col+(ivar-1)*2),'b',reg_times,vars(:,output_col+(ivar-1)*2+1),'r')
ylabel(['out:' num2str(ivar)])
subplot(num_output_vars/2,2,(ivar-1)*2+2),plot(reg_times,vars(:,output_col+(ivar-1)*2)-vars(:,output_col+(ivar-1)*2+1))
end
xlabel('time (s)')
case 'le'
title('Learning vars')
for ivar=1:num_learning_vars,
subplot(num_learning_vars,1,ivar),plot(reg_times,vars(:,learning_col+ivar-1))
ylabel(['ler:' num2str(ivar)])
end
xlabel('time (s)')
case 'er'
title('Error vars')
for ivar=1:num_error_vars,
subplot(num_error_vars,1,ivar),plot(reg_times,vars(:,error_col+ivar-1))
ylabel(['err:' num2str(ivar)])
end
xlabel('time (s)')
case 'ma'
mae_evol=[];
cur_traj_start=reg_times(1);
while cur_traj_start < reg_times(end)
cur_traj_end=cur_traj_start + single_exec_dur-mod(cur_traj_start,single_exec_dur);
traj_reg_times_i=find(reg_times>=cur_traj_start & reg_times<cur_traj_end);
tra_mae=0;
for ivar=1:num_state_vars/3,
tra_mae=tra_mae+mae(vars(traj_reg_times_i,state_col+ivar-1)-vars(traj_reg_times_i,input_col+ivar-1));
end
mae_evol=[mae_evol [cur_traj_end;tra_mae ]];
cur_traj_start=cur_traj_end;
end
plot(mae_evol(1,:),mae_evol(2,:));
title('MAE evolution')
xlabel('time (s)')
case 'gain'
gain_evol=[];
cur_traj_start=reg_times(1);
f=linspace(0, 1/sim_slot_time, length(reg_times));
while cur_traj_start < reg_times(end)
cur_traj_end=cur_traj_start + single_exec_dur-mod(cur_traj_start,single_exec_dur);
traj_reg_times_i=find(reg_times>=cur_traj_start & reg_times<cur_traj_end);
tra_gain=0;
for ivar=1:num_fft_vars/3,
EYE=abs(FourierT(vars(traj_reg_times_i,state_col+ivar-1),sim_slot_time));
HEAD=abs(FourierT(vars(traj_reg_times_i,input_col+ivar-1),sim_slot_time));
tra_gain=max(EYE)/max(HEAD);
end
gain_evol=[gain_evol [cur_traj_end;tra_gain ]];
cur_traj_start=cur_traj_end;
end
stem(gain_evol(1,:),gain_evol(2,:),'diamondr');
title('GAIN Evolution')
xlabel('time (s)')
case 'phase'
phase_evol=[];
cur_traj_start=reg_times(1);
f=linspace(0, 1/sim_slot_time, length(reg_times));
while cur_traj_start < reg_times(end)
cur_traj_end=cur_traj_start + single_exec_dur-mod(cur_traj_start,single_exec_dur);
traj_reg_times_i=find(reg_times>=cur_traj_start & reg_times<cur_traj_end);
tra_phase=0;
for ivar=1:num_fft_vars/3,
EYE=vars(traj_reg_times_i,state_col+ivar-1);
HEAD=vars(traj_reg_times_i,input_col+ivar-1);
L=length(traj_reg_times_i)-1;
x = xcorr((EYE-mean(EYE)),HEAD,'coeff');
tx = [-L:L]*sim_slot_time;
[mx,ix] = max(x);
tra_phase=tx(ix);
end
phase_evol=[phase_evol [cur_traj_end;abs(tra_phase*360)]];
cur_traj_start=cur_traj_end;
end
stem(phase_evol(1,:),phase_evol(2,:),'diamondr');
title('LAG Evolution')
xlabel('Time (s)')
case 'fft'
title('FFT')
pair=0;
f=linspace(0, 1/sim_slot_time, length(reg_times));
for ivar=1:num_fft_vars,
s1=vars(:,input_col+ivar-1);
s2=vars(:,fft_col+ivar-1);% CAREFUL THIS IS FOR VOR
subplot(num_fft_vars,3,ivar+pair),plot(reg_times,s1,'b',reg_times,-s2,'r')
ylabel(['FFT aimed signals:' num2str(ivar)])
xlabel('time (s)')
grid on
hold on
%fourier transforms
fs1=abs(FourierT(vars(:,input_col+ivar-1),sim_slot_time));
fs2=abs(FourierT(vars(:,fft_col+ivar-1),sim_slot_time));
subplot(num_fft_vars,3,ivar+pair+1),stem(f,fs1,'b'),hold on, stem(f,fs2,'r') %plot(f,fs1,'b',f,fs2,'r')
ylabel(['fft:' num2str(ivar)])
xlabel('Hz')
grid on
S = sprintf('Gain first harmonic S1= %5.2f vs S2=%5.2f',max(fs1),max(fs2));
title(S)
%xcorr lag_phase
%
% Now cross-correlate the two signals
%
L=length(reg_times)-1;
x = xcorr(s1,-(s2-mean(s2)),'coeff');
tx = [-L:L]*sim_slot_time;
%
% Determine the lag
%
[mx,ix] = max(x);
lag = tx(ix);
tm = [lag,lag];
mm = [-1,1];
subplot(num_fft_vars,3,ivar+pair+2),plot(tx,x,'b',tm,mm,'k')
grid
% Note that the lag is only as close as the time resolution.
T = sprintf('xCorr Lag = %5.2f',lag);
title(T)
pair=pair+2;
end
end
% READ_LINE_TIME gets the time of the next register from the simulation-log file.
% REGTIME = READ_FILE_PART(FID) advances the file position indicator in the
% file associated with the given FID to the beginning of the next text
% line, then read and returns that register's time.
function regtime=read_line_time(fid)
time_read=0;
regtime=-1;
while time_read==0
[regtime,time_read]=fscanf(fid,'%g',1);
if time_read==0
fgetl(fid);
end
end
end
% FINDLINE_BACKWARDS move the a file pointer back to the beginning of the
% previous text line.
% FINDLINE_BACKWARDS(FID) repositions the file position indicator in the
% file associated with the given FID. FINDLINE_BACKWARDS sets the
% position indicator to the byte at beginning of the line before the
% current one.
function findline_backwards(fid)
newline=sprintf('\n');
tchar=' '; % look for the current-file-line start position
while ~isempty(tchar) && isempty(strfind(tchar,newline))
if fseek(fid,-2,'cof')==-1
break
end
tchar = fscanf(fid,'%1c',1);
end
end
% READ_FILE_PART loads registers from the simulation-log file.
% REGS = READ_FILE_PART(FID,STARTTIME,ENDTIME) returns the registers of
% a file associated with file identifier FID as a MATLAB matrix. Only
% the registers from STARTTIME to ENDTIME are loaded.
function regs=read_file_part(fid,starttime,endtime)
disp(' Searching for specified registers in the file...')
while read_line_time(fid) > starttime
findline_backwards(fid); % go back to the current line start
fseek(fid,-1,'cof'); % jump before \n
findline_backwards(fid); % go back to the previous line start
end
findline_backwards(fid);
while read_line_time(fid) < starttime
fgetl(fid); % jump after \n
end
findline_backwards(fid);
disp(' Loading registers from the file...')
disp(' 00%')
app_regs_size=(endtime-starttime)/sim_slot_time; % estimated size for regs
regs=zeros(ceil(app_regs_size),tot_num_cols); % allocate matrix memory (for execution time optmization)
regs_size=0;
cur_file_time=starttime;
tline=' ';
while cur_file_time < endtime && ischar(tline) && ~isempty(tline)
tline=fgetl(fid);
if ischar(tline) && ~isempty(tline) && isempty(~strfind(tline,'%'))
vline=str2num(tline);
regs_size=regs_size+1;
regs(regs_size,:)=vline;
cur_file_time=vline(1);
if mod(regs_size,fix(app_regs_size/100)) == 0
fprintf(1,'\b\b\b\b% 3.f%%',(regs_size/app_regs_size)*100);
end
end
end
regs=regs(1:regs_size,:); % trim array in case of initial overdimensioning
fprintf(1,'\b\b\b\b100%%\n');
end
function y = FourierT(x, dt)
% FourierT(x,dt) computes forward FFT of x with sampling time interval dt
% FourierT approximates the Fourier transform where the integrand of the
% transform is x*exp(2*pi*i*f*t)
% For VOR applications the frequency components are normally in kHz,
% dt in milliseconds
[nr, nc] = size(x);
if nr == 1,
N = nc;
else
N = nr;
end
y = N*dt*ifft(x);
end
end
