79 lines
3.0 KiB
Mathematica
79 lines
3.0 KiB
Mathematica
|
function robot = estimate_dyn(robot,opt)
|
||
|
|
% -------------------------------------------------------------------
|
||
|
|
% Get datas
|
||
|
|
% ------------------------------------------------------------------------
|
||
|
|
time = 0:0.01:2;
|
||
|
|
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);
|
||
|
|
q=[q_J;-q_J];
|
||
|
|
qd=[qd_J; -qd_J];
|
||
|
|
qdd=[qdd_J; -qdd_J];
|
||
|
|
g = [0; 0; -9.8];
|
||
|
|
tau = zeros([2,101]);
|
||
|
|
robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
|
||
|
|
robot_pi2=[2;1;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
|
||
|
|
robot_pi=[robot_pi1;robot_pi2];
|
||
|
|
for i = 1:length(q_J)
|
||
|
|
regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
|
||
|
|
tau(:,i)=regressor*robot_pi;
|
||
|
|
end
|
||
|
|
idntfcnTrjctry.t = time;
|
||
|
|
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);
|
||
|
|
|
||
|
|
% ---------------------------------------------------------------------
|
||
|
|
% Estimate parameters
|
||
|
|
% ---------------------------------------------------------------------
|
||
|
|
sol = struct;
|
||
|
|
method = '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, 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 [Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains)
|
||
|
|
% 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];
|
||
|
|
base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
|
||
|
|
Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ...
|
||
|
|
idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i));
|
||
|
|
Wb = vertcat(Wb, Yb);
|
||
|
|
% Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
|
||
|
|
Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));
|
||
|
|
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
|
||
|
|
end
|