update MLS
This commit is contained in:
cosmic_power
parent
d7d355a51f
commit
b20d613462
5
E1gen.m
5
E1gen.m
|
|
@ -1,5 +0,0 @@
|
|||
function E=E1gen(A,i,j)
|
||||
n=size(A); %求矩阵A的行数和列数
|
||||
m=min(n); %获取矩阵行数和列数中的最小值
|
||||
E=eye(m); %产生单位对角阵
|
||||
E(i,i)=0; E(j,j)=0; E(i,j)=1; E(j,i)=1;
|
||||
|
|
@ -1,110 +0,0 @@
|
|||
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
|
||||
|
|
@ -37,23 +37,24 @@ end
|
|||
% Estimate parameters
|
||||
% ---------------------------------------------------------------------
|
||||
sol = struct;
|
||||
for i = 9:-1:1
|
||||
for i = robot.ndof:-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:end))*Tau;
|
||||
if i == robot.ndof
|
||||
pib_OLS=pinv(Wb(:,baseQR.numberOfBaseParameters-observationMatrix(i).rank+1:end))*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:end)*pib_MLS+Tau);
|
||||
pib_OLS=pinv(Wb(:,baseQR.numberOfBaseParameters-observationMatrix(i).rank+1:...
|
||||
baseQR.numberOfBaseParameters-observationMatrix(i+1).rank))*...
|
||||
(-Wb(:,baseQR.numberOfBaseParameters-observationMatrix(i+1).rank+1:end)*pib_MLS+Tau);
|
||||
else
|
||||
break;
|
||||
end
|
||||
pifrctn_OLS = 0;
|
||||
pib_MLS = [pib_OLS;pib_MLS];
|
||||
end
|
||||
a=1
|
||||
sol.pib_MLS = pib_MLS;
|
||||
robot.sol = sol;
|
||||
% method = 'OLS';
|
||||
% if strcmp(method, 'OLS')
|
||||
% % Usual least squares
|
||||
|
|
|
|||
|
|
@ -1,30 +0,0 @@
|
|||
time = 0:0.01:1;
|
||||
f=1;
|
||||
q_J = sin(2*pi*f*time);
|
||||
qd_J = (2*pi*f)*cos(2*pi*f*time);
|
||||
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
|
||||
zero_ = zeros(1,length(q_J));
|
||||
|
||||
|
||||
% thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
thetalist = zeros(N,1);
|
||||
|
||||
Mlist_CG = robot.kine.Mlist_CG;
|
||||
Slist=robot.slist;
|
||||
|
||||
% Get general mass matrix
|
||||
Glist=[];
|
||||
for i = 1:N
|
||||
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
|
||||
Glist = cat(3, Glist, Gb);
|
||||
end
|
||||
gravity = [0;0;-9.806];
|
||||
|
||||
for i = 1:N
|
||||
gravityForces(:,i) = Glist(:,:,i)*[zeros(3,1);gravity];
|
||||
Jacoblist(:,:,i) = JacobianSpace(Slist(:,1:i),thetalist(1:i));
|
||||
end
|
||||
|
||||
for i = N:-1:1
|
||||
gravityTorques(i) = transpose(Jacoblist(:,:,i))*gravityForces(:,i);
|
||||
end
|
||||
Loading…
Reference in New Issue