identification of drive parameters fails even in simluation with some noise. We thoughts that weighted least sqaures can help, but for now it doesn't. It seems that we don't do it right.
This commit is contained in:
Shamil Mamedov
parent
6065d44dbd
commit
8307ef7d99
|
|
@ -1,105 +1,102 @@
|
||||||
% ------------------------------------------------------------------------
|
% ---------------------------------------------------------------------
|
||||||
% Positions
|
% In this script we plot data processing of real data from UR
|
||||||
% ------------------------------------------------------------------------
|
% ---------------------------------------------------------------------
|
||||||
|
trajectory = unloadedTrajectory; % choose trajectory
|
||||||
|
|
||||||
|
%% Positions
|
||||||
|
% Ploting ideal posititions and obtained ones
|
||||||
% %{
|
% %{
|
||||||
figure
|
figure
|
||||||
subplot(2,1,1)
|
subplot(2,1,1)
|
||||||
hold on
|
hold on
|
||||||
for i = 1:3
|
for i = 1:3
|
||||||
plot(t_msrd, q_msrd(:,i))
|
plot(trajectory.t, trajectory.q(:,i))
|
||||||
plot(traj_par.t, q(i,:), '--')
|
|
||||||
end
|
end
|
||||||
xlabel('$t$,\ sec','interpreter','latex')
|
xlabel('$t$,\ sec','interpreter','latex')
|
||||||
legend('$q_1$, rad','$q_2$, rad','$q_3$, rad','interpreter','latex')
|
legend('$q_1$','$q_2$','$q_3$','interpreter','latex')
|
||||||
grid on
|
grid on
|
||||||
subplot(2,1,2)
|
subplot(2,1,2)
|
||||||
hold on
|
hold on
|
||||||
for i = 4:6
|
for i = 4:6
|
||||||
plot(t_msrd, qd_msrd(:,i))
|
plot(trajectory.t, trajectory.q(:,i))
|
||||||
plot(traj_par.t, qd(i,:), '--')
|
|
||||||
end
|
end
|
||||||
xlabel('$t$,\ sec','interpreter','latex')
|
xlabel('$t$,\ sec','interpreter','latex')
|
||||||
legend('$q_4$, rad','$q_5$, rad','$q_6$, rad','interpreter','latex')
|
legend('$q_4$','$q_5$','$q_6$','interpreter','latex')
|
||||||
grid on
|
grid on
|
||||||
%}
|
%}
|
||||||
% ------------------------------------------------------------------------
|
|
||||||
% Velocities
|
%% Velocities
|
||||||
% ------------------------------------------------------------------------
|
% %{
|
||||||
%{
|
|
||||||
figure
|
figure
|
||||||
subplot(2,1,1)
|
subplot(2,1,1)
|
||||||
hold on
|
hold on
|
||||||
for i = 1:3
|
for i = 1:3
|
||||||
plot(t_msrd, qd_msrd(:,i))
|
plot(trajectory.t, trajectory.qd(:,i))
|
||||||
plot(traj_par.t, qd(i,:), '--')
|
plot(trajectory.t, trajectory.qd_fltrd(:,i))
|
||||||
end
|
end
|
||||||
xlabel('$t$,\ sec','interpreter','latex')
|
xlabel('$t$,\ sec','interpreter','latex')
|
||||||
legend('$\dot{q}_1$, rad/s','$\dot{q}_2$, rad/s','$\dot{q}_3$, rad/s',...
|
legend('$\dot{q}_1$','$\dot{q}_2$','$\dot{q}_3$',...
|
||||||
'interpreter','latex')
|
'interpreter','latex')
|
||||||
grid on
|
grid on
|
||||||
subplot(2,1,2)
|
subplot(2,1,2)
|
||||||
hold on
|
hold on
|
||||||
for i = 4:6
|
for i = 4:6
|
||||||
plot(t_msrd, qd_msrd(:,i))
|
plot(trajectory.t, trajectory.qd(:,i))
|
||||||
plot(traj_par.t, qd(i,:), '--')
|
plot(trajectory.t, trajectory.qd_fltrd(:,i))
|
||||||
end
|
end
|
||||||
xlabel('$t$,\ sec','interpreter','latex')
|
xlabel('$t$,\ sec','interpreter','latex')
|
||||||
legend('$\dot{q}_4$, rad/s','$\dot{q}_5$, rad/s','$\dot{q}_6$, rad/s',...
|
legend('$\dot{q}_4$','$\dot{q}_5$','$\dot{q}_6$',...
|
||||||
'interpreter','latex')
|
'interpreter','latex')
|
||||||
grid on
|
grid on
|
||||||
%}
|
%}
|
||||||
|
|
||||||
% ------------------------------------------------------------------------
|
%% Accelerations
|
||||||
% Accelerations
|
% %{
|
||||||
% ------------------------------------------------------------------------
|
|
||||||
%{
|
|
||||||
figure
|
figure
|
||||||
subplot(2,1,1)
|
subplot(2,1,1)
|
||||||
hold on
|
hold on
|
||||||
for i = 1:3
|
for i = 1:3
|
||||||
plot(t_msrd, q2d_est(:,i))
|
plot(trajectory.t, trajectory.q2d_est(:,i))
|
||||||
plot(traj_par.t, q2d(i,:), '--')
|
|
||||||
end
|
end
|
||||||
xlabel('$t$,\ sec','interpreter','latex')
|
xlabel('$t$,\ sec','interpreter','latex')
|
||||||
ylabel('$\ddot{q}$,\ rad','interpreter','latex')
|
ylabel('$\ddot{q}$,\ rad','interpreter','latex')
|
||||||
|
legend('$\ddot{q}_1$','$\ddot{q}_2$','$\ddot{q}_3$',...
|
||||||
|
'interpreter','latex')
|
||||||
ylim([-2.5, 2.5])
|
ylim([-2.5, 2.5])
|
||||||
grid on
|
grid on
|
||||||
subplot(2,1,2)
|
subplot(2,1,2)
|
||||||
hold on
|
hold on
|
||||||
for i = 4:6
|
for i = 4:6
|
||||||
plot(t_msrd, q2d_est(:,i))
|
plot(trajectory.t, trajectory.q2d_est(:,i))
|
||||||
plot(traj_par.t, q2d(i,:), '--')
|
|
||||||
end
|
end
|
||||||
xlabel('$t$,\ sec','interpreter','latex')
|
xlabel('$t$,\ sec','interpreter','latex')
|
||||||
ylabel('$\ddot{q}$,\ rad','interpreter','latex')
|
ylabel('$\ddot{q}$,\ rad','interpreter','latex')
|
||||||
|
legend('$\ddot{q}_4$','$\ddot{q}_5$','$\ddot{q}_6$',...
|
||||||
|
'interpreter','latex')
|
||||||
ylim([-2.5, 2.5])
|
ylim([-2.5, 2.5])
|
||||||
grid on
|
grid on
|
||||||
%}
|
%}
|
||||||
|
|
||||||
% ------------------------------------------------------------------------
|
%% measured currents vs desired currents
|
||||||
% measured torque vs desired torque
|
% %{
|
||||||
% ------------------------------------------------------------------------
|
|
||||||
%{
|
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
figure
|
figure
|
||||||
plot(t_msrd,i_msrd(:,i))
|
plot(trajectory.t,trajectory.i(:,i))
|
||||||
hold on
|
hold on
|
||||||
plot(t_msrd,i_des(:,i))
|
plot(trajectory.t,trajectory.i_des(:,i))
|
||||||
grid on
|
grid on
|
||||||
legend('msrd','dsrd','interpreter','latex')
|
legend('msrd','dsrd','interpreter','latex')
|
||||||
end
|
end
|
||||||
%}
|
%}
|
||||||
|
|
||||||
% ------------------------------------------------------------------------
|
%% currents vs Filteres currents
|
||||||
% currents vs Filteres currents
|
% %{
|
||||||
% ------------------------------------------------------------------------
|
|
||||||
%{
|
|
||||||
figure
|
figure
|
||||||
subplot(2,1,1)
|
subplot(2,1,1)
|
||||||
hold on
|
hold on
|
||||||
for i = 1:3
|
for i = 1:3
|
||||||
plot(t_msrd,i_msrd(:,i))
|
plot(trajectory.t,trajectory.i(:,i))
|
||||||
plot(t_msrd,i_fltrd(:,i))
|
plot(trajectory.t,trajectory.i_fltrd(:,i))
|
||||||
end
|
end
|
||||||
xlabel('$t$,\ sec','interpreter','latex')
|
xlabel('$t$,\ sec','interpreter','latex')
|
||||||
legend('$i_1$, A','$i_2$, A','$i_3$, A','interpreter','latex')
|
legend('$i_1$, A','$i_2$, A','$i_3$, A','interpreter','latex')
|
||||||
|
|
@ -107,14 +104,11 @@ subplot(2,1,1)
|
||||||
subplot(2,1,2)
|
subplot(2,1,2)
|
||||||
hold on
|
hold on
|
||||||
for i = 4:6
|
for i = 4:6
|
||||||
plot(t_msrd,i_msrd(:,i))
|
plot(trajectory.t,trajectory.i(:,i))
|
||||||
plot(t_msrd,i_fltrd(:,i))
|
plot(trajectory.t,trajectory.i_fltrd(:,i))
|
||||||
end
|
end
|
||||||
xlabel('$t$,\ sec','interpreter','latex')
|
xlabel('$t$,\ sec','interpreter','latex')
|
||||||
legend('$i_4$, A','$i_5$, A','$i_6$, A','interpreter','latex')
|
legend('$i_4$, A','$i_5$, A','$i_6$, A','interpreter','latex')
|
||||||
grid on
|
grid on
|
||||||
|
|
||||||
%}
|
%}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -3,40 +3,44 @@ clc; clear all; close all;
|
||||||
% ------------------------------------------------------------------------
|
% ------------------------------------------------------------------------
|
||||||
% Load and calculate desired trajectory
|
% Load and calculate desired trajectory
|
||||||
% ------------------------------------------------------------------------
|
% ------------------------------------------------------------------------
|
||||||
load('opt_sltn_N5T20.mat');
|
load('ptrnSrch_N5T20QR.mat');
|
||||||
[q,qd,q2d] = mixed_traj(traj_par.t,c_pol,a,b,traj_par.wf,traj_par.N);
|
[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
|
% Load the real trajectory of the robot without load
|
||||||
% ------------------------------------------------------------------------
|
% ------------------------------------------------------------------------
|
||||||
traj_data = csvread('ur-19_10_29-20_sec.csv');
|
traj_data = csvread('ur-19_12_23_free.csv');
|
||||||
|
|
||||||
int_idx = 256; % get the point when the trajectory begins
|
int_idx = 1; % get the point when the trajectory begins
|
||||||
fnl_idx = 2158;
|
fnl_idx = 2016;
|
||||||
|
|
||||||
t_msrd = traj_data(int_idx:fnl_idx,1) - traj_data(int_idx,1); %subtract offset
|
unloadedTrajectory = struct; % structure with trajectory data
|
||||||
q_msrd = traj_data(int_idx:fnl_idx,2:7);
|
|
||||||
qd_msrd = traj_data(int_idx:fnl_idx,8:13);
|
unloadedTrajectory.t = traj_data(int_idx:fnl_idx,1) - traj_data(int_idx,1);
|
||||||
i_msrd = traj_data(int_idx:fnl_idx,14:19);
|
unloadedTrajectory.q = traj_data(int_idx:fnl_idx,2:7);
|
||||||
i_des = traj_data(int_idx:fnl_idx,20:25);
|
unloadedTrajectory.qd = traj_data(int_idx:fnl_idx,8:13);
|
||||||
tau_des = traj_data(int_idx:fnl_idx,26:31);
|
unloadedTrajectory.i = traj_data(int_idx:fnl_idx,14:19);
|
||||||
|
unloadedTrajectory.i_des = traj_data(int_idx:fnl_idx,20:25);
|
||||||
|
unloadedTrajectory.tau_des = traj_data(int_idx:fnl_idx,26:31);
|
||||||
|
|
||||||
|
|
||||||
% -----------------------------------------------------------------------
|
% -----------------------------------------------------------------------
|
||||||
% Load the real trajectory of the robot with load
|
% Load the real trajectory of the robot with load
|
||||||
% -----------------------------------------------------------------------
|
% -----------------------------------------------------------------------
|
||||||
traj_ldd = csvread('ur-19_11_05-20_sec_mass_2.csv');
|
traj_ldd = csvread('ur-19_12_23_load.csv');
|
||||||
|
|
||||||
int_idx_ldd = 8;
|
int_idx_ldd = 1;
|
||||||
fnl_idx_ldd = 1877;
|
fnl_idx_ldd = 2040;
|
||||||
|
|
||||||
t_msrd_ldd = traj_ldd(int_idx_ldd:fnl_idx_ldd,1) - traj_ldd(int_idx_ldd,1); %subtract offset
|
loadedTrajectory = struct;
|
||||||
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);
|
loadedTrajectory.t = traj_ldd(int_idx_ldd:fnl_idx_ldd,1) - traj_ldd(int_idx_ldd,1);
|
||||||
i_msrd_ldd = traj_ldd(int_idx_ldd:fnl_idx_ldd,14:19);
|
loadedTrajectory.q = traj_ldd(int_idx_ldd:fnl_idx_ldd,2:7);
|
||||||
i_des_ldd = traj_ldd(int_idx_ldd:fnl_idx_ldd,20:25);
|
loadedTrajectory.qd = traj_ldd(int_idx_ldd:fnl_idx_ldd,8:13);
|
||||||
tau_des_ldd = traj_ldd(int_idx_ldd:fnl_idx_ldd,26:31);
|
loadedTrajectory.i = traj_ldd(int_idx_ldd:fnl_idx_ldd,14:19);
|
||||||
|
loadedTrajectory.i_des = traj_ldd(int_idx_ldd:fnl_idx_ldd,20:25);
|
||||||
|
loadedTrajectory.tau_des = traj_ldd(int_idx_ldd:fnl_idx_ldd,26:31);
|
||||||
|
|
||||||
|
|
||||||
% ------------------------------------------------------------------------
|
% ------------------------------------------------------------------------
|
||||||
|
|
@ -46,14 +50,14 @@ tau_des_ldd = traj_ldd(int_idx_ldd:fnl_idx_ldd,26:31);
|
||||||
vel_filt = designfilt('lowpassiir','FilterOrder',3, ...
|
vel_filt = designfilt('lowpassiir','FilterOrder',3, ...
|
||||||
'HalfPowerFrequency',0.2,'DesignMethod','butter');
|
'HalfPowerFrequency',0.2,'DesignMethod','butter');
|
||||||
|
|
||||||
qd_fltrd = zeros(size(qd_msrd));
|
unloadedTrajectory.qd_fltrd = zeros(size(unloadedTrajectory.qd));
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
qd_fltrd(:,i) = filtfilt(vel_filt,qd_msrd(:,i));
|
unloadedTrajectory.qd_fltrd(:,i) = filtfilt(vel_filt,unloadedTrajectory.qd(:,i));
|
||||||
end
|
end
|
||||||
|
|
||||||
qd_fltrd_ldd = zeros(size(qd_msrd_ldd));
|
loadedTrajectory.qd_fltrd = zeros(size(loadedTrajectory.qd));
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
qd_fltrd_ldd(:,i) = filtfilt(vel_filt,qd_msrd_ldd(:,i));
|
loadedTrajectory.qd_fltrd(:,i) = filtfilt(vel_filt,loadedTrajectory.qd(:,i));
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -61,31 +65,34 @@ end
|
||||||
% Estimating accelerations
|
% Estimating accelerations
|
||||||
% ------------------------------------------------------------------------
|
% ------------------------------------------------------------------------
|
||||||
% Three point central difference
|
% Three point central difference
|
||||||
q2d_est = zeros(size(qd_fltrd));
|
unloadedTrajectory.q2d_est = zeros(size(unloadedTrajectory.qd_fltrd));
|
||||||
for i = 2:length(qd_fltrd)-1
|
for i = 2:length(unloadedTrajectory.qd_fltrd)-1
|
||||||
dlta_qd_fltrd = qd_fltrd(i+1,:) - qd_fltrd(i-1,:);
|
dlta_qd_fltrd = unloadedTrajectory.qd_fltrd(i+1,:) - ...
|
||||||
dlta_t_msrd = t_msrd(i+1) - t_msrd(i-1);
|
unloadedTrajectory.qd_fltrd(i-1,:);
|
||||||
q2d_est(i,:) = dlta_qd_fltrd/dlta_t_msrd;
|
dlta_t_msrd = unloadedTrajectory.t(i+1) - unloadedTrajectory.t(i-1);
|
||||||
|
unloadedTrajectory.q2d_est(i,:) = dlta_qd_fltrd/dlta_t_msrd;
|
||||||
end
|
end
|
||||||
|
|
||||||
q2d_est_ldd = zeros(size(qd_fltrd_ldd));
|
loadedTrajectory.q2d_est = zeros(size(loadedTrajectory.qd_fltrd));
|
||||||
for i = 2:length(qd_fltrd_ldd)-1
|
for i = 2:length(loadedTrajectory.qd_fltrd)-1
|
||||||
dlta_qd_fltrd_ldd = qd_fltrd_ldd(i+1,:) - qd_fltrd_ldd(i-1,:);
|
dlta_qd_fltrd_ldd = loadedTrajectory.qd_fltrd(i+1,:) - ...
|
||||||
dlta_t_msrd_ldd = t_msrd_ldd(i+1) - t_msrd_ldd(i-1);
|
loadedTrajectory.qd_fltrd(i-1,:);
|
||||||
q2d_est_ldd(i,:) = dlta_qd_fltrd_ldd/dlta_t_msrd_ldd;
|
dlta_t_msrd_ldd = loadedTrajectory.t(i+1) - loadedTrajectory.t(i-1);
|
||||||
|
loadedTrajectory.q2d_est(i,:) = dlta_qd_fltrd_ldd/dlta_t_msrd_ldd;
|
||||||
end
|
end
|
||||||
|
|
||||||
% Zeros phase filtering acceleration obtained by finite difference
|
% Zeros phase filtering acceleration obtained by finite difference
|
||||||
accel_filt = designfilt('lowpassiir','FilterOrder',5, ...
|
accel_filt = designfilt('lowpassiir','FilterOrder',5, ...
|
||||||
'HalfPowerFrequency',0.05,'DesignMethod','butter');
|
'HalfPowerFrequency',0.05,'DesignMethod','butter');
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
q2d_est(:,i) = filtfilt(accel_filt,q2d_est(:,i));
|
unloadedTrajectory.q2d_est(:,i) = filtfilt(accel_filt,unloadedTrajectory.q2d_est(:,i));
|
||||||
end
|
end
|
||||||
|
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
q2d_est_ldd(:,i) = filtfilt(accel_filt,q2d_est_ldd(:,i));
|
loadedTrajectory.q2d_est(:,i) = filtfilt(accel_filt,loadedTrajectory.q2d_est(:,i));
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|
||||||
% ------------------------------------------------------------------------
|
% ------------------------------------------------------------------------
|
||||||
% Filtering current
|
% Filtering current
|
||||||
% ------------------------------------------------------------------------
|
% ------------------------------------------------------------------------
|
||||||
|
|
@ -94,90 +101,10 @@ curr_filt = designfilt('lowpassiir','FilterOrder',5, ...
|
||||||
'HalfPowerFrequency',0.1,'DesignMethod','butter');
|
'HalfPowerFrequency',0.1,'DesignMethod','butter');
|
||||||
|
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
i_fltrd(:,i) = filtfilt(curr_filt,i_msrd(:,i));
|
unloadedTrajectory.i_fltrd(:,i) = filtfilt(curr_filt,unloadedTrajectory.i(:,i));
|
||||||
end
|
end
|
||||||
|
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
i_fltrd_ldd(:,i) = filtfilt(curr_filt,i_msrd_ldd(:,i));
|
loadedTrajectory.i_fltrd(:,i) = filtfilt(curr_filt,loadedTrajectory.i(:,i));
|
||||||
end
|
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);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -6,9 +6,13 @@ function Y = regressorWithMotorDynamics(q,qd,q2d)
|
||||||
% parameter is added to existing vector of each link [pi_i I_rflctd_i]
|
% parameter is added to existing vector of each link [pi_i I_rflctd_i]
|
||||||
% so that each link has 11 parameters
|
% so that each link has 11 parameters
|
||||||
% ----------------------------------------------------------------------
|
% ----------------------------------------------------------------------
|
||||||
|
if size(q,1)==6 && size(q,2)==1 && size(qd,1)==6 && size(qd,2)==1 ...
|
||||||
|
&& size(q2d,1)==6 && size(q2d,2)==1
|
||||||
Y_rgd_bdy = full_regressor_UR10E(q,qd,q2d);
|
Y_rgd_bdy = full_regressor_UR10E(q,qd,q2d);
|
||||||
Y_mtrs = diag(q2d);
|
Y_mtrs = diag(q2d);
|
||||||
Y = [Y_rgd_bdy(:,1:10), Y_mtrs(:,1), Y_rgd_bdy(:,11:20), Y_mtrs(:,2),...
|
Y = [Y_rgd_bdy(:,1:10), Y_mtrs(:,1), Y_rgd_bdy(:,11:20), Y_mtrs(:,2),...
|
||||||
Y_rgd_bdy(:,21:30), Y_mtrs(:,3), Y_rgd_bdy(:,31:40), Y_mtrs(:,4),...
|
Y_rgd_bdy(:,21:30), Y_mtrs(:,3), Y_rgd_bdy(:,31:40), Y_mtrs(:,4),...
|
||||||
Y_rgd_bdy(:,41:50), Y_mtrs(:,5), Y_rgd_bdy(:,51:60), Y_mtrs(:,6)];
|
Y_rgd_bdy(:,41:50), Y_mtrs(:,5), Y_rgd_bdy(:,51:60), Y_mtrs(:,6)];
|
||||||
|
else
|
||||||
|
error('Input dimension error!')
|
||||||
end
|
end
|
||||||
|
|
@ -8,51 +8,123 @@ run('data_prcsng.m')
|
||||||
% Generate Regressors based on data
|
% Generate Regressors based on data
|
||||||
% ------------------------------------------------------------------------
|
% ------------------------------------------------------------------------
|
||||||
% Load matrices that map standard set of paratmers to base parameters
|
% Load matrices that map standard set of paratmers to base parameters
|
||||||
load('full2base_mapping.mat');
|
% load('full2base_mapping.mat');
|
||||||
|
load('baseQR.mat'); % load mapping from full parameters to base parameters
|
||||||
|
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
|
||||||
m_load = 1.069;
|
m_load = 1.069;
|
||||||
|
|
||||||
%Constracting regressor matrix
|
%Constracting regressor matrix for unloaded case
|
||||||
Wb_uldd = []; I_uldd = [];
|
Wb_uldd = []; I_uldd = [];
|
||||||
for i = 1:2:length(t_msrd)
|
for i = 1:1:length(unloadedTrajectory.t)
|
||||||
Yb_ulddi = base_regressor_UR10E(q_msrd(i,:)',...
|
Y_ulddi = regressorWithMotorDynamics(unloadedTrajectory.q(i,:)',...
|
||||||
qd_fltrd(i,:)',q2d_est(i,:)');
|
unloadedTrajectory.qd_fltrd(i,:)',...
|
||||||
Yfrctni = ur10_frctn_rgsr(qd_fltrd(i,:)');
|
unloadedTrajectory.q2d_est(i,:)');
|
||||||
Ydrvi = ur10_drv_rgsr(q2d_est(i,:)');
|
|
||||||
Wb_uldd = vertcat(Wb_uldd,[Yb_ulddi, Ydrvi, Yfrctni]);
|
Yfrctni = frictionRegressor(unloadedTrajectory.qd_fltrd(i,:)');
|
||||||
I_uldd = vertcat(I_uldd, diag(i_fltrd(i,:)));
|
Ybi_uldd = [Y_ulddi*E1, Yfrctni];
|
||||||
|
|
||||||
|
Wb_uldd = vertcat(Wb_uldd, Ybi_uldd);
|
||||||
|
I_uldd = vertcat(I_uldd, diag(unloadedTrajectory.i_fltrd(i,:)));
|
||||||
end
|
end
|
||||||
|
|
||||||
|
% Constracting regressor matrix for loaded case
|
||||||
Wb_ldd = []; Wl = []; I_ldd = [];
|
Wb_ldd = []; Wl = []; I_ldd = [];
|
||||||
for i = 1:2:length(t_msrd_ldd)
|
for i = 1:1:length(loadedTrajectory.t)
|
||||||
Yb_lddi = base_regressor_UR10E(q_msrd_ldd(i,:)',...
|
Y_lddi = regressorWithMotorDynamics(loadedTrajectory.q(i,:)',...
|
||||||
qd_fltrd_ldd(i,:)',q2d_est_ldd(i,:)');
|
loadedTrajectory.qd_fltrd(i,:)',...
|
||||||
Yfrctni = ur10_frctn_rgsr(qd_fltrd_ldd(i,:)');
|
loadedTrajectory.q2d_est(i,:)');
|
||||||
Ydrvi = ur10_drv_rgsr(q2d_est_ldd(i,:)');
|
|
||||||
|
|
||||||
Yli = load_regressor_UR10E(q_msrd_ldd(i,:)',...
|
Yfrctni = frictionRegressor(loadedTrajectory.qd_fltrd(i,:)');
|
||||||
qd_fltrd_ldd(i,:)',q2d_est_ldd(i,:)');
|
|
||||||
|
|
||||||
Wb_ldd = vertcat(Wb_ldd,[Yb_lddi, Ydrvi, Yfrctni]);
|
Yli = load_regressor_UR10E(loadedTrajectory.q(i,:)',...
|
||||||
|
loadedTrajectory.qd_fltrd(i,:)',...
|
||||||
|
loadedTrajectory.q2d_est(i,:)');
|
||||||
|
|
||||||
|
Ybi_ldd = [Y_lddi*E1, Yfrctni];
|
||||||
|
|
||||||
|
Wb_ldd = vertcat(Wb_ldd, Ybi_ldd);
|
||||||
Wl = vertcat(Wl,Yli);
|
Wl = vertcat(Wl,Yli);
|
||||||
I_ldd = vertcat(I_ldd, diag(i_fltrd_ldd(i,:)));
|
I_ldd = vertcat(I_ldd, diag(loadedTrajectory.i_fltrd(i,:)));
|
||||||
|
end
|
||||||
|
Wl_uknown = Wl(:,1:9);
|
||||||
|
Wl_known = Wl(:,10); % mass of the load is known
|
||||||
|
|
||||||
|
|
||||||
|
%% Using total least squares
|
||||||
|
Wb_tls = [I_uldd -Wb_uldd zeros(size(I_uldd,1), size(Wl,2));
|
||||||
|
I_ldd -Wb_ldd -Wl_uknown -Wl_known*m_load];
|
||||||
|
|
||||||
|
% SVD decompostion of Wb_tls to solve total least squares
|
||||||
|
[~,~,V] = svd(Wb_tls,'econ');
|
||||||
|
% Scaling of the solution
|
||||||
|
lmda = 1/V(end,end);
|
||||||
|
pi_tls = lmda*V(:,end);
|
||||||
|
% drive gains
|
||||||
|
drvGains = pi_tls(1:6)
|
||||||
|
|
||||||
|
% Finding weighting matrix, joint by joint
|
||||||
|
G = zeros(6);
|
||||||
|
for i = 1:6
|
||||||
|
Wib_tls = Wb_tls(i:6:end,:);
|
||||||
|
[~,Si,Vi] = svd(Wib_tls,'econ');
|
||||||
|
sgmai = Si(end,end)/sqrt((size(Wib_tls,1)-size(Wib_tls,2)));
|
||||||
|
G(i,i) = 1/sgmai^2;
|
||||||
end
|
end
|
||||||
|
|
||||||
% ----------------------------------------------------------------------
|
for i = 1:6:size(Wb_tls,1)
|
||||||
% Set-up SDP optimization procedure
|
Wb_tls(i:i+5,:) = G*Wb_tls(i:i+5,:);
|
||||||
% -----------------------------------------------------------------------
|
end
|
||||||
|
[~,~,V] = svd(Wb_tls,'econ');
|
||||||
|
lmda = 1/V(end,end);
|
||||||
|
pi_tls = lmda*V(:,end);
|
||||||
|
drvGains = pi_tls(1:6)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
%% Identification of parameters including drive gains
|
||||||
|
Wb_ls = [I_uldd -Wb_uldd zeros(size(I_uldd,1), size(Wl_uknown,2));
|
||||||
|
I_ldd -Wb_ldd -Wl_uknown];
|
||||||
|
|
||||||
|
Yb_ts = [zeros(size(I_uldd,1),1); Wl_known*m_load];
|
||||||
|
|
||||||
|
% Compute least squares solution
|
||||||
|
pi_ls = ((Wb_ls'*Wb_ls)\Wb_ls')*Yb_ts;
|
||||||
|
drvGains = pi_ls(1:6)
|
||||||
|
|
||||||
|
|
||||||
|
G = zeros(6);
|
||||||
|
for i = 1:6
|
||||||
|
Wib_ls = Wb_ls(i:6:end,:);
|
||||||
|
Yib_ls = Yb_ts(i:6:end);
|
||||||
|
sgmai_sqrd = norm(Yib_ls - Wib_ls*pi_ls,2)^2/(size(Wib_ls,1)-size(Wib_ls,2));
|
||||||
|
G(i,i) = 1/sgmai_sqrd;
|
||||||
|
end
|
||||||
|
|
||||||
|
for i = 1:6:size(Wb_ls,1)
|
||||||
|
Wb_ls(i:i+5,:) = G*Wb_ls(i:i+5,:);
|
||||||
|
Yb_ts(i:i+5) = G*Yb_ts(i:i+5);
|
||||||
|
end
|
||||||
|
pi_tot = ((Wb_ls'*Wb_ls)\Wb_ls')*Yb_ts;
|
||||||
|
drvGains = pi_tot(1:6)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
%% Set-up SDP optimization procedure
|
||||||
drv_gns = sdpvar(6,1); % variables for base paramters
|
drv_gns = sdpvar(6,1); % variables for base paramters
|
||||||
pi_load_unknw = sdpvar(9,1); % varaibles for unknown load paramters
|
pi_load_unknw = sdpvar(9,1); % varaibles for unknown load paramters
|
||||||
pi_frctn = sdpvar(18,1);
|
pi_frctn = sdpvar(18,1);
|
||||||
pi_rtr = sdpvar(4,1);
|
pi_b = sdpvar(baseQR.numberOfBaseParameters,1); % variables for base paramters
|
||||||
pi_b = sdpvar(36,1); % variables for base paramters
|
pi_d = sdpvar(26,1); % variables for dependent paramters
|
||||||
pi_d = sdpvar(24,1); % variables for dependent paramters
|
|
||||||
|
|
||||||
% Bijective mapping from [pi_b; pi_d] to standard parameters pi
|
% Bijective mapping from [pi_b; pi_d] to standard parameters pi
|
||||||
pii = [Pb' Pd']*[ eye(36) -Kd; zeros(24,36) eye(24) ]*[pi_b; pi_d];
|
pii = baseQR.permutationMatrix*[ eye(baseQR.numberOfBaseParameters), ...
|
||||||
|
-baseQR.beta; ...
|
||||||
|
zeros(26,baseQR.numberOfBaseParameters), ...
|
||||||
|
eye(26) ]*[pi_b; pi_d];
|
||||||
|
|
||||||
% Feasibility contrraints of the link paramteres
|
% Feasibility contrraints of the link paramteres and rotor inertia
|
||||||
cnstr = diag(drv_gns)>0;
|
cnstr = diag(drv_gns)>0;
|
||||||
for i = 1:10:60
|
for i = 1:11:66
|
||||||
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
|
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
|
||||||
pii(i+1), pii(i+3), pii(i+4); ...
|
pii(i+1), pii(i+3), pii(i+4); ...
|
||||||
pii(i+2), pii(i+4), pii(i+5)];
|
pii(i+2), pii(i+4), pii(i+5)];
|
||||||
|
|
@ -60,7 +132,7 @@ for i = 1:10:60
|
||||||
frst_mmnt_i = vec2skewSymMat(pii(i+6:i+8));
|
frst_mmnt_i = vec2skewSymMat(pii(i+6:i+8));
|
||||||
|
|
||||||
Di = [link_inertia_i, frst_mmnt_i'; frst_mmnt_i, pii(i+9)*eye(3)];
|
Di = [link_inertia_i, frst_mmnt_i'; frst_mmnt_i, pii(i+9)*eye(3)];
|
||||||
cnstr = [cnstr, Di>0];
|
cnstr = [cnstr, Di>0, pii(i+10)>0];
|
||||||
end
|
end
|
||||||
|
|
||||||
% Feasibility constraints on the load paramters
|
% Feasibility constraints on the load paramters
|
||||||
|
|
@ -77,23 +149,22 @@ for i = 1:6
|
||||||
cnstr = [cnstr, pi_frctn(3*i-2)>0, pi_frctn(3*i-1)>0];
|
cnstr = [cnstr, pi_frctn(3*i-2)>0, pi_frctn(3*i-1)>0];
|
||||||
end
|
end
|
||||||
|
|
||||||
% Feasibiliy of the rotor inertia
|
|
||||||
cnstr = [cnstr, diag(pi_rtr)>0];
|
|
||||||
|
|
||||||
% Defining pbjective function
|
% Defining pbjective function
|
||||||
t1 = [zeros(size(I_uldd,1),1); -Wl(:,end)*m_load];
|
t1 = [zeros(size(I_uldd,1),1); -Wl(:,end)*m_load];
|
||||||
|
|
||||||
t2 = [-I_uldd, Wb_uldd, zeros(size(Wb_uldd,1), size(Wl,2)-1); ...
|
t2 = [-I_uldd, Wb_uldd, zeros(size(Wb_uldd,1), size(Wl,2)-1); ...
|
||||||
-I_ldd, Wb_ldd, Wl(:,1:9) ];
|
-I_ldd, Wb_ldd, Wl(:,1:9) ];
|
||||||
|
|
||||||
obj = norm(t1 - t2*[drv_gns; pi_b; pi_rtr; pi_frctn; pi_load_unknw]);
|
obj = norm(t1 - t2*[drv_gns; pi_b; pi_frctn; pi_load_unknw]);
|
||||||
|
|
||||||
% Solving sdp problem
|
% Solving sdp problem
|
||||||
sol = optimize(cnstr,obj,sdpsettings('solver','sdpt3'));
|
sol = optimize(cnstr,obj,sdpsettings('solver','sdpt3'));
|
||||||
|
|
||||||
% Getting values of the estimated patamters
|
% Getting values of the estimated patamters
|
||||||
drv_gns = value(drv_gns);
|
drv_gns = value(drv_gns)
|
||||||
|
|
||||||
|
|
||||||
|
return
|
||||||
% -----------------------------------------------------------------------
|
% -----------------------------------------------------------------------
|
||||||
% When drive gains are known we optimize for paramters
|
% When drive gains are known we optimize for paramters
|
||||||
% -----------------------------------------------------------------------
|
% -----------------------------------------------------------------------
|
||||||
|
|
|
||||||
|
|
@ -1,38 +1,224 @@
|
||||||
% clc; clear all; close all;
|
% ---------------------------------------------------------------------
|
||||||
% run('main_ur10.m');
|
% In this script identification of dynamic parameters as well as
|
||||||
run('ur10_base_params_sym.m');
|
% drive gains are performed on ideal trajectory found from
|
||||||
|
% optimization procedure.
|
||||||
|
% ---------------------------------------------------------------------
|
||||||
|
clc; clear all; close all;
|
||||||
|
|
||||||
load('opt_sltn_N5T20.mat'); % optimization trajectory paramters
|
% get robot parameters from urdf
|
||||||
load('pi_base.mat'); % base parameters given by manufacturer URDF
|
run('main_ur.m');
|
||||||
|
|
||||||
|
% add reflected rotor inertia to parameters
|
||||||
|
reflectedRotorInertia = rand([1,6]);
|
||||||
|
ur10.pi = [ur10.pi; reflectedRotorInertia];
|
||||||
|
ur10.pi = reshape(ur10.pi,[size(ur10.pi,1)*size(ur10.pi,2), 1]);
|
||||||
|
|
||||||
|
% get base parameters
|
||||||
|
load('baseQR.mat');
|
||||||
|
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
|
||||||
|
ur10.pi_base = [eye(baseQR.numberOfBaseParameters) baseQR.beta]*...
|
||||||
|
baseQR.permutationMatrix'*ur10.pi;
|
||||||
|
|
||||||
|
% add friction parmeters to the vector of base parameters
|
||||||
|
frictionParameters = rand([18,1]);
|
||||||
|
ur10.pi_base = [ur10.pi_base; frictionParameters];
|
||||||
|
|
||||||
|
% optimized trajectory
|
||||||
|
load('ptrnSrch_N5T20QR.mat');
|
||||||
traj_par.t_smp = 1e-1; % sampling time
|
traj_par.t_smp = 1e-1; % sampling time
|
||||||
traj_par.t = 0:traj_par.t_smp:traj_par.T; % time
|
traj_par.t = 0:traj_par.t_smp:traj_par.T; % time
|
||||||
|
|
||||||
% generilized corrdinates, velocities and acelerations of the trajectory
|
% generilized corrdinates, velocities and acelerations of the trajectory
|
||||||
[q,qd,q2d] = mixed_traj(traj_par.t,c_pol,a,b,traj_par.wf,traj_par.N);
|
[q,qd,q2d] = mixed_traj(traj_par.t,c_pol,a,b,traj_par.wf,traj_par.N);
|
||||||
|
|
||||||
% load paramters
|
% add noise to data
|
||||||
|
noiseVariance = 0.01;
|
||||||
|
q = q + sqrt(noiseVariance)*rand(size(q));
|
||||||
|
qd = qd + sqrt(noiseVariance)*rand(size(qd));
|
||||||
|
q2d = q2d + sqrt(2*noiseVariance)*rand(size(q2d));
|
||||||
|
|
||||||
|
% load and drive paramters
|
||||||
pi_load = [0.2,0.05,0.07,0.15,0.01,0.4, 0.05, 0, 0, 1.069]';
|
pi_load = [0.2,0.05,0.07,0.15,0.01,0.4, 0.05, 0, 0, 1.069]';
|
||||||
K_drv = [11 13 15 10 12 14]';
|
K_drv = [11 13 15 10 12 14]';
|
||||||
m_load = 1.069;
|
m_load = 1.069;
|
||||||
|
|
||||||
% building regressor based on the trajectory
|
|
||||||
W = []; Wl = []; I_uldd = []; I_ldd = [];
|
|
||||||
for i = 1:length(traj_par.t)
|
|
||||||
Yi = base_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i));
|
|
||||||
Yli = load_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i));
|
|
||||||
|
|
||||||
tau_ulddi = Yi*pir_base_vctr;
|
% Constracting regressor matrix for unloaded case
|
||||||
tau_lddi = [Yi Yli]*[pir_base_vctr; pi_load];
|
Wb_uldd = []; I_uldd = [];
|
||||||
|
for i = 1:1:length(traj_par.t)
|
||||||
|
Y_ulddi = regressorWithMotorDynamics(q(:,i),qd(:,i),q2d(:,i));
|
||||||
|
Yfrctni = frictionRegressor(qd(:,i));
|
||||||
|
Ybi_uldd = [Y_ulddi*E1, Yfrctni];
|
||||||
|
taui_uldd = Ybi_uldd*ur10.pi_base;
|
||||||
|
|
||||||
I_uldd = vertcat(I_uldd, diag(diag(K_drv)\tau_ulddi));
|
Wb_uldd = vertcat(Wb_uldd, Ybi_uldd);
|
||||||
I_ldd = vertcat(I_ldd, diag(diag(K_drv)\tau_lddi));
|
I_uldd = vertcat(I_uldd, diag(diag(K_drv)\taui_uldd + ...
|
||||||
|
sqrt(noiseVariance)*rand(6,1)));
|
||||||
W = vertcat(W,Yi);
|
|
||||||
Wl = vertcat(Wl,Yli);
|
|
||||||
end
|
end
|
||||||
|
|
||||||
% getting torques based on the parameters given in URDF
|
% Constracting regressor matrix for loaded case
|
||||||
Tau = W*pir_base_vctr;
|
Wb_ldd = []; Wl = []; I_ldd = [];
|
||||||
|
for i = 1:1:length(traj_par.t)
|
||||||
|
Y_lddi = regressorWithMotorDynamics(q(:,i),qd(:,i),q2d(:,i));
|
||||||
|
Yfrctni = frictionRegressor(qd(:,i));
|
||||||
|
Yli = load_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i));
|
||||||
|
Ybi_ldd = [Y_lddi*E1, Yfrctni];
|
||||||
|
taui_ldd = [Ybi_ldd Yli]*[ur10.pi_base; pi_load];
|
||||||
|
|
||||||
|
Wb_ldd = vertcat(Wb_ldd, Ybi_ldd);
|
||||||
|
Wl = vertcat(Wl, Yli);
|
||||||
|
I_ldd = vertcat(I_ldd, diag(diag(K_drv)\taui_ldd + ...
|
||||||
|
sqrt(0)*rand(6,1)));
|
||||||
|
end
|
||||||
|
Wl_uknown = Wl(:,1:9);
|
||||||
|
Wl_known = Wl(:,10); % mass of the load is known
|
||||||
|
|
||||||
|
|
||||||
|
%% Identification of parameters including drive gains
|
||||||
|
Wb_ls = [I_uldd -Wb_uldd zeros(size(I_uldd,1), size(Wl_uknown,2));
|
||||||
|
I_ldd -Wb_ldd -Wl_uknown];
|
||||||
|
|
||||||
|
Yb_ts = [zeros(size(I_uldd,1),1); Wl_known*m_load];
|
||||||
|
|
||||||
|
% Compute least squares solution
|
||||||
|
pi_ls = ((Wb_ls'*Wb_ls)\Wb_ls')*Yb_ts;
|
||||||
|
drvGains = [pi_ls(1:6) K_drv]
|
||||||
|
|
||||||
|
G = zeros(6);
|
||||||
|
for i = 1:6
|
||||||
|
Wib_ls = Wb_ls(i:6:end,:);
|
||||||
|
Yib_ls = Yb_ts(i:6:end);
|
||||||
|
sgmai_sqrd = norm(Yib_ls - Wib_ls*pi_ls,2)^2/(size(Wib_ls,1)-size(Wib_ls,2));
|
||||||
|
G(i,i) = 1/sqrt(sgmai_sqrd);
|
||||||
|
end
|
||||||
|
|
||||||
|
for i = 1:6:size(Wb_ls,1)
|
||||||
|
Wb_ls(i:i+5,:) = G*Wb_ls(i:i+5,:);
|
||||||
|
Yb_ts(i:i+5) = G*Yb_ts(i:i+5);
|
||||||
|
end
|
||||||
|
pi_tot = ((Wb_ls'*Wb_ls)\Wb_ls')*Yb_ts;
|
||||||
|
drvGains = pi_tot(1:6)
|
||||||
|
|
||||||
|
|
||||||
|
nmbrRwsUnldd = size(Wb_uldd,1);
|
||||||
|
G_uldd = zeros(6);
|
||||||
|
G_ldd = zeros(6);
|
||||||
|
for i = 1:6
|
||||||
|
Wib_uldd = Wb_ls(i:6:nmbrRwsUnldd,:);
|
||||||
|
Yib_uldd = Yb_ts(i:6:nmbrRwsUnldd);
|
||||||
|
sgmai_sqrd = norm(Yib_uldd - Wib_uldd*pi_ls,2)^2/(size(Wib_uldd,1)-rank(Wib_uldd));
|
||||||
|
G_uldd(i,i) = 1/sqrt(sgmai_sqrd);
|
||||||
|
|
||||||
|
Wib_ldd = Wb_ls(nmbrRwsUnldd+i:6:end,:);
|
||||||
|
Yib_ldd = Yb_ts(nmbrRwsUnldd+i:6:end);
|
||||||
|
sgmai_sqrd = norm(Yib_ldd - Wib_ldd*pi_ls,2)^2/(size(Wib_ldd,1)-rank(Wib_ldd));
|
||||||
|
G_ldd(i,i) = 1/sqrt(sgmai_sqrd);
|
||||||
|
end
|
||||||
|
|
||||||
|
|
||||||
|
for i = 1:6:size(Wb_uldd,1)
|
||||||
|
Wb_ls(i:i+5,:) = G_uldd*Wb_ls(i:i+5,:);
|
||||||
|
Yb_ts(i:i+5) = G_uldd*Yb_ts(i:i+5);
|
||||||
|
end
|
||||||
|
|
||||||
|
for i = nmbrRwsUnldd+1:6:nmbrRwsUnldd+size(Wb_ldd,1)
|
||||||
|
Wb_ls(i:i+5,:) = G_ldd*Wb_ls(i:i+5,:);
|
||||||
|
Yb_ts(i:i+5) = G_ldd*Yb_ts(i:i+5);
|
||||||
|
end
|
||||||
|
|
||||||
|
pi_tot = ((Wb_ls'*Wb_ls)\Wb_ls')*Yb_ts;
|
||||||
|
drvGains = pi_tot(1:6)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
return
|
||||||
|
%% Using total least squares
|
||||||
|
Wb_tls = [I_uldd -Wb_uldd zeros(size(I_uldd,1), size(Wl,2));
|
||||||
|
I_ldd -Wb_ldd -Wl_uknown -Wl_known*m_load];
|
||||||
|
|
||||||
|
% SVD decompostion of Wb_tls to solve total least squares
|
||||||
|
[~,~,V] = svd(Wb_tls,'econ');
|
||||||
|
% Scaling of the solution
|
||||||
|
lmda = 1/V(end,end);
|
||||||
|
pi_tls = lmda*V(:,end);
|
||||||
|
% drive gains
|
||||||
|
drvGains = [pi_tls(1:6) K_drv]
|
||||||
|
|
||||||
|
% Finding weighting matrix, joint by joint
|
||||||
|
G = zeros(6);
|
||||||
|
for i = 1:6
|
||||||
|
Wib_tls = Wb_tls(i:6:end,:);
|
||||||
|
[~,Si,Vi] = svd(Wib_tls,'econ');
|
||||||
|
sgmai = Si(end,end)/sqrt((size(Wib_tls,1)-size(Wib_tls,2)));
|
||||||
|
G(i,i) = 1/sgmai^2;
|
||||||
|
end
|
||||||
|
|
||||||
|
for i = 1:6:size(Wb_tls,1)
|
||||||
|
Wb_tls(i:i+5,:) = G*Wb_tls(i:i+5,:);
|
||||||
|
end
|
||||||
|
[~,~,V] = svd(Wb_tls,'econ');
|
||||||
|
lmda = 1/V(end,end);
|
||||||
|
pi_tls = lmda*V(:,end);
|
||||||
|
drvGains = pi_tls(1:6)
|
||||||
|
|
||||||
|
|
||||||
|
%% Set-up SDP optimization procedure
|
||||||
|
drv_gns = sdpvar(6,1); % variables for base paramters
|
||||||
|
pi_load_unknw = sdpvar(9,1); % varaibles for unknown load paramters
|
||||||
|
pi_frctn = sdpvar(18,1);
|
||||||
|
pi_b = sdpvar(baseQR.numberOfBaseParameters,1); % variables for base paramters
|
||||||
|
pi_d = sdpvar(26,1); % variables for dependent paramters
|
||||||
|
|
||||||
|
% Bijective mapping from [pi_b; pi_d] to standard parameters pi
|
||||||
|
pii = baseQR.permutationMatrix*[ eye(baseQR.numberOfBaseParameters), ...
|
||||||
|
-baseQR.beta; ...
|
||||||
|
zeros(26,baseQR.numberOfBaseParameters), ...
|
||||||
|
eye(26) ]*[pi_b; pi_d];
|
||||||
|
|
||||||
|
% Feasibility contrraints of the link paramteres and rotor inertia
|
||||||
|
cnstr = diag(drv_gns)>0;
|
||||||
|
for i = 1:11:66
|
||||||
|
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
|
||||||
|
pii(i+1), pii(i+3), pii(i+4); ...
|
||||||
|
pii(i+2), pii(i+4), pii(i+5)];
|
||||||
|
|
||||||
|
frst_mmnt_i = vec2skewSymMat(pii(i+6:i+8));
|
||||||
|
|
||||||
|
Di = [link_inertia_i, frst_mmnt_i'; frst_mmnt_i, pii(i+9)*eye(3)];
|
||||||
|
cnstr = [cnstr, Di>0, pii(i+10)>0];
|
||||||
|
end
|
||||||
|
|
||||||
|
% Feasibility constraints on the load paramters
|
||||||
|
load_inertia = [pi_load_unknw(1), pi_load_unknw(2), pi_load_unknw(3); ...
|
||||||
|
pi_load_unknw(2), pi_load_unknw(4), pi_load_unknw(5); ...
|
||||||
|
pi_load_unknw(3), pi_load_unknw(5), pi_load_unknw(6)];
|
||||||
|
load_frst_mmnt = vec2skewSymMat(pi_load_unknw(7:9));
|
||||||
|
Dl = [load_inertia, load_frst_mmnt'; load_frst_mmnt, m_load*eye(3)];
|
||||||
|
|
||||||
|
cnstr = [cnstr, Dl>0];
|
||||||
|
|
||||||
|
% Feasibility constraints on the friction prameters
|
||||||
|
for i = 1:6
|
||||||
|
cnstr = [cnstr, pi_frctn(3*i-2)>0, pi_frctn(3*i-1)>0];
|
||||||
|
end
|
||||||
|
|
||||||
|
% Defining pbjective function
|
||||||
|
t1 = [zeros(size(I_uldd,1),1); -Wl(:,end)*m_load];
|
||||||
|
|
||||||
|
t2 = [-I_uldd, Wb_uldd, zeros(size(Wb_uldd,1), size(Wl,2)-1); ...
|
||||||
|
-I_ldd, Wb_ldd, Wl(:,1:9) ];
|
||||||
|
|
||||||
|
obj = norm(t1 - t2*[drv_gns; pi_b; pi_frctn; pi_load_unknw]);
|
||||||
|
|
||||||
|
% Solving sdp problem
|
||||||
|
sol = optimize(cnstr,obj,sdpsettings('solver','sdpt3'));
|
||||||
|
|
||||||
|
% Getting values of the estimated patamters
|
||||||
|
drv_gns = [value(drv_gns) K_drv]
|
||||||
|
|
||||||
|
|
||||||
|
return
|
||||||
|
|
||||||
% ------------------------------------------------------------------------
|
% ------------------------------------------------------------------------
|
||||||
% Using SDP to find load parmeters along with drive gains
|
% Using SDP to find load parmeters along with drive gains
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,464 @@
|
||||||
|
% ---------------------------------------------------------------------
|
||||||
|
% In this script identification of dynamic parameters as well as
|
||||||
|
% drive gains are performed on ideal trajectory found from
|
||||||
|
% optimization procedure.
|
||||||
|
% ---------------------------------------------------------------------
|
||||||
|
clc; clear all; close all;
|
||||||
|
|
||||||
|
% get robot parameters from urdf
|
||||||
|
run('main_ur.m');
|
||||||
|
|
||||||
|
% add reflected rotor inertia to parameters
|
||||||
|
reflectedRotorInertia = rand([1,6]);
|
||||||
|
ur10.pi = [ur10.pi; reflectedRotorInertia];
|
||||||
|
ur10.pi = reshape(ur10.pi,[size(ur10.pi,1)*size(ur10.pi,2), 1]);
|
||||||
|
|
||||||
|
% get base parameters
|
||||||
|
load('baseQR.mat');
|
||||||
|
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
|
||||||
|
ur10.pi_base = [eye(baseQR.numberOfBaseParameters) baseQR.beta]*...
|
||||||
|
baseQR.permutationMatrix'*ur10.pi;
|
||||||
|
|
||||||
|
% add friction parmeters to the vector of base parameters
|
||||||
|
frictionParameters = rand([18,1]);
|
||||||
|
ur10.pi_base = [ur10.pi_base; frictionParameters];
|
||||||
|
|
||||||
|
% optimized trajectory
|
||||||
|
load('ptrnSrch_N5T20QR.mat');
|
||||||
|
traj_par.t_smp = 1e-1; % sampling time
|
||||||
|
traj_par.t = 0:traj_par.t_smp:traj_par.T; % time
|
||||||
|
|
||||||
|
% generilized corrdinates, velocities and acelerations of the trajectory
|
||||||
|
[q,qd,q2d] = mixed_traj(traj_par.t,c_pol,a,b,traj_par.wf,traj_par.N);
|
||||||
|
|
||||||
|
% add noise to data
|
||||||
|
noiseVariance = 0.01;
|
||||||
|
q = q + sqrt(noiseVariance)*rand(size(q));
|
||||||
|
qd = qd + sqrt(noiseVariance)*rand(size(qd));
|
||||||
|
q2d = q2d + sqrt(2*noiseVariance)*rand(size(q2d));
|
||||||
|
|
||||||
|
% load and drive paramters
|
||||||
|
pi_load = [0.2,0.05,0.07,0.15,0.01,0.4, 0.05, 0, 0, 1.069]';
|
||||||
|
K_drv = [11 13 15 10 12 14]';
|
||||||
|
m_load = 1.069;
|
||||||
|
|
||||||
|
|
||||||
|
% Constracting regressor matrix for unloaded case
|
||||||
|
Wb_uldd = []; I_uldd = [];
|
||||||
|
for i = 1:1:length(traj_par.t)
|
||||||
|
Y_ulddi = regressorWithMotorDynamics(q(:,i),qd(:,i),q2d(:,i));
|
||||||
|
Yfrctni = frictionRegressor(qd(:,i));
|
||||||
|
Ybi_uldd = [Y_ulddi*E1, Yfrctni];
|
||||||
|
taui_uldd = Ybi_uldd*ur10.pi_base;
|
||||||
|
|
||||||
|
Wb_uldd = vertcat(Wb_uldd, Ybi_uldd);
|
||||||
|
I_uldd = vertcat(I_uldd, diag(diag(K_drv)\taui_uldd + ...
|
||||||
|
sqrt(noiseVariance)*rand(6,1)));
|
||||||
|
end
|
||||||
|
|
||||||
|
% Constracting regressor matrix for loaded case
|
||||||
|
Wb_ldd = []; Wl = []; I_ldd = [];
|
||||||
|
for i = 1:1:length(traj_par.t)
|
||||||
|
Y_lddi = regressorWithMotorDynamics(q(:,i),qd(:,i),q2d(:,i));
|
||||||
|
Yfrctni = frictionRegressor(qd(:,i));
|
||||||
|
Yli = load_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i));
|
||||||
|
Ybi_ldd = [Y_lddi*E1, Yfrctni];
|
||||||
|
taui_ldd = [Ybi_ldd Yli]*[ur10.pi_base; pi_load];
|
||||||
|
|
||||||
|
Wb_ldd = vertcat(Wb_ldd, Ybi_ldd);
|
||||||
|
Wl = vertcat(Wl, Yli);
|
||||||
|
I_ldd = vertcat(I_ldd, diag(diag(K_drv)\taui_ldd + ...
|
||||||
|
sqrt(0)*rand(6,1)));
|
||||||
|
end
|
||||||
|
Wl_uknown = Wl(:,1:9);
|
||||||
|
Wl_known = Wl(:,10); % mass of the load is known
|
||||||
|
|
||||||
|
|
||||||
|
%% Identification of parameters including drive gains
|
||||||
|
Wb_ls = [I_uldd -Wb_uldd zeros(size(I_uldd,1), size(Wl_uknown,2));
|
||||||
|
I_ldd -Wb_ldd -Wl_uknown];
|
||||||
|
|
||||||
|
Yb_ts = [zeros(size(I_uldd,1),1); Wl_known*m_load];
|
||||||
|
|
||||||
|
% Compute least squares solution
|
||||||
|
pi_ls = ((Wb_ls'*Wb_ls)\Wb_ls')*Yb_ts;
|
||||||
|
drvGains = [pi_ls(1:6) K_drv]
|
||||||
|
|
||||||
|
G = zeros(6);
|
||||||
|
for i = 1:6
|
||||||
|
Wib_ls = Wb_ls(i:6:end,:);
|
||||||
|
Yib_ls = Yb_ts(i:6:end);
|
||||||
|
sgmai_sqrd = norm(Yib_ls - Wib_ls*pi_ls,2)^2/(size(Wib_ls,1)-size(Wib_ls,2));
|
||||||
|
G(i,i) = 1/sgmai_sqrd;
|
||||||
|
end
|
||||||
|
|
||||||
|
for i = 1:6:size(Wb_ls,1)
|
||||||
|
Wb_ls(i:i+5,:) = G*Wb_ls(i:i+5,:);
|
||||||
|
Yb_ts(i:i+5) = G*Yb_ts(i:i+5);
|
||||||
|
end
|
||||||
|
pi_tot = ((Wb_ls'*Wb_ls)\Wb_ls')*Yb_ts;
|
||||||
|
drvGains = pi_tot(1:6)
|
||||||
|
|
||||||
|
|
||||||
|
nmbrRwsUnldd = size(Wb_uldd,1);
|
||||||
|
G_uldd = zeros(6);
|
||||||
|
G_ldd = zeros(6);
|
||||||
|
for i = 1:6
|
||||||
|
Wib_uldd = Wb_ls(i:6:nmbrRwsUnldd,:);
|
||||||
|
Yib_uldd = Yb_ts(i:6:nmbrRwsUnldd);
|
||||||
|
sgmai_sqrd = norm(Yib_uldd - Wib_uldd*pi_ls,2)^2/(size(Wib_uldd,1)-rank(Wib_uldd));
|
||||||
|
G_uldd(i,i) = 1/sgmai_sqrd;
|
||||||
|
|
||||||
|
Wib_ldd = Wb_ls(nmbrRwsUnldd+i:6:end,:);
|
||||||
|
Yib_ldd = Yb_ts(nmbrRwsUnldd+i:6:end);
|
||||||
|
sgmai_sqrd = norm(Yib_ldd - Wib_ldd*pi_ls,2)^2/(size(Wib_ldd,1)-rank(Wib_ldd));
|
||||||
|
G_ldd(i,i) = 1/sgmai_sqrd;
|
||||||
|
end
|
||||||
|
|
||||||
|
|
||||||
|
for i = 1:6:size(Wb_uldd,1)
|
||||||
|
Wb_ls(i:i+5,:) = G_uldd*Wb_ls(i:i+5,:);
|
||||||
|
Yb_ts(i:i+5) = G_uldd*Yb_ts(i:i+5);
|
||||||
|
|
||||||
|
Wb_ls(size(Wb_uldd,1)+i:i+5,:) = G_ldd*Wb_ls(size(Wb_uldd,1)+i:i+5,:);
|
||||||
|
Yb_ts(size(Wb_uldd,1)+i:i+5) = G_ldd*Yb_ts(size(Wb_uldd,1)+i:i+5);
|
||||||
|
end
|
||||||
|
pi_tot = ((Wb_ls'*Wb_ls)\Wb_ls')*Yb_ts;
|
||||||
|
drvGains = pi_tot(1:6)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
return
|
||||||
|
%% Using total least squares
|
||||||
|
Wb_tls = [I_uldd -Wb_uldd zeros(size(I_uldd,1), size(Wl,2));
|
||||||
|
I_ldd -Wb_ldd -Wl_uknown -Wl_known*m_load];
|
||||||
|
|
||||||
|
% SVD decompostion of Wb_tls to solve total least squares
|
||||||
|
[~,~,V] = svd(Wb_tls,'econ');
|
||||||
|
% Scaling of the solution
|
||||||
|
lmda = 1/V(end,end);
|
||||||
|
pi_tls = lmda*V(:,end);
|
||||||
|
% drive gains
|
||||||
|
drvGains = [pi_tls(1:6) K_drv]
|
||||||
|
|
||||||
|
% Finding weighting matrix, joint by joint
|
||||||
|
G = zeros(6);
|
||||||
|
for i = 1:6
|
||||||
|
Wib_tls = Wb_tls(i:6:end,:);
|
||||||
|
[~,Si,Vi] = svd(Wib_tls,'econ');
|
||||||
|
sgmai = Si(end,end)/sqrt((size(Wib_tls,1)-size(Wib_tls,2)));
|
||||||
|
G(i,i) = 1/sgmai^2;
|
||||||
|
end
|
||||||
|
|
||||||
|
for i = 1:6:size(Wb_tls,1)
|
||||||
|
Wb_tls(i:i+5,:) = G*Wb_tls(i:i+5,:);
|
||||||
|
end
|
||||||
|
[~,~,V] = svd(Wb_tls,'econ');
|
||||||
|
lmda = 1/V(end,end);
|
||||||
|
pi_tls = lmda*V(:,end);
|
||||||
|
drvGains = pi_tls(1:6)
|
||||||
|
|
||||||
|
|
||||||
|
%% Set-up SDP optimization procedure
|
||||||
|
drv_gns = sdpvar(6,1); % variables for base paramters
|
||||||
|
pi_load_unknw = sdpvar(9,1); % varaibles for unknown load paramters
|
||||||
|
pi_frctn = sdpvar(18,1);
|
||||||
|
pi_b = sdpvar(baseQR.numberOfBaseParameters,1); % variables for base paramters
|
||||||
|
pi_d = sdpvar(26,1); % variables for dependent paramters
|
||||||
|
|
||||||
|
% Bijective mapping from [pi_b; pi_d] to standard parameters pi
|
||||||
|
pii = baseQR.permutationMatrix*[ eye(baseQR.numberOfBaseParameters), ...
|
||||||
|
-baseQR.beta; ...
|
||||||
|
zeros(26,baseQR.numberOfBaseParameters), ...
|
||||||
|
eye(26) ]*[pi_b; pi_d];
|
||||||
|
|
||||||
|
% Feasibility contrraints of the link paramteres and rotor inertia
|
||||||
|
cnstr = diag(drv_gns)>0;
|
||||||
|
for i = 1:11:66
|
||||||
|
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
|
||||||
|
pii(i+1), pii(i+3), pii(i+4); ...
|
||||||
|
pii(i+2), pii(i+4), pii(i+5)];
|
||||||
|
|
||||||
|
frst_mmnt_i = vec2skewSymMat(pii(i+6:i+8));
|
||||||
|
|
||||||
|
Di = [link_inertia_i, frst_mmnt_i'; frst_mmnt_i, pii(i+9)*eye(3)];
|
||||||
|
cnstr = [cnstr, Di>0, pii(i+10)>0];
|
||||||
|
end
|
||||||
|
|
||||||
|
% Feasibility constraints on the load paramters
|
||||||
|
load_inertia = [pi_load_unknw(1), pi_load_unknw(2), pi_load_unknw(3); ...
|
||||||
|
pi_load_unknw(2), pi_load_unknw(4), pi_load_unknw(5); ...
|
||||||
|
pi_load_unknw(3), pi_load_unknw(5), pi_load_unknw(6)];
|
||||||
|
load_frst_mmnt = vec2skewSymMat(pi_load_unknw(7:9));
|
||||||
|
Dl = [load_inertia, load_frst_mmnt'; load_frst_mmnt, m_load*eye(3)];
|
||||||
|
|
||||||
|
cnstr = [cnstr, Dl>0];
|
||||||
|
|
||||||
|
% Feasibility constraints on the friction prameters
|
||||||
|
for i = 1:6
|
||||||
|
cnstr = [cnstr, pi_frctn(3*i-2)>0, pi_frctn(3*i-1)>0];
|
||||||
|
end
|
||||||
|
|
||||||
|
% Defining pbjective function
|
||||||
|
t1 = [zeros(size(I_uldd,1),1); -Wl(:,end)*m_load];
|
||||||
|
|
||||||
|
t2 = [-I_uldd, Wb_uldd, zeros(size(Wb_uldd,1), size(Wl,2)-1); ...
|
||||||
|
-I_ldd, Wb_ldd, Wl(:,1:9) ];
|
||||||
|
|
||||||
|
obj = norm(t1 - t2*[drv_gns; pi_b; pi_frctn; pi_load_unknw]);
|
||||||
|
|
||||||
|
% Solving sdp problem
|
||||||
|
sol = optimize(cnstr,obj,sdpsettings('solver','sdpt3'));
|
||||||
|
|
||||||
|
% Getting values of the estimated patamters
|
||||||
|
drv_gns = [value(drv_gns) K_drv]
|
||||||
|
|
||||||
|
|
||||||
|
return
|
||||||
|
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
% Using SDP to find load parmeters along with drive gains
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
%{
|
||||||
|
drv_gns = sdpvar(6,1); % variables for base paramters
|
||||||
|
pi_load_unknw = sdpvar(9,1); % varaibles for unknown load paramters
|
||||||
|
% u = sdpvar(1,1); % variable for cost
|
||||||
|
%
|
||||||
|
% % Writing |Tau - W*pi_b| using Schur compliment
|
||||||
|
% U1 = [u, (-Wl(:,end)*m_load - [-(I_ldd-I_uldd) Wl(:,1:9)]*[drv_gns;pi_load_unknw])'; ...
|
||||||
|
% -Wl(:,end)*m_load - [-(I_ldd-I_uldd) Wl(:,1:9)]*[drv_gns;pi_load_unknw], sparse(eye(size(Wl,1)))];
|
||||||
|
|
||||||
|
% Feasibility contrraints
|
||||||
|
% U2 = diag(drv_gns);
|
||||||
|
load_inertia = [pi_load_unknw(1), pi_load_unknw(2), pi_load_unknw(3); ...
|
||||||
|
pi_load_unknw(2), pi_load_unknw(4), pi_load_unknw(5); ...
|
||||||
|
pi_load_unknw(3), pi_load_unknw(5), pi_load_unknw(6)];
|
||||||
|
|
||||||
|
load_frst_mmnt = vec2skewSymMat(pi_load_unknw(7:9));
|
||||||
|
|
||||||
|
D = [load_inertia, load_frst_mmnt'; load_frst_mmnt, m_load*eye(3)];
|
||||||
|
% U2 = blkdiag(U2,D);
|
||||||
|
% Overall constraints
|
||||||
|
% cnstr = blkdiag(U1,U2)>0;
|
||||||
|
cnstr = [drv_gns>0, D>0];
|
||||||
|
|
||||||
|
% Objective function
|
||||||
|
obj = norm(-Wl(:,end)*m_load - [-(I_ldd-I_uldd) Wl(:,1:9)]*[drv_gns;pi_load_unknw]);
|
||||||
|
|
||||||
|
% Solving sdp problem
|
||||||
|
sol = optimize(cnstr,obj,sdpsettings('solver','sdpt3'));
|
||||||
|
|
||||||
|
%}
|
||||||
|
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
% Computing drive gains using second method
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
drv_gns = sdpvar(6,1); % variables for base paramters
|
||||||
|
pi_load_unknw = sdpvar(9,1); % varaibles for unknown load paramters
|
||||||
|
pi_b = sdpvar(36,1); % variables for base paramters
|
||||||
|
pi_d = sdpvar(24,1); % variables for dependent paramters
|
||||||
|
|
||||||
|
% Bijective mapping from [pi_b; pi_d] to standard parameters pi
|
||||||
|
pii = [Pb' Pd']*[ eye(36) -double(Kd); zeros(24,36) eye(24)]*[pi_b; pi_d];
|
||||||
|
|
||||||
|
% Feasibility contrraints
|
||||||
|
cnstr = drv_gns>0;
|
||||||
|
for i = 1:10:60
|
||||||
|
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
|
||||||
|
pii(i+1), pii(i+3), pii(i+4); ...
|
||||||
|
pii(i+2), pii(i+4), pii(i+5)];
|
||||||
|
|
||||||
|
frst_mmnt_i = vec2skewSymMat(pii(i+6:i+8));
|
||||||
|
|
||||||
|
Di = [link_inertia_i, frst_mmnt_i'; frst_mmnt_i, pii(i+9)*eye(3)];
|
||||||
|
cnstr = [cnstr, Di>0];
|
||||||
|
end
|
||||||
|
|
||||||
|
load_inertia = [pi_load_unknw(1), pi_load_unknw(2), pi_load_unknw(3); ...
|
||||||
|
pi_load_unknw(2), pi_load_unknw(4), pi_load_unknw(5); ...
|
||||||
|
pi_load_unknw(3), pi_load_unknw(5), pi_load_unknw(6)];
|
||||||
|
|
||||||
|
load_frst_mmnt = vec2skewSymMat(pi_load_unknw(7:9));
|
||||||
|
|
||||||
|
Dl = [load_inertia, load_frst_mmnt'; load_frst_mmnt, m_load*eye(3)];
|
||||||
|
cnstr = [cnstr, Dl>0];
|
||||||
|
|
||||||
|
t1 = [zeros(size(I_uldd,1),1); -Wl(:,end)*m_load];
|
||||||
|
t2 = [-I_uldd, W, zeros(size(W,1), size(Wl,2)-1); ...
|
||||||
|
-I_ldd, W, Wl(:,1:9) ];
|
||||||
|
obj = norm(t1 - t2*[drv_gns;pi_b;pi_load_unknw]);
|
||||||
|
|
||||||
|
% Solving sdp problem
|
||||||
|
sol = optimize(cnstr,obj,sdpsettings('solver','sdpt3'));
|
||||||
|
return
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
% Least square
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
pir_hat = (W'*W)\(W'*Tau);
|
||||||
|
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
% Ridge regression
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
% pi_hat = mldivide(W'*W + 0.001*eye(size(W'*W)),W'*tau);
|
||||||
|
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
% Using SDP to find inertial parameters
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
pi_b = sdpvar(36,1); % variables for base paramters
|
||||||
|
pi_d = sdpvar(24,1); % variables for dependent paramters
|
||||||
|
u = sdpvar(1,1); % variable for cost
|
||||||
|
|
||||||
|
% Writing |Tau - W*pi_b| using Schur compliment
|
||||||
|
U1 = [u, (Tau - W*pi_b)'; Tau - W*pi_b, eye(size(W,1))];
|
||||||
|
|
||||||
|
% Bijective mapping from [pi_b; pi_d] to standard parameters pi
|
||||||
|
pii = [Pb' Pd']*[ eye(36) -double(Kd); zeros(24,36) eye(24)]*[pi_b; pi_d];
|
||||||
|
|
||||||
|
% Feasibility contrraints
|
||||||
|
U2 = [];
|
||||||
|
for i = 1:10:60
|
||||||
|
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
|
||||||
|
pii(i+1), pii(i+3), pii(i+4); ...
|
||||||
|
pii(i+2), pii(i+4), pii(i+5)];
|
||||||
|
|
||||||
|
frst_mmnt_i = vec2skewSymMat(pii(i+6:i+8));
|
||||||
|
|
||||||
|
Di = [link_inertia_i, frst_mmnt_i'; frst_mmnt_i, pii(i+9)*eye(3)];
|
||||||
|
U2 = blkdiag(U2,Di);
|
||||||
|
end
|
||||||
|
% Overall constraints
|
||||||
|
cnstr = blkdiag(U1,U2)>0;
|
||||||
|
|
||||||
|
% Objective function
|
||||||
|
obj = u;
|
||||||
|
|
||||||
|
% Solving sdp problem
|
||||||
|
sol = optimize(cnstr,obj,sdpsettings('solver','sdpt3'));
|
||||||
|
|
||||||
|
% Getting numerical values of optimization paramters
|
||||||
|
base_prms = value(pi_b);
|
||||||
|
dpndn_prms = value(pi_d);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
return
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
% DREM
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
alpha = [1, 1.3, 1.6, 1.3, 1.15, 1.5];
|
||||||
|
beta = [1, 1.25, 1.5, 1.75, 2, 2.25];
|
||||||
|
dly = [3, 6, 9, 12, 15];
|
||||||
|
|
||||||
|
% tau(:,1) = base_regressor(q(:,1),qd(:,1),q2d(:,1))*pir_base_vctr;
|
||||||
|
% tau_f1(:,1) = tau(:,1); tau_f2(:,1) = tau(:,1); tau_f3(:,1) = tau(:,1);
|
||||||
|
% tau_f4(:,1) = tau(:,1); tau_f5(:,1) = tau(:,1); tau_f6(:,1) = tau(:,1);
|
||||||
|
%
|
||||||
|
% for i = 2:length(traj_par.t)
|
||||||
|
% tau(:,i) = base_regressor(q(:,i),qd(:,i),q2d(:,i))*pir_base_vctr;
|
||||||
|
% for j = 1:6
|
||||||
|
% tau_f1(j,i) = low_pass_filter(tau(j,i),tau_f1(j,i-1),alpha(1),beta(1),dt);
|
||||||
|
% tau_f2(j,i) = low_pass_filter(tau(j,i),tau_f2(j,i-1),alpha(2),beta(2),dt);
|
||||||
|
% tau_f3(j,i) = low_pass_filter(tau(j,i),tau_f3(j,i-1),alpha(3),beta(3),dt);
|
||||||
|
% tau_f4(j,i) = low_pass_filter(tau(j,i),tau_f4(j,i-1),alpha(4),beta(4),dt);
|
||||||
|
% tau_f5(j,i) = low_pass_filter(tau(j,i),tau_f5(j,i-1),alpha(5),beta(5),dt);
|
||||||
|
% end
|
||||||
|
% end
|
||||||
|
% tau_e1 = vertcat(tau,tau_f1,tau_f2,tau_f3,tau_f4,tau_f5);
|
||||||
|
|
||||||
|
for i = 1:length(traj_par.t)
|
||||||
|
tau(:,i) = base_regressor(q(:,i),qd(:,i),q2d(:,i))*pir_base_vctr;
|
||||||
|
if i>dly(1); tau_f1(:,i) = tau(:,i-dly(1)); else; tau_f1(:,i) = zeros(6,1); end
|
||||||
|
if i>dly(2); tau_f2(:,i) = tau(:,i-dly(2)); else; tau_f2(:,i) = zeros(6,1); end
|
||||||
|
if i>dly(3); tau_f3(:,i) = tau(:,i-dly(3)); else; tau_f3(:,i) = zeros(6,1); end
|
||||||
|
if i>dly(4); tau_f4(:,i) = tau(:,i-dly(4)); else; tau_f4(:,i) = zeros(6,1); end
|
||||||
|
if i>dly(5); tau_f5(:,i) = tau(:,i-dly(5)); else; tau_f5(:,i) = zeros(6,1); end
|
||||||
|
end
|
||||||
|
tau_e2 = vertcat(tau,tau_f1,tau_f2,tau_f3,tau_f4,tau_f5);
|
||||||
|
|
||||||
|
%{
|
||||||
|
jnt = 4;
|
||||||
|
figure
|
||||||
|
plot(traj_par.t,tau(jnt,:))
|
||||||
|
hold on
|
||||||
|
plot(traj_par.t,tau_f1(jnt,:))
|
||||||
|
plot(traj_par.t,tau_f2(jnt,:))
|
||||||
|
plot(traj_par.t,tau_f3(jnt,:))
|
||||||
|
plot(traj_par.t,tau_f4(jnt,:))
|
||||||
|
plot(traj_par.t,tau_f5(jnt,:))
|
||||||
|
%}
|
||||||
|
|
||||||
|
% Y(:,:,1) = base_regressor(q(:,1),qd(:,1),q2d(:,1));
|
||||||
|
% Y_f1(:,:,1) = Y(:,:,1); Y_f2(:,:,1) = Y(:,:,1); Y_f3(:,:,1) = Y(:,:,1);
|
||||||
|
% Y_f4(:,:,1) = Y(:,:,1); Y_f5(:,:,1) = Y(:,:,1); Y_f6(:,:,1) = Y(:,:,1);
|
||||||
|
% for i = 2:length(traj_par.t)
|
||||||
|
% Y(:,:,i) = base_regressor(q(:,i),qd(:,i),q2d(:,i));
|
||||||
|
% for j = 1:6
|
||||||
|
% for k = 1:36
|
||||||
|
% Y_f1(j,k,i) = low_pass_filter(Y(j,k,i),Y_f1(j,k,i-1),alpha(1),beta(1),dt);
|
||||||
|
% Y_f2(j,k,i) = low_pass_filter(Y(j,k,i),Y_f2(j,k,i-1),alpha(2),beta(2),dt);
|
||||||
|
% Y_f3(j,k,i) = low_pass_filter(Y(j,k,i),Y_f3(j,k,i-1),alpha(3),beta(3),dt);
|
||||||
|
% Y_f4(j,k,i) = low_pass_filter(Y(j,k,i),Y_f4(j,k,i-1),alpha(4),beta(4),dt);
|
||||||
|
% Y_f5(j,k,i) = low_pass_filter(Y(j,k,i),Y_f5(j,k,i-1),alpha(5),beta(5),dt);
|
||||||
|
% end
|
||||||
|
% end
|
||||||
|
%
|
||||||
|
% end
|
||||||
|
% Y_e1 = vertcat(Y, Y_f1, Y_f2, Y_f3, Y_f4, Y_f5);
|
||||||
|
|
||||||
|
for i = 1:length(traj_par.t)
|
||||||
|
Y(:,:,i) = base_regressor(q(:,i),qd(:,i),q2d(:,i));
|
||||||
|
if i>dly(1); Y_f1(:,:,i) = Y(:,:,i-dly(1)); else; Y_f1(:,:,i) = zeros(6,36); end
|
||||||
|
if i>dly(2); Y_f2(:,:,i) = Y(:,:,i-dly(2)); else; Y_f2(:,:,i) = zeros(6,36); end
|
||||||
|
if i>dly(3); Y_f3(:,:,i) = Y(:,:,i-dly(3)); else; Y_f3(:,:,i) = zeros(6,36); end
|
||||||
|
if i>dly(4); Y_f4(:,:,i) = Y(:,:,i-dly(4)); else; Y_f4(:,:,i) = zeros(6,36); end
|
||||||
|
if i>dly(5); Y_f5(:,:,i) = Y(:,:,i-dly(5)); else; Y_f5(:,:,i) = zeros(6,36); end
|
||||||
|
end
|
||||||
|
Y_e2 = vertcat(Y, Y_f1, Y_f2, Y_f3, Y_f4, Y_f5);
|
||||||
|
|
||||||
|
%{
|
||||||
|
row_Y = 1;
|
||||||
|
column_Y = 8;
|
||||||
|
figure
|
||||||
|
plot(traj_par.t, reshape(Y(row_Y,column_Y,:),[size(Y,3),1]))
|
||||||
|
hold on
|
||||||
|
plot(traj_par.t, reshape(Y_f1(row_Y,column_Y,:),[size(Y_f1,3),1]))
|
||||||
|
plot(traj_par.t, reshape(Y_f2(row_Y,column_Y,:),[size(Y_f2,3),1]))
|
||||||
|
plot(traj_par.t, reshape(Y_f3(row_Y,column_Y,:),[size(Y_f3,3),1]))
|
||||||
|
plot(traj_par.t, reshape(Y_f4(row_Y,column_Y,:),[size(Y_f4,3),1]))
|
||||||
|
plot(traj_par.t, reshape(Y_f5(row_Y,column_Y,:),[size(Y_f5,3),1]))
|
||||||
|
%}
|
||||||
|
|
||||||
|
gma1 = 1e-2;
|
||||||
|
pi_drem(:,1) = zeros(36,1);
|
||||||
|
for i = 2:length(traj_par.t)
|
||||||
|
Yei = Y_e2(:,:,i);
|
||||||
|
tauei = tau_e2(:,i);
|
||||||
|
|
||||||
|
adjYei = adjoint(Yei);
|
||||||
|
detYei = det(Yei);
|
||||||
|
YYi = adjYei*tauei;
|
||||||
|
erri = YYi - detYei*pi_drem(:,i-1);
|
||||||
|
gradi = detYei/(gma1 + detYei^2)*erri;
|
||||||
|
pi_drem(:,i) = pi_drem(:,i-1) + gma1*gradi;
|
||||||
|
end
|
||||||
|
|
||||||
|
figure
|
||||||
|
hold on
|
||||||
|
for i = 1:5
|
||||||
|
plot(traj_par.t,pi_drem(i,1:end))
|
||||||
|
end
|
||||||
|
|
||||||
|
return
|
||||||
|
|
||||||
|
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
% Low pass filter for DREM
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
function y_cur = low_pass_filter(u_cur,y_prev,alpha,beta,T)
|
||||||
|
t1 = 1+beta*T;
|
||||||
|
t2 = alpha*T;
|
||||||
|
y_cur = 1/t1*y_prev + t2/t1*u_cur;
|
||||||
|
end
|
||||||
|
|
@ -37,8 +37,8 @@ traj_par.t = 0:traj_par.t_smp:traj_par.T; % time
|
||||||
traj_par.N = 5; % number of harmonics
|
traj_par.N = 5; % number of harmonics
|
||||||
traj_par.q0 = deg2rad([0 -90 0 -90 0 0 ]');
|
traj_par.q0 = deg2rad([0 -90 0 -90 0 0 ]');
|
||||||
% Use different limit for positions for safety
|
% Use different limit for positions for safety
|
||||||
traj_par.q_min = deg2rad([-180 -180 -90 -180 -90 -90]');
|
traj_par.q_min = -deg2rad([180 180 160 180 90 90]');
|
||||||
traj_par.q_max = deg2rad([180 0 90 0 90 90]');
|
traj_par.q_max = deg2rad([180 0 160 0 90 90]');
|
||||||
traj_par.qd_max = qd_max;
|
traj_par.qd_max = qd_max;
|
||||||
traj_par.q2d_max = 2*ones(6,1);
|
traj_par.q2d_max = 2*ones(6,1);
|
||||||
|
|
||||||
|
|
@ -124,7 +124,7 @@ subplot(3,1,3)
|
||||||
pathToFolder = 'trajectory_optmzn/';
|
pathToFolder = 'trajectory_optmzn/';
|
||||||
t1 = strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T));
|
t1 = strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T));
|
||||||
if strcmp(optmznAlgorithm, 'patternsearch')
|
if strcmp(optmznAlgorithm, 'patternsearch')
|
||||||
filename = strcat(pathToFolder,'ptrnSrch_',t1,'QR.mat');
|
filename = strcat(pathToFolder,'ptrnSrch_',t1,'QR2.mat');
|
||||||
elseif strcmp(optmznAlgorithm, 'ga')
|
elseif strcmp(optmznAlgorithm, 'ga')
|
||||||
filename = strcat(pathToFolder,'ga_',t1,'.mat');
|
filename = strcat(pathToFolder,'ga_',t1,'.mat');
|
||||||
elseif strcmp(optmznAlgorithm, 'fmincon')
|
elseif strcmp(optmznAlgorithm, 'fmincon')
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue