Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

MLSE GC

This commit is contained in:
cosmic_power 2024-11-14 20:49:05 +08:00
parent 1185d20dd1
commit d7d355a51f
5 changed files with 133 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Identification_main.m
View File

@ -32,4 +32,4 @@ robot = get_baseParams(robot, opt);
% robot.currentData = currentData; % robot.currentData = currentData;
% robot = estimate_dyn(robot,opt); % robot = estimate_dyn(robot,opt);
% robot = estimate_dyn_form_data(robot,opt); % robot = estimate_dyn_form_data(robot,opt);
% robot = estimate_dyn_MLS(robot,opt); robot = estimate_dyn_MLS(robot,opt);

13
estimate_dyn.m
View File

@ -17,15 +17,16 @@ toruqeSensorRange = [500;500;230;90;60;60;60;20;20];
robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]); robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
if opt.isJointTorqueSensor if opt.isJointTorqueSensor
%hack %hack
% tau = zeros([robot.ndof,length(q_J)]); tau = zeros([robot.ndof,length(q_J)]);
tau = zeros([1,length(q_J)]); % tau = zeros([1,length(q_J)]);
for i = 1:length(q_J) for i = 1:length(q_J)
% regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i)); % regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName); standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i)); regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i));
tau_Full=regressor*robot_pi; tau_Full=regressor*robot_pi;
joint_idex = robot.ndof-2; tau(:,i) = tau_Full;
tau(:,i) = tau_Full((joint_idex-1)+1:(joint_idex)); % joint_idex = robot.ndof-2;
% tau(:,i) = tau_Full((joint_idex-1)+1:(joint_idex));
% tau(:,i) = tau(:,i) + 5*10^-3*toruqeSensorRange.*rand(size(tau(:,i))); % tau(:,i) = tau(:,i) + 5*10^-3*toruqeSensorRange.*rand(size(tau(:,i)));
end end
elseif opt.isSixAxisFTSensor elseif opt.isSixAxisFTSensor
@ -82,8 +83,8 @@ robot.sol = sol;
Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ... Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ...
idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR); idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR);
%hack %hack
joint_idex = robot.ndof-2; % joint_idex = robot.ndof-2;
Yb = Yb((joint_idex-1)+1:(joint_idex),:); % Yb = Yb((joint_idex-1)+1:(joint_idex),:);
Wb = vertcat(Wb, Yb); Wb = vertcat(Wb, Yb);
% Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)'); % Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i)); Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));

110
estimate_dyn_MLS.asv Normal file
View File

