90 lines
2.8 KiB
Matlab
90 lines
2.8 KiB
Matlab
% run data processing script
|
|
run('pndbt_data_processing.m')
|
|
|
|
% load mapping from standard parameters to base parameters
|
|
load('plnrBaseQR.mat')
|
|
|
|
% compose observation matrix and torque vector
|
|
noObservations = length(pendubot.time);
|
|
W = []; Tau = [];
|
|
for i = 1:1:noObservations
|
|
qi = [pendubot.shldr_position(i), pendubot.elbw_position(i)]';
|
|
qdi = [pendubot.shldr_velocity_filtered(i), pendubot.elbw_velocity_filtered(i)]';
|
|
q2di = [pendubot.shldr_acceleration_filtered(i), pendubot.elbow_acceleration_filtered(i)]';
|
|
|
|
if plnrBaseQR.motorDynamicsIncluded
|
|
Yi = regressorWithMotorDynamicsPndbt(qi, qdi, q2di);
|
|
else
|
|
Yi = full_regressor_plnr(qi, qdi, q2di);
|
|
end
|
|
Ybi = Yi*plnrBaseQR.permutationMatrix(:,1:plnrBaseQR.numberOfBaseParameters);
|
|
Yfrctni = frictionRegressor(qdi);
|
|
W = vertcat(W, [Ybi, Yfrctni]);
|
|
|
|
taui = [-pendubot.torque(i), 0]';
|
|
% taui = [pendubot.current(i)*0.123, 0]';
|
|
Tau = vertcat(Tau, taui);
|
|
end
|
|
|
|
|
|
%% Usual Least Squares Approach
|
|
pi_hat = (W'*W)\(W'*Tau)
|
|
|
|
|
|
%% Set-up SDP optimization procedure
|
|
physicalConsistency = 0;
|
|
|
|
pi_frctn = sdpvar(6,1);
|
|
pi_b = sdpvar(plnrBaseQR.numberOfBaseParameters,1); % variables for base paramters
|
|
pi_d = sdpvar(15,1); % variables for dependent paramters
|
|
|
|
% Bijective mapping from [pi_b; pi_d] to standard parameters pi
|
|
pii = plnrBaseQR.permutationMatrix*[eye(plnrBaseQR.numberOfBaseParameters), ...
|
|
-plnrBaseQR.beta; ...
|
|
zeros(15,plnrBaseQR.numberOfBaseParameters), ...
|
|
eye(15)]*[pi_b; pi_d];
|
|
|
|
cnstr = [];
|
|
if physicalConsistency
|
|
for i = 1:11:21
|
|
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 = pii(i+6:i+8);
|
|
|
|
Di = [0.5*trace(link_inertia_i)*eye(3) - link_inertia_i, ...
|
|
frst_mmnt_i; frst_mmnt_i', pii(i+9)];
|
|
|
|
cnstr = [cnstr, Di>0];
|
|
end
|
|
else
|
|
for i = 1:11:21
|
|
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
|
|
end
|
|
cnstr = [cnstr, pii(11) > 0]; % first motor inertia constraint
|
|
|
|
% % Feasibility constraints on the friction prameters
|
|
% for i = 1:2
|
|
% cnstr = [cnstr, pi_frctn(3*i-2)>0, pi_frctn(3*i-1)>0];
|
|
% end
|
|
|
|
% Defining pbjective function
|
|
obj = norm(Tau - W*[pi_b; pi_frctn], 2);
|
|
|
|
% Solving sdp problem
|
|
sol2 = optimize(cnstr, obj, sdpsettings('solver','sdpt3'));
|
|
|
|
pi_b = value(pi_b) % variables for base paramters
|
|
pi_frctn = value(pi_frctn)
|
|
|
|
|
|
|