119 lines
3.3 KiB
Matlab
119 lines
3.3 KiB
Matlab
% ------------------------------------------------------------------------
|
|
% Load validation trajectory
|
|
% ------------------------------------------------------------------------
|
|
% vldtn_data = csvread('ur-19_10_01-13_51_41.csv');
|
|
vldtn_data = csvread('ur-19_10_01-14_04_13.csv');
|
|
|
|
t_msrd = vldtn_data(:,1) - vldtn_data(1,1); %subtract offset
|
|
q_msrd = vldtn_data(:,2:7);
|
|
qd_msrd = vldtn_data(:,8:13);
|
|
i_msrd = vldtn_data(:,14:19);
|
|
i_des = vldtn_data(:,20:25);
|
|
tau_des = vldtn_data(:,26:31);
|
|
|
|
% ------------------------------------------------------------------------
|
|
% Filtering Velocities
|
|
% ------------------------------------------------------------------------
|
|
vel_filt = designfilt('lowpassiir','FilterOrder',3, ...
|
|
'HalfPowerFrequency',0.2,'DesignMethod','butter');
|
|
|
|
qd_fltrd = zeros(size(qd_msrd));
|
|
for i = 1:6
|
|
qd_fltrd(:,i) = filtfilt(vel_filt,qd_msrd(:,i));
|
|
end
|
|
|
|
% ------------------------------------------------------------------------
|
|
% Estimating accelerations
|
|
% ------------------------------------------------------------------------
|
|
q2d_est = zeros(size(qd_fltrd));
|
|
|
|
% Three point central difference
|
|
for i = 2:length(qd_fltrd)-1
|
|
dlta_qd_fltrd = qd_fltrd(i+1,:) - qd_fltrd(i-1,:);
|
|
dlta_t_msrd = t_msrd(i+1) - t_msrd(i-1);
|
|
q2d_est(i,:) = dlta_qd_fltrd/dlta_t_msrd;
|
|
end
|
|
|
|
% Zeros phase filtering acceleration obtained by finite difference
|
|
accel_filt = designfilt('lowpassiir','FilterOrder',5, ...
|
|
'HalfPowerFrequency',0.05,'DesignMethod','butter');
|
|
for i = 1:6
|
|
q2d_est(:,i) = filtfilt(accel_filt,q2d_est(:,i));
|
|
end
|
|
|
|
|
|
|
|
% -----------------------------------------------------------------------
|
|
% Predicting torques
|
|
% -----------------------------------------------------------------------
|
|
%Constracting regressor matrix
|
|
Wb_uldd = []; tau_msrd = []; tau_prdctd = [];
|
|
for i = 1:length(t_msrd)
|
|
Y_ulddi = regressorWithMotorDynamics(q_msrd(i,:)',...
|
|
qd_fltrd(i,:)',q2d_est(i,:)');
|
|
% Yfrctni = frictionRegressor(qd_fltrd(i,:)');
|
|
% Yb_ulddi = [Y_ulddi*E1, Yfrctni];
|
|
%
|
|
% Wb_uldd = vertcat(Wb_uldd, Yb_ulddi);
|
|
% tau_msrd = horzcat(tau_msrd, diag(drvGains)*i_msrd(i,:)');
|
|
% tau_prdctd = horzcat(tau_prdctd, Yb_ulddi*[pi_b; pi_frctn]);
|
|
|
|
Yb_ulddi = Y_ulddi*E1;
|
|
Wb_uldd = vertcat(Wb_uldd, Yb_ulddi);
|
|
tau_msrd = horzcat(tau_msrd, diag(drvGains)*i_msrd(i,:)');
|
|
tau_prdctd = horzcat(tau_prdctd, Yb_ulddi*pi_b);
|
|
end
|
|
|
|
%%
|
|
for i = 1:6
|
|
figure
|
|
hold on
|
|
plot(t_msrd, tau_msrd(i,:),'r-')
|
|
plot(t_msrd,tau_prdctd(i,:),'k-')
|
|
grid on
|
|
end
|
|
|
|
return
|
|
fig = figure;
|
|
subplot(3,2,1)
|
|
plot(t_msrd,tau_msrd(1,:),'r-')
|
|
hold on
|
|
plot(t_msrd,tau_prdctd(1,:),'k-')
|
|
ylabel('\tau_1')
|
|
grid on
|
|
subplot(3,2,2)
|
|
plot(t_msrd,tau_msrd(2,:),'r-')
|
|
hold on
|
|
plot(t_msrd,tau_prdctd(2,:),'k-')
|
|
ylabel('\tau_2')
|
|
grid on
|
|
subplot(3,2,3)
|
|
plot(t_msrd,tau_msrd(3,:),'r-')
|
|
hold on
|
|
plot(t_msrd,tau_prdctd(3,:),'k-')
|
|
ylabel('\tau_3')
|
|
grid on
|
|
subplot(3,2,4)
|
|
plot(t_msrd,tau_msrd(4,:),'r-')
|
|
hold on
|
|
plot(t_msrd,tau_prdctd(4,:),'k-')
|
|
ylabel('\tau_4')
|
|
grid on
|
|
subplot(3,2,5)
|
|
plot(t_msrd,tau_msrd(5,:),'r-')
|
|
hold on
|
|
plot(t_msrd,tau_prdctd(5,:),'k-')
|
|
ylabel('\tau_5')
|
|
xlabel('t')
|
|
grid on
|
|
subplot(3,2,6)
|
|
plot(t_msrd,tau_msrd(6,:),'r-')
|
|
hold on
|
|
plot(t_msrd,tau_prdctd(6,:),'k-')
|
|
ylabel('\tau_6')
|
|
xlabel('t')
|
|
grid on
|
|
|
|
|
|
|