IRDYn/get_baseParams.m

function robot = get_baseParams(robot,opt)
% Seed the random number generator based on the current time
rng("default");
ndof = robot.ndof;
includeMotorDynamics = false;
% ------------------------------------------------------------------------
% Set limits on posistion and velocities
% ------------------------------------------------------------------------
q_min = -pi*ones(ndof,1);
q_max = pi*ones(ndof,1);
qd_max = 3*pi*ones(ndof,1);
q2d_max = 6*pi*ones(ndof,1); 
% -----------------------------------------------------------------------
% Find relation between independent columns and dependent columns
% -----------------------------------------------------------------------
% Get observation matrix of identifiable paramters
W = [];   
for i = 1:25
    q_rnd = q_min + (q_max - q_min).*rand(ndof,1);
    qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1);
    q2d_rnd = -q2d_max + 2*q2d_max.*rand(ndof,1);
    
    if includeMotorDynamics
%         Y = regressorWithMotorDynamics(q_rnd,qd_rnd,q2d_rnd);
    elseif opt.isJointTorqueSensor
        standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
        Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
%         joint_idex = ndof-2;
%         Y = Y(joint_idex:end,:);
    elseif opt.isSixAxisFTSensor
        regressor_func = sprintf('regressor_%s',opt.robotName);
        Y = feval(regressor_func, q_rnd,qd_rnd,q2d_rnd);
        joint_idex = ndof-2;
        Y = Y(6*(joint_idex-1)+1:6*(joint_idex),:);
        % FIXME hack here
%         standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
%         Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
%         Zeros_ = zeros(size(Y));
%         Zeros_(ndof-2,:) = Y(ndof-2,:);
%         Y = Zeros_;
    end
    W = vertcat(W,Y);
end

% QR decomposition with pivoting: W*E = Q*R
%   R is upper triangular matrix
%   Q is unitary matrix
%   E is permutation matrix
[~, R, E] = qr(W);

% matrix W has rank qr_rank which is number number of base parameters 
qr_rank = rank(W);

% R = [R1 R2; 
%      0  0]
% R1 is bbxbb upper triangular and regu ar matrix
% R2 is bbx(c-qr_rank) matrix where c is number of standard parameters
R1 = R(1:qr_rank,1:qr_rank);
R2 = R(1:qr_rank,qr_rank+1:end);
beta = R1\R2; % the zero rows of K correspond to independent columns of WP
beta(abs(beta)<10^-5) = 0; % get rid of numerical errors
% W2 = W1*beta

% Make sure that the relation holds
W1 = W*E(:,1:qr_rank);
W2 = W*E(:,qr_rank+1:end);
assert(norm(W2 - W1*beta) < 1e-3,... 
        'Found realationship between W1 and W2 is not correct\n');
% sort QR result
[~,RR]=qr(W);
[~,index]=sort(abs(diag(RR)),'descend');
A = sort(index(1:qr_rank));
% this matrix is wrong, beacause A sort again, should be [(W*E1gen1)(1:qr_rank),
% (W*E1gen2)(qr_rank+1:end)]? for example: 9 to 1, 1 to 17: 9->17; 
% No, only leaf E1gen(RR,index(i),i); still not correct
P = eye(90);
for i = 1:90
    if i <16
        temp(:,:,i) = E1gen(RR,i,A(i));
    else
        temp(:,:,i) = E1gen(RR,i,index(i));
    end
    P = P*temp(:,:,i);
end

[~,RRR]=qr(W(:,[A;index(16:end)]));
RRR1 = RRR(1:qr_rank,1:qr_rank);
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_test(abs(beta_test)<10^-5) = 0; % get rid of numerical errors
% W2 = W1*beta

% Make sure that the relation holds
% WW1 = W*P(:,1:qr_rank); % swap col
% WW2 = W*P(:,qr_rank+1:end);
% assert(norm(WW2 - WW1*beta_test) < 1e-3,... 
%         'Found realationship between W1 and W2 is not correct\n');
% get sort result : pi=pi1+beta_test*pi2;
% -----------------------------------------------------------------------
% Find base parmaters
% -----------------------------------------------------------------------
if(opt.Isreal)
    pi_lgr_num = robot.pi;
    [nLnkPrms, nLnks] = size(pi_lgr_num);
    pi_lgr_num = reshape(pi_lgr_num, [nLnkPrms*nLnks, 1]);
    pi1 = E(:,1:qr_rank)'*pi_lgr_num; % independent paramters
    pi2 = E(:,qr_rank+1:end)'*pi_lgr_num; % dependent paramteres
    pi_lgr_base = pi1 + beta*pi2;
else
    pi_lgr_sym = robot.regressor.pi;
    pi1 = E(:,1:qr_rank)'*pi_lgr_sym; % independent paramters
    pi2 = E(:,qr_rank+1:end)'*pi_lgr_sym; % dependent paramteres
    
    % all of the expressions below are equivalent
    pi_lgr_base = pi1 + beta*pi2;
    % pi_lgr_base = [eye(qr_rank) beta]*[pi1;pi2];
    % pi_lgr_base = [eye(qr_rank) beta]*E'*pi_lgr_sym;
end
% ---------------------------------------------------------------------
% Create structure with the result of QR decompositon a
% ---------------------------------------------------------------------
baseQR = struct;
baseQR.numberOfBaseParameters = qr_rank;
baseQR.permutationMatrix = E;
baseQR.beta = beta;
baseQR.motorDynamicsIncluded = includeMotorDynamics;
baseQR.baseParams = pi_lgr_base;
robot.baseQR = baseQR;
robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);

% Get observation matrix of identifiable paramters
for j = 1:robot.ndof
    W = [];
    for i = 1:25
        q_rnd = q_min + (q_max - q_min).*rand(ndof,1);
        qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1);
        q2d_rnd = -q2d_max + 2*q2d_max.*rand(ndof,1);
        standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
        Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
        W = vertcat(W,Y(j,:));
    end
    % matrix W has rank qr_rank which is number number of base parameters 
    robot.baseQR.rank(j) = rank(W); 
end

if(opt.reGenerate)
    tic
    disp('compiling robot.baseQR.regressor');
    % ---------------------------------------------------------------------
    % Gen base_regressor
    % ---------------------------------------------------------------------
    q_sym = sym('q%d',[ndof+1,1],'real');
    qd_sym = sym('qd%d',[ndof+1,1],'real');
    q2d_sym = sym('qdd%d',[ndof+1,1],'real');
    matlabFunction(robot.baseQR.regressor,'File',sprintf('autogen/base_regressor_%s',opt.robotName),...
        'Vars',{q_sym,qd_sym,q2d_sym});
    fprintf("The total compile time was: = %d minutes, %d seconds\n", floor(compileTime/60), ceil(rem(compileTime,60)));
end