IRDYn/get_regressor.m

function robot = get_regressor(robot, opt)
% Create symbolic generilized coordiates, their first and second deriatives
ndof = robot.ndof;
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');
% init regressor
robot.regressor.m = sym('m%d',[ndof,1],'real');
robot.regressor.mc_x = sym('mc%d_x',[ndof,1],'real');
robot.regressor.mc_y = sym('mc%d_y',[ndof,1],'real');
robot.regressor.mc_z = sym('mc%d_z',[ndof,1],'real');
robot.regressor.ixx = sym('i%d_xx',[ndof,1],'real');
robot.regressor.ixy = sym('i%d_xy',[ndof,1],'real');
robot.regressor.ixz = sym('i%d_xz',[ndof,1],'real');
robot.regressor.iyy = sym('i%d_yy',[ndof,1],'real');
robot.regressor.iyz = sym('i%d_yz',[ndof,1],'real');
robot.regressor.izz = sym('i%d_zz',[ndof,1],'real');
robot.regressor.im = sym('im%d',[ndof,1],'real');
for i = 1:ndof
    robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),...
        robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),...
        robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]';
end
[nLnkPrms, nLnks] = size(robot.regressor.pi);
robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]);
% init matrix
R = robot.kine.R;
P = robot.kine.t;
w = robot.vel.w ;
dw = robot.vel.dw ;
dv = robot.vel.dv ;
switch opt.LD_method
    case 'Direct'
        switch opt.KM_method
            case {'MDH' , 'SCREW'}
                for i = 1:ndof
                    p_skew(:,:,i) = vec2skewSymMat(P(:,:,i));
                    w_skew(:,:,i) = vec2skewSymMat(w(:,i));
                    dw_skew(:,:,i) = vec2skewSymMat(dw(:,i));
                    dv_skew(:,:,i) = vec2skewSymMat(dv(:,i));
                    w_l(:,:,i) = vec2linearSymMat(w(:,i));
                    dw_l(:,:,i) = vec2linearSymMat(dw(:,i));
                    % size of matrix A is 6*10, need to -1
                    robot.regressor.A(:,:,i) = [dv(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
                        zeros(3,1),-dv_skew(:,:,i),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
                    %? match screw method
                    robot.regressor.A(:,:,i) = Adjoint(RpToTrans(robot.TW(1:3,1:3,i),[0;0;0]))'*robot.regressor.A(:,:,i);
                end
                % construct matrix U, size: [6*n,10*n]
                % U_ = sym(zeros([6*ndof,10*ndof]));
                U_ = [];
                for i = 1:ndof
                    % tricky
                    for j = i:ndof
                        if(j == i)
                            TT = eye(6,6);
                            U_row = TT*robot.regressor.A(:,:,j);
                        else
                            TT = TT*Adjoint(RpToTrans(R(:,:,j),P(:,:,j)));
                            U_row = [U_row,TT*robot.regressor.A(:,:,j)];
                        end
                    end
                    U_ = [U_;zeros(6,(i-1)*10),U_row];
                end
                robot.regressor.U = U_;
                delta_ = zeros([ndof,6*ndof]);
                for i = 1:ndof
                    delta_(i,6*i) = 1;

%                     %FIXME: use link type
%                     if(i==ndof)
%                         delta_(i,6*i) = 0;
%                     end
                end
                robot.regressor.K = delta_*robot.regressor.U;
                if(opt.debug)
                    sprintf('size of U=%dx%d.',size(robot.regressor.U))
                    sprintf('size of K=%dx%d.',size(robot.regressor.K))
                end
        end
        %         matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
        %             'Vars',{q_sym,qd_sym,q2d_sym});
        if(opt.reGenerate)
            tic
            disp('compiling robot.regressor.K');
            matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
                'Vars',{q_sym});
            compileTime = toc;
            fprintf("The total compile time was: = %d minutes, %d seconds\n", floor(compileTime/60), ceil(rem(compileTime,60)));
        end
%     matlabFunction(Y_f,'File','autogen/standard_regressor_Two_bar',...
%                    'Vars',{q_sym,qd_sym,q2d_sym});
    case 'Lagrange'
        disp('TODO opt.LD_method Lagrange!')
        return;
    otherwise
        disp('Bad opt.KM_method!')
        return;
end