@ -0,0 +1,110 @@
function robot = estimate_dyn_MLS(robot,opt)
% -------------------------------------------------------------------
% Get datas
% ------------------------------------------------------------------------
get_GCTraj_R1000_EVT;
% -------------------------------------------------------------------
% Generate Regressors based on data
% ------------------------------------------------------------------------
drvGains = [];
baseQR = robot.baseQR;
for i = 1:1:robot.ndof
q = idntfcnTrjctry(i).q;
qd = idntfcnTrjctry(i).qd;
qdd = idntfcnTrjctry(i).qdd;
[nRow,nCol] = size(idntfcnTrjctry(i).qd);
Wb = []; Tau = [];
for j = 1:nRow/robot.ndof
for k = 1:nCol
if opt.isJointTorqueSensor
base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
Yb = feval(base_regressor_func, q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
qd(robot.ndof*(j-1)+1:robot.ndof*j,k),...
qdd(robot.ndof*(j-1)+1:robot.ndof*j,k),baseQR);
Yb = Yb(i,:);
Wb = vertcat(Wb, Yb);
Tau = vertcat(Tau, idntfcnTrjctry(i).tau(j,k));
end
end
end
observationMatrix(i).Wb = Wb;
observationMatrix(i).Tau = Tau;
observationMatrix(i).rank = robot.baseQR.rank(i);
end
% ---------------------------------------------------------------------
% Estimate parameters
% ---------------------------------------------------------------------
sol = struct;
for i = 9:-1:1
Wb = observationMatrix(i).Wb;
Tau = observationMatrix(i).Tau;
% [nRow,nCol] = size(Wb);
if i == 9
pib_OLS=pinv(Wb(:,15-observationMatrix(i).rank+1"))*Tau;
pib_MLS = [];
elseif i > 1
pib_OLS=pinv(Wb(:,15-observationMatrix(i).rank+1:15-observationMatrix(i+1).rank))*...
(-Wb(:,15-observationMatrix(i+1).rank+1)*pib_MLS+Tau);
else
break;
end
pifrctn_OLS = 0;
pib_MLS = [pib_OLS;pib_MLS];
end
a=1
% method = 'OLS';
% if strcmp(method, 'OLS')
% % Usual least squares
% [sol.pi_b, sol.pi_fr] = ordinaryLeastSquareEstimation(observationMatrix);
% elseif strcmp(method, 'PC-OLS')
% % Physically consistent OLS using SDP optimization
% [sol.pi_b, sol.pi_fr, sol.pi_s] = physicallyConsistentEstimation(Tau, Wb, baseQR);
% else
% error("Chosen method for dynamic parameter estimation does not exist");
% end
% robot.sol = sol;
% % Local unctions
% function observationMatrix = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains,opt)
% for i = 1:1:robot.ndof
% [nRow,nCol] = size(idntfcnTrjctry(i).qd);
% Wb = []; Tau = [];
% for j = 1:nRow/robot.ndof
% for k = 1:nCol
% if opt.isJointTorqueSensor
% base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
% Yb = feval(base_regressor_func, q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
% qd(robot.ndof*(j-1)+1:robot.ndof*j,k),...
% qdd(robot.ndof*(j-1)+1:robot.ndof*j,k),baseQR);
% Yb = Yb(i,:);
% Wb = vertcat(Wb, Yb);
% Tau = vertcat(Tau, idntfcnTrjctry(i).tau(j,k));
% end
% end
% end
% observationMatrix(i).Wb = Wb;
% observationMatrix(i).Tau = Tau;
% end
% end
%
%
% function [pib_OLS, pifrctn_OLS] = ordinaryLeastSquareEstimation(observationMatrix)
% % Function perfroms ordinary least squares estimation of parameters
% % pi_OLS = (Wb'*Wb)\(Wb'*Tau);
% % pib_OLS = pi_OLS(1:40); % variables for base paramters
% % pifrctn_OLS = pi_OLS(41:end);
% for i = 9:-1:1
% Wb = observationMatrix(i).Wb;
% Tau = observationMatrix(i).Tau;
% pib_OLS(i)=pinv(Wb)*Tau;
% pifrctn_OLS = 0;
% end
% end
% function [pib_OLS, pifrctn_OLS] = MultiLeastSquareEstimation(idntfcnTrjctry, Tau, Wb)
% % Function perfroms Multi step ordinary least squares estimation of parameters
%
% pib_OLS=pinv(Wb)*Tau;
% pifrctn_OLS = 0;
% end
end

8
estimate_dyn_MLS.m
View File

@ -42,16 +42,16 @@ for i = 9:-1:1
Tau = observationMatrix(i).Tau; Tau = observationMatrix(i).Tau;
% [nRow,nCol] = size(Wb); % [nRow,nCol] = size(Wb);
if i == 9 if i == 9
pib_OLS=pinv(Wb(:,1:observationMatrix(i).rank))*Tau; pib_OLS=pinv(Wb(:,15-observationMatrix(i).rank+1:end))*Tau;
pib_MLS = []; pib_MLS = [];
elseif i > 1 elseif i > 1
pib_OLS=pinv(Wb(:,observationMatrix(i+1).rank+1:observationMatrix(i).rank))*... pib_OLS=pinv(Wb(:,15-observationMatrix(i).rank+1:15-observationMatrix(i+1).rank))*...
(-Wb(:,1:observationMatrix(i+1).rank)*pib_MLS+Tau); (-Wb(:,15-observationMatrix(i+1).rank+1:end)*pib_MLS+Tau);
else else
break; break;
end end
pifrctn_OLS = 0; pifrctn_OLS = 0;
pib_MLS = [pib_MLS;pib_OLS]; pib_MLS = [pib_OLS;pib_MLS];
end end
a=1 a=1
% method = 'OLS'; % method = 'OLS';

28
get_baseParams.m
View File

@ -70,31 +70,25 @@ assert(norm(W2 - W1*beta) < 1e-3,...
[~,RR]=qr(W); [~,RR]=qr(W);
[~,index]=sort(abs(diag(RR)),'descend'); [~,index]=sort(abs(diag(RR)),'descend');
A = sort(index(1:qr_rank)); A = sort(index(1:qr_rank));
% this matrix is wrong, beacause A sort again, should be [(W*E1gen1)(1:qr_rank), A = [A;index(qr_rank+1:end)];
% (W*E1gen2)(qr_rank+1:end)]? for example: 9 to 1, 1 to 17: 9->17; P = zeros(90);
% No, only leaf E1gen(RR,index(i),i); still not correct
P = eye(90);
for i = 1:90 for i = 1:90
if i <16 P(A(i),i) = 1;
temp(:,:,i) = E1gen(RR,i,A(i));
else
temp(:,:,i) = E1gen(RR,i,index(i));
end
P = P*temp(:,:,i);
end end
[~,RRR]=qr(W(:,[A;index(16:end)])); [~,RRR]=qr(W*P);
RRR1 = RRR(1:qr_rank,1:qr_rank); RRR1 = RRR(1:qr_rank,1:qr_rank);
RRR2 = RRR(1:qr_rank,qr_rank+1:end); RRR2 = RRR(1:qr_rank,qr_rank+1:end);
beta_test = RRR1\RRR2; % the zero rows of K correspond to independent columns of WP beta = RRR1\RRR2; % the zero rows of K correspond to independent columns of WP
beta_test(abs(beta_test)<10^-5) = 0; % get rid of numerical errors beta(abs(beta)<10^-5) = 0; % get rid of numerical errors
% W2 = W1*beta % W2 = W1*beta
% Make sure that the relation holds % Make sure that the relation holds
% WW1 = W*P(:,1:qr_rank); % swap col WW1 = W*P(:,1:qr_rank); % swap col
% WW2 = W*P(:,qr_rank+1:end); WW2 = W*P(:,qr_rank+1:end);
% assert(norm(WW2 - WW1*beta_test) < 1e-3,... assert(norm(WW2 - WW1*beta) < 1e-3,...
% 'Found realationship between W1 and W2 is not correct\n'); 'Found realationship between W1 and W2 is not correct\n');
E = P;
% get sort result : pi=pi1+beta_test*pi2; % get sort result : pi=pi1+beta_test*pi2;
% ----------------------------------------------------------------------- % -----------------------------------------------------------------------
% Find base parmaters % Find base parmaters