64 lines
2.6 KiB
Matlab
64 lines
2.6 KiB
Matlab
% ------------------------------------------------------------------------
|
|
% Load validation trajectory
|
|
% ------------------------------------------------------------------------
|
|
|
|
vldtnTrjctry = parseURData('ur-19_10_01-14_04_13.csv', 1, 759);
|
|
% vldtnTrjctry = parseURData('ur-19_10_01-13_51_41.csv', 1, 660);
|
|
% vldtnTrjctry = parseURData('ur-19_09_27-11_28_22.csv', 1, 594);
|
|
% vldtnTrjctry = parseURData('ur-20_01_17-p8.csv', 1, 250);
|
|
% vldtnTrjctry = parseURData('ur-20_01_17-ptp_10_points.csv', 1, 5346);
|
|
% vldtnTrjctry = parseURData('ur-19_12_23_free.csv', 1, 2005);
|
|
vldtnTrjctry = filterData(vldtnTrjctry);
|
|
|
|
|
|
% -----------------------------------------------------------------------
|
|
% Predicting torques
|
|
% -----------------------------------------------------------------------
|
|
%Constracting regressor matrix
|
|
tau_msrd = []; tau_OLS = []; tau_SDP = [];
|
|
i_OLS1 = []; i_OLS2 = []; i_OLS3 = [];
|
|
i_SPD1 = []; i_SPD2 = []; i_SPD3 = [];
|
|
for i = 1:length(vldtnTrjctry.t)
|
|
Yi = regressorWithMotorDynamics(vldtnTrjctry.q(i,:)',...
|
|
vldtnTrjctry.qd_fltrd(i,:)',...
|
|
vldtnTrjctry.q2d_est(i,:)');
|
|
|
|
Ybi = Yi*baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
|
|
Yfrctni = frictionRegressor(vldtnTrjctry.qd_fltrd(i,:)');
|
|
|
|
% tau_msrd = horzcat(tau_msrd, diag(drvGains)*vldtnTrjctry.i(i,:)');
|
|
% tau_OLS = horzcat(tau_OLS, [Ybi Yfrctni]*[pib_OLS; pifrctn_OLS]);
|
|
% tau_SDP = horzcat(tau_SDP, [Ybi Yfrctni]*[pib_SDP; pifrctn_SDP]);
|
|
|
|
i_OLS1 = horzcat(i_OLS1, diag(drvGains)\([Ybi Yfrctni]*[pib_OLS(:,1); pifrctn_OLS(:,1)]));
|
|
i_OLS2 = horzcat(i_OLS2, diag(drvGains2)\([Ybi Yfrctni]*[pib_OLS(:,2); pifrctn_OLS(:,2)]));
|
|
i_OLS3 = horzcat(i_OLS3, diag(drvGains3)\([Ybi Yfrctni]*[pib_OLS(:,3); pifrctn_OLS(:,3)]));
|
|
|
|
i_SPD1 = horzcat(i_SPD1, diag(drvGains)\([Ybi Yfrctni]*[pib_SDP(:,1); pifrctn_SDP(:,1)]));
|
|
i_SPD2 = horzcat(i_SPD2, diag(drvGains2)\([Ybi Yfrctni]*[pib_SDP(:,2); pifrctn_SDP(:,2)]));
|
|
i_SPD3 = horzcat(i_SPD3, diag(drvGains3)\([Ybi Yfrctni]*[pib_SDP(:,3); pifrctn_SDP(:,3)]));
|
|
end
|
|
|
|
%%
|
|
|
|
for i = 1:6
|
|
figure
|
|
hold on
|
|
plot(vldtnTrjctry.t, vldtnTrjctry.i(:,i), 'r-')
|
|
plot(vldtnTrjctry.t, i_OLS1(i,:), 'k-', 'LineWidth',1.5)
|
|
plot(vldtnTrjctry.t, i_OLS2(i,:), 'k--', 'LineWidth',1.5)
|
|
plot(vldtnTrjctry.t, i_OLS3(i,:), 'k-.', 'LineWidth',1.5)
|
|
plot(vldtnTrjctry.t, i_SPD1(i,:), 'b-', 'LineWidth',1.5)
|
|
plot(vldtnTrjctry.t, i_SPD2(i,:), 'b--', 'LineWidth',1.5)
|
|
plot(vldtnTrjctry.t, i_SPD3(i,:), 'b-.', 'LineWidth',1.5)
|
|
legend('measured', 'OLS1', 'OLS2', 'OLS3', 'SDP1', 'SDP2', 'SDP3')
|
|
grid on
|
|
end
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|