111 lines
4.8 KiB
Matlab
111 lines
4.8 KiB
Matlab
function robot = estimate_dyn_form_data(robot,opt)
|
|
% -------------------------------------------------------------------
|
|
% Get datas
|
|
% ------------------------------------------------------------------------
|
|
posData = robot.posData;
|
|
currentData = robot.currentData;
|
|
[nRow,nCol] = size(posData);
|
|
index_p = find(posData(:,nCol-1)==7);
|
|
jointPos_p = posData(index_p,1:9);
|
|
jointPos_p = jointPos_p(1:100:end,:);
|
|
index_n = find(posData(:,nCol-1)==-7);
|
|
jointPos_n = posData(index_n,1:9);
|
|
jointPos_n = jointPos_n(1:100:end,:);
|
|
q = [jointPos_p]';
|
|
qd = q; qdd = q;
|
|
motorConst = [0.405, 0.405, 0.167, 0.126,0.073,0.151,0.083,0.073,0.03185];
|
|
gearRatio = [101,101,100,100,100,100,100,100,100];
|
|
% pi -> [m;mc;I] 10 element
|
|
index_p = find(currentData(:,nCol-1)==7);
|
|
jointCur_p = currentData(index_p,1:9);
|
|
jointCur_p = jointCur_p(1:100:end,:);
|
|
index_n = find(currentData(:,nCol-1)==-7);
|
|
jointCur_n = currentData(index_n,1:9);
|
|
jointCur_n = jointCur_n(1:100:end,:);
|
|
current = (jointCur_p+jointCur_n)/2;
|
|
torque_cur = gearRatio.*motorConst.*current;
|
|
tau = torque_cur(:,7)';
|
|
|
|
idntfcnTrjctry.t = 1:length(q);
|
|
idntfcnTrjctry.q = q;
|
|
idntfcnTrjctry.qd = qd;
|
|
idntfcnTrjctry.qdd = qdd;
|
|
idntfcnTrjctry.tau = tau;
|
|
% -------------------------------------------------------------------
|
|
% Generate Regressors based on data
|
|
% ------------------------------------------------------------------------
|
|
drvGains = [];
|
|
baseQR = robot.baseQR;
|
|
[Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains, opt);
|
|
|
|
% ---------------------------------------------------------------------
|
|
% Estimate parameters
|
|
% ---------------------------------------------------------------------
|
|
sol = struct;
|
|
method = 'PC-OLS';
|
|
if strcmp(method, 'OLS')
|
|
% Usual least squares
|
|
[sol.pi_b, sol.pi_fr] = ordinaryLeastSquareEstimation(Tau, Wb);
|
|
elseif strcmp(method, 'PC-OLS')
|
|
% Physically consistent OLS using SDP optimization
|
|
[sol.pi_b] = physicallyConsistentEstimation(Tau, Wb);
|
|
else
|
|
error("Chosen method for dynamic parameter estimation does not exist");
|
|
end
|
|
robot.sol = sol;
|
|
% Local unctions
|
|
function [Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains,opt)
|
|
% The function builds observation matrix for UR10E
|
|
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
|
|
|
|
Wb = []; Tau = [];
|
|
for i = 1:1:length(idntfcnTrjctry.t)
|
|
% Yi = regressorWithMotorDynamics(idntfcnTrjctry.q(i,:)',...
|
|
% idntfcnTrjctry.qd(i,:)',...
|
|
% idntfcnTrjctry.q2d(i,:)');
|
|
% Yfrctni = frictionRegressor(idntfcnTrjctry.qd_fltrd(i,:)');
|
|
% Ybi = [Yi*E1, Yfrctni];
|
|
if opt.isJointTorqueSensor
|
|
base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
|
|
Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ...
|
|
idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR);
|
|
%hack
|
|
joint_idex = robot.ndof-2;
|
|
Yb = Yb((joint_idex-1)+1:(joint_idex),:);
|
|
Wb = vertcat(Wb, Yb);
|
|
% Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
|
|
Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));
|
|
elseif opt.isSixAxisFTSensor
|
|
base_6AxisFT_regressor_func = sprintf('base_6AxisFT_regressor_%s',opt.robotName);
|
|
Yb = feval(base_6AxisFT_regressor_func, idntfcnTrjctry.q(:,i), ...
|
|
idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR);
|
|
joint_idex = robot.ndof-2;
|
|
Yb = Yb(6*(joint_idex-1)+1:6*(joint_idex),:);
|
|
Wb = vertcat(Wb, Yb);
|
|
% Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
|
|
Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));
|
|
end
|
|
end
|
|
end
|
|
|
|
|
|
function [pib_OLS, pifrctn_OLS] = ordinaryLeastSquareEstimation(Tau, Wb)
|
|
% 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);
|
|
pib_OLS=pinv(Wb)*Tau;
|
|
pifrctn_OLS = 0;
|
|
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
|
|
function pib = physicallyConsistentEstimation(Tau, Wb)
|
|
% 求解线性规划问题
|
|
options = optimoptions('lsqlin','Algorithm','trust-region-reflective','Display','iter');
|
|
pib = lsqlin(Wb,Tau,[],[],[],[],-2*ones(5,1),2*ones(5,1),[0.9151,0.0092,0.0002,-0.0644,0.1067]',options)
|
|
end
|
|
end |