forked from shamilmamedov/dynamic_calibration
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathestimate_drive_gains.m
174 lines (147 loc) · 7.74 KB
/
estimate_drive_gains.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
function drvGains = estimate_drive_gains(baseQR, method)
% ------------------------------------------------------------------------
% The function estimates drive gains for the UR10E robot.
% To do that several methods were used: total least squares approach;
% ordinary least squares approach; and ordinary least squares with physical
% feasibility constraints that is solved using semidefinite programming
%
% Note that trajectories are hardcoded !!!!
%
% Inputs:
% baseQR - QR decomposition of the observation matrix used for
% calculatung base regressor
% method - method for finding drive gains, can be TLS, OLS, PC-OLS
%
% Outputs:
% drvGains - estimated drive gains
% -----------------------------------------------------------------------
m_load = 2.805;
path_to_unloaded_traj = 'ur-20_02_19_14harm50sec.csv';
path_to_loaded_traj = 'ur-20_02_19_14harm50secLoad.csv';
% ------------------------------------------------------------------------
% Load raw data and procces it (filter and estimate accelerations).
% Several trajectories were recorded for unloaded and loaded cases.
% Different combination of trajectories provide slighlty different results.
% Nonetheless, during validation they provide the more or less the same
% result. So, any unloaded trajectory from the given list can be chosen
% ------------------------------------------------------------------------
unloadedTrajectory = parseURData(path_to_unloaded_traj, 195, 4966);
unloadedTrajectory = filterData(unloadedTrajectory);
loadedTrajectory = parseURData(path_to_loaded_traj, 308, 5071);
loadedTrajectory = filterData(loadedTrajectory);
% ------------------------------------------------------------------------
% Generate Regressors based on data.
% Here we generate base regressor, that is obtained form the standard
% regressor by multiplying it by the mapping from full standard paramters
% to base parametrs using numerical approach based on QR decomposition
% ------------------------------------------------------------------------
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
% Constracting regressor matrix for unloaded case
Wb_uldd = []; I_uldd = [];
for i = 1:1:length(unloadedTrajectory.t)
Y_ulddi = regressorWithMotorDynamics(unloadedTrajectory.q(i,:)',...
unloadedTrajectory.qd_fltrd(i,:)',...
unloadedTrajectory.q2d_est(i,:)');
Yfrctni = frictionRegressor(unloadedTrajectory.qd_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
% Constracting regressor matrix for loaded case
Wb_ldd = []; Wl = []; I_ldd = [];
for i = 1:1:length(loadedTrajectory.t)
Y_lddi = regressorWithMotorDynamics(loadedTrajectory.q(i,:)',...
loadedTrajectory.qd_fltrd(i,:)',...
loadedTrajectory.q2d_est(i,:)');
Yfrctni = frictionRegressor(loadedTrajectory.qd_fltrd(i,:)');
Ybi_ldd = [Y_lddi*E1, Yfrctni];
Yli = load_regressor_UR10E(loadedTrajectory.q(i,:)',...
loadedTrajectory.qd_fltrd(i,:)',...
loadedTrajectory.q2d_est(i,:)');
Wb_ldd = vertcat(Wb_ldd, Ybi_ldd);
Wl = vertcat(Wl,Yli);
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
% Estimate drive gains using a specified method
if strcmp(method, 'TLS')
% -------------------------------------------------------------------
% Using total least squares
% Note: TLS provides rather bad results. Even normilizing by mass
% and using weighting does not help to improve results
% -------------------------------------------------------------------
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);
elseif strcmp(method, 'OLS')
% --------------------------------------------------------------------
% Estimation of parameters including drive gains using OLS
% Although according to Handbook of robotics and papers of Gautier
% OLS has correlated noise for correct drive gain estimation
% it provides good results. Weighted least square does not improve the
% result.
% --------------------------------------------------------------------
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);
elseif strcmp(method, 'PC-OLS')
% ----------------------------------------------------------------------
% Estimate parameters using OLS with physical feasibility constraints
% Set-up SDP optimization procedure
% Provides more or less the same result as OLS but with physical
% consistencty. The drive gains estimated using this appraoch is
% further used for indetification of inertial parameters
% ----------------------------------------------------------------------
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 = [drv_gns(1)>10]; % strong constraint on minimum value of K1
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 objective 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
drvGains = value(drv_gns);
else
error("Chosen method for drive gain estimation does not exist");
end