112 lines
4.5 KiB
Matlab
112 lines
4.5 KiB
Matlab
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 = []; isSixAxisFTSensor =opt.isSixAxisFTSensor; isJointTorqueSensor =opt.isJointTorqueSensor;
|
|
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 isJointTorqueSensor
|
|
standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
|
|
Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
|
|
elseif 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');
|
|
|
|
% -----------------------------------------------------------------------
|
|
% 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);
|
|
|
|
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 |