184 lines
5.8 KiB
Matlab
184 lines
5.8 KiB
Matlab
clc; clear all; close all;
|
|
|
|
% ------------------------------------------------------------------------
|
|
% Load and calculate desired trajectory
|
|
% ------------------------------------------------------------------------
|
|
load('opt_sltn_N5T20.mat');
|
|
[q,qd,q2d] = mixed_traj(traj_par.t,c_pol,a,b,traj_par.wf,traj_par.N);
|
|
|
|
|
|
% ------------------------------------------------------------------------
|
|
% Load the real trajectory of the robot without load
|
|
% ------------------------------------------------------------------------
|
|
traj_data = csvread('ur-19_10_29-20_sec.csv');
|
|
|
|
int_idx = 256; % get the point when the trajectory begins
|
|
fnl_idx = 2158;
|
|
|
|
t_msrd = traj_data(int_idx:fnl_idx,1) - traj_data(int_idx,1); %subtract offset
|
|
q_msrd = traj_data(int_idx:fnl_idx,2:7);
|
|
qd_msrd = traj_data(int_idx:fnl_idx,8:13);
|
|
i_msrd = traj_data(int_idx:fnl_idx,14:19);
|
|
i_des = traj_data(int_idx:fnl_idx,20:25);
|
|
tau_des = traj_data(int_idx:fnl_idx,26:31);
|
|
|
|
|
|
% -----------------------------------------------------------------------
|
|
% Load the real trajectory of the robot with load
|
|
% -----------------------------------------------------------------------
|
|
traj_ldd = csvread('ur-19_11_05-20_sec_mass_2.csv');
|
|
|
|
int_idx_ldd = 8;
|
|
fnl_idx_ldd = 1877;
|
|
|
|
t_msrd_ldd = traj_ldd(int_idx_ldd:fnl_idx_ldd,1) - traj_ldd(int_idx_ldd,1); %subtract offset
|
|
q_msrd_ldd = traj_ldd(int_idx_ldd:fnl_idx_ldd,2:7);
|
|
qd_msrd_ldd = traj_ldd(int_idx_ldd:fnl_idx_ldd,8:13);
|
|
i_msrd_ldd = traj_ldd(int_idx_ldd:fnl_idx_ldd,14:19);
|
|
i_des_ldd = traj_ldd(int_idx_ldd:fnl_idx_ldd,20:25);
|
|
tau_des_ldd = traj_ldd(int_idx_ldd:fnl_idx_ldd,26:31);
|
|
|
|
|
|
% ------------------------------------------------------------------------
|
|
% Filtering Velocities
|
|
% ------------------------------------------------------------------------
|
|
% Design filter
|
|
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
|
|
|
|
qd_fltrd_ldd = zeros(size(qd_msrd_ldd));
|
|
for i = 1:6
|
|
qd_fltrd_ldd(:,i) = filtfilt(vel_filt,qd_msrd_ldd(:,i));
|
|
end
|
|
|
|
|
|
% ------------------------------------------------------------------------
|
|
% Estimating accelerations
|
|
% ------------------------------------------------------------------------
|
|
% Three point central difference
|
|
q2d_est = zeros(size(qd_fltrd));
|
|
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
|
|
|
|
q2d_est_ldd = zeros(size(qd_fltrd_ldd));
|
|
for i = 2:length(qd_fltrd_ldd)-1
|
|
dlta_qd_fltrd_ldd = qd_fltrd_ldd(i+1,:) - qd_fltrd_ldd(i-1,:);
|
|
dlta_t_msrd_ldd = t_msrd_ldd(i+1) - t_msrd_ldd(i-1);
|
|
q2d_est_ldd(i,:) = dlta_qd_fltrd_ldd/dlta_t_msrd_ldd;
|
|
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
|
|
|
|
for i = 1:6
|
|
q2d_est_ldd(:,i) = filtfilt(accel_filt,q2d_est_ldd(:,i));
|
|
end
|
|
|
|
% ------------------------------------------------------------------------
|
|
% Filtering current
|
|
% ------------------------------------------------------------------------
|
|
% Zeros phase filtering acceleration obtained by finite difference
|
|
curr_filt = designfilt('lowpassiir','FilterOrder',5, ...
|
|
'HalfPowerFrequency',0.1,'DesignMethod','butter');
|
|
|
|
for i = 1:6
|
|
i_fltrd(:,i) = filtfilt(curr_filt,i_msrd(:,i));
|
|
end
|
|
|
|
for i = 1:6
|
|
i_fltrd_ldd(:,i) = filtfilt(curr_filt,i_msrd_ldd(:,i));
|
|
end
|
|
|
|
|
|
|
|
|
|
|
|
|
|
return
|
|
% ------------------------------------------------------------------------
|
|
% Construncting regressor and performing least squares
|
|
% ------------------------------------------------------------------------
|
|
|
|
W1 = []; W2 = []; W3 = []; W4 = []; W5 = []; W6 = [];
|
|
I1 = []; I2 = []; I3 = []; I4 = []; I5 = []; I6 = [];
|
|
|
|
n1 = 1;
|
|
n2 = 2;
|
|
n3 = 9;
|
|
n4 = 16;
|
|
n5 = 23;
|
|
n6 = 30;
|
|
xi = 5e-4;
|
|
|
|
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,:)');
|
|
|
|
% As drive gains are unknown we want to build regressor for each joint in
|
|
% order to identify paramters as seen from this joint point of view
|
|
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)];
|
|
|
|
% W1 = vertcat(W1,[Yi(1,n1:4),Yi(1,6:11),Yi(1,13:18),Yi(1,20:end), frctni(1,:)]);
|
|
% W2 = vertcat(W2,[Yi(2,n2:end),frctni(2,:)]);
|
|
% W3 = vertcat(W3,[Yi(3,n3:end),frctni(3,:)]);
|
|
% W4 = vertcat(W4,[Yi(4,n4:end),frctni(4,:)]);
|
|
% W5 = vertcat(W5,[Yi(5,n5:end),frctni(5,:)]);
|
|
% W6 = vertcat(W6,[Yi(6,n6:end),frctni(6,:)]);
|
|
|
|
W2 = vertcat(W2,[Yi_j2, frctni(2,:)]);
|
|
W3 = vertcat(W3,[Yi_j3, frctni(3,:)]);
|
|
W4 = vertcat(W4,[Yi_j4, frctni(4,:)]);
|
|
W5 = vertcat(W5,[Yi_j5, frctni(5,:)]);
|
|
W6 = vertcat(W6,[Yi_j6, frctni(6,:)]);
|
|
|
|
% I1 = vertcat(I1,i_fltrd(i,1));
|
|
I2 = vertcat(I2,i_fltrd(i,2));
|
|
I3 = vertcat(I3,i_fltrd(i,3));
|
|
I4 = vertcat(I4,i_fltrd(i,4));
|
|
I5 = vertcat(I5,i_fltrd(i,5));
|
|
I6 = vertcat(I6,i_fltrd(i,6));
|
|
end
|
|
|
|
% pi1_hat = (W1'*W1)\(W1'*I1);
|
|
pi2_hat = (W2'*W2)\(W2'*I2);
|
|
pi3_hat = (W3'*W3)\(W3'*I3);
|
|
pi4_hat = (W4'*W4)\(W4'*I4);
|
|
pi5_hat = (W5'*W5)\(W5'*I5);
|
|
pi6_hat = (W6'*W6)\(W6'*I6);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|