154 lines
4.9 KiB
Matlab
154 lines
4.9 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
|
|
% ------------------------------------------------------------------------
|
|
% wf = 2*pi/traj_par.T; % main frequency of the data
|
|
% wc = 10*wf*traj_par.N/2/pi;
|
|
|
|
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)
|
|
Yb_ulddi = base_regressor_UR10E(q_msrd(i,:)',...
|
|
qd_fltrd(i,:)',q2d_est(i,:)');
|
|
Yfrctni = ur10_frctn_rgsr(qd_fltrd(i,:)');
|
|
Ydrvi = ur10_drv_rgsr(q2d_est(i,:)');
|
|
Wb_uldd = vertcat(Wb_uldd,[Yb_ulddi, Ydrvi, Yfrctni]);
|
|
tau_msrd = horzcat(tau_msrd, diag(drv_gns)*i_msrd(i,:)');
|
|
tau_prdctd = horzcat(tau_prdctd, [Yb_ulddi, Ydrvi, Yfrctni]*...
|
|
[pi_b; pi_rtr; pi_frctn]);
|
|
end
|
|
|
|
%%
|
|
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
|
|
|
|
|
|
return
|
|
% ------------------------------------------------------------------------
|
|
% Predicting currents
|
|
% -----------------------------------------------------------------------
|
|
I = cell(1,6);
|
|
|
|
for i = 1:length(t_msrd)
|
|
Yi = base_regressor(q_msrd(i,:)',qd_fltrd(i,:)',q2d_est(i,:)');
|
|
% Complementing regressor with friction terms
|
|
% drvi = ur10_drv_rgsr(qd_fltrd(i,:)');
|
|
drvi = ur10_drv_rgsr(q2d_est(i,:)');
|
|
frctni = ur10_frctn_rgsr(qd_fltrd(i,:)');
|
|
|
|
Yi_l1 = Yi(:,1);
|
|
Yi_l2 = Yi(:,2:8);
|
|
Yi_l3 = Yi(:,9:15);
|
|
Yi_l4 = Yi(:,16:22);
|
|
Yi_l5 = Yi(:,23:29);
|
|
Yi_l6 = Yi(:,30:36);
|
|
|
|
% Yi_j2 = [Yi_l2(2,:), drvi(2), Yi_l3(2,:), Yi_l4(2,:), Yi_l5(2,:), Yi_l6(2,:)];
|
|
Yi_j2 = [Yi_l2(2,:), Yi_l3(2,:), Yi_l4(2,:), Yi_l5(2,:), Yi_l6(2,:)];
|
|
Yi_j3 = [Yi_l3(3,:), drvi(3), Yi_l4(3,:), Yi_l5(3,:), Yi_l6(3,:)];
|
|
Yi_j4 = [Yi_l4(4,:), drvi(4), Yi_l5(4,:), Yi_l6(4,:)];
|
|
Yi_j5 = [Yi_l5(5,:), drvi(5), Yi_l6(5,:)];
|
|
Yi_j6 = [Yi_l6(6,:), drvi(6)];
|
|
|
|
% Computing currents
|
|
% I{1} = vertcat(I{1},[Yi(1,n1:4),Yi(1,6:11),Yi(1,13:18),Yi(1,20:end) frctn1i]*pi1_hat);
|
|
% I{2} = vertcat(I{2},[Yi(2,n2:end),frctn2i]*pi2_hat);
|
|
% I{3} = vertcat(I{3},[Yi(3,n3:end),frctn3i]*pi3_hat);
|
|
% I{4} = vertcat(I{4},[Yi(4,n4:end),frctn4i]*pi4_hat);
|
|
% I{5} = vertcat(I{5},[Yi(5,n5:end),frctn5i]*pi5_hat);
|
|
% I{6} = vertcat(I{6},[Yi(6,n6:end),frctn6i]*pi6_hat);
|
|
|
|
I{2} = vertcat(I{2},[Yi_j2,frctni(2,:)]*pi2_hat);
|
|
I{3} = vertcat(I{3},[Yi_j3,frctni(3,:)]*pi3_hat);
|
|
I{4} = vertcat(I{4},[Yi_j4,frctni(4,:)]*pi4_hat);
|
|
I{5} = vertcat(I{5},[Yi_j5,frctni(5,:)]*pi5_hat);
|
|
I{6} = vertcat(I{6},[Yi_j6,frctni(6,:)]*pi6_hat);
|
|
|
|
end
|
|
|
|
for i = 2:6
|
|
figure
|
|
plot(t_msrd,i_msrd(:,i))
|
|
hold on
|
|
plot(t_msrd,I{i})
|
|
end
|