112 lines
3.4 KiB
Matlab
112 lines
3.4 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-p1.csv', 1, 250);
|
|
vldtnTrjctry = parseURData('ur-20_01_17-ptp_10_points.csv', 1, 5346);
|
|
|
|
vldtnTrjctry = filterData(vldtnTrjctry);
|
|
|
|
|
|
% -----------------------------------------------------------------------
|
|
% Predicting torques
|
|
% -----------------------------------------------------------------------
|
|
pi_rigidBody = reshape(identifiedUR10E.standardParameters,[11,6]);
|
|
pi_rigidBody = pi_rigidBody(1:10,:);
|
|
pi_rigidBody = reshape(pi_rigidBody,[60 1]);
|
|
|
|
%Constracting regressor matrix
|
|
tau_msrd = []; tau_prdctd1 = []; tau_prdctd2 = [];
|
|
i_prdct1 = []; i_prdct2 = [];
|
|
for i = 1:length(vldtnTrjctry.t)
|
|
Yi = regressorWithMotorDynamics(vldtnTrjctry.q(i,:)',...
|
|
vldtnTrjctry.qd_fltrd(i,:)',...
|
|
vldtnTrjctry.q2d_est(i,:)');
|
|
|
|
tau_withoutFriction = Yi*E1*identifiedUR10E.baseParameters;
|
|
|
|
tau_lnr_frcn = zeros(6,1);
|
|
% tau_nonlnr_frcn = zeros(6,1);
|
|
for j = 1:6
|
|
tau_lnr_frcn(j) = linearFrictionModel(pi_frctn(3*(j-1)+1:3*(j-1)+3),...
|
|
vldtnTrjctry.qd_fltrd(i,j)');
|
|
% tau_nonlnr_frcn(j) = nonlinearFrictionModel(pi_nonlnr_frcn(5*(j-1)+1:5*(j-1)+5),...
|
|
% vldtnTrjctry.qd_fltrd(i,j)');
|
|
end
|
|
tau_msrd = horzcat(tau_msrd, diag(drvGains)*vldtnTrjctry.i_fltrd(i,:)');
|
|
tau_prdctd1 = horzcat(tau_prdctd1, tau_withoutFriction + tau_lnr_frcn);
|
|
% tau_prdctd2 = horzcat(tau_prdctd2, tau_withoutFriction + tau_nonlnr_frcn);
|
|
|
|
i_prdct1 = horzcat(i_prdct1, diag(drvGains)\(tau_withoutFriction + tau_lnr_frcn));
|
|
% i_prdct2 = horzcat(i_prdct2, diag(drvGains)\(tau_withoutFriction + tau_nonlnr_frcn));
|
|
end
|
|
|
|
%%
|
|
|
|
for i = 1:6
|
|
figure
|
|
hold on
|
|
plot(vldtnTrjctry.t, tau_msrd(i,:), 'r-')
|
|
% plot(vldtnTrjctry.t, vldtnTrjctry.tau_des(:,i))
|
|
plot(vldtnTrjctry.t, tau_prdctd1(i,:),'k-')
|
|
legend('measured', 'desired', 'predicted')
|
|
grid on
|
|
end
|
|
return
|
|
for i = 1:6
|
|
figure
|
|
hold on
|
|
plot(vldtnTrjctry.t, vldtnTrjctry.i(:,i),'r-')
|
|
plot(vldtnTrjctry.t, i_prdct1(i,:),'k-')
|
|
plot(vldtnTrjctry.t, i_prdct2(i,:),'b-')
|
|
legend('real','with linear friction', 'with nonlinear friction')
|
|
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
|
|
|
|
|
|
|