119 lines
5.1 KiB
Matlab
119 lines
5.1 KiB
Matlab
function robot = estimate_dyn(robot,opt)
|
|
% -------------------------------------------------------------------
|
|
% Get datas
|
|
% ------------------------------------------------------------------------
|
|
time = 0:0.01:0.5;
|
|
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;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
|
|
qd=[qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J];
|
|
qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J];
|
|
g = [0; 0; -9.8];
|
|
toruqeSensorRange = [500;500;230;90;60;60;60;20;20];
|
|
% pi -> [m;mc;I] 10 element
|
|
[nLnkPrms, nLnks] = size(robot.pi);
|
|
robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
|
|
if opt.isJointTorqueSensor
|
|
%hack
|
|
tau = zeros([robot.ndof,length(q_J)]);
|
|
% tau = zeros([1,length(q_J)]);
|
|
for i = 1:length(q_J)
|
|
% regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
|
|
standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
|
|
regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i));
|
|
tau_Full=regressor*robot_pi;
|
|
tau(:,i) = tau_Full;
|
|
% 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)));
|
|
end
|
|
elseif opt.isSixAxisFTSensor
|
|
tau = zeros([6,length(q_J)]);
|
|
for i = 1:length(q_J)
|
|
regressor_func = sprintf('regressor_%s',opt.robotName);
|
|
regressor = feval(regressor_func,q(:,i),qd(:,i),qdd(:,i));
|
|
FT = regressor*robot_pi;
|
|
joint_idex = robot.ndof-2;
|
|
tau(:,i) = FT(6*(joint_idex-1)+1:6*(joint_idex));
|
|
end
|
|
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, opt);
|
|
|
|
% ---------------------------------------------------------------------
|
|
% 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,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
|
|
end |