IRDYn/get_baseParams.m

function robot = get_baseParams(robot,opt)
% Seed the random number generator based on the current time
rng('shuffle');
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);
    else
        standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
        Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
    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
[Q, 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 reguar 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)<sqrt(eps)) = 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-6,... 
        'Found realationship between W1 and W2 is not correct\n');

% -----------------------------------------------------------------------
% Find base parmaters
% -----------------------------------------------------------------------
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;

% ---------------------------------------------------------------------
% 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;

% ---------------------------------------------------------------------
% 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');
robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);
matlabFunction(robot.baseQR.regressor,'File',sprintf('autogen/base_regressor_%s',opt.robotName),...
    'Vars',{q_sym,qd_sym,q2d_sym});