IRDYn/complie/R1000 DVT GravityModel V1/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 ;
v = robot.vel.v ;
dv = robot.vel.dv ;
%def
p_skew = zeros(3,3,9);w_skew = zeros(3,3,9);dw_skew=zeros(3,3,9);
v_skew = zeros(3,3,9);dv_skew = zeros(3,3,9);w_l=zeros(3,6,9);dw_l=zeros(3,6,9);
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));
                    v_skew(:,:,i) = vec2skewSymMat(v(:,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
                    % Traversaro el 2015
%                     robot.regressor.A(:,:,i) = [dv(:,i)+w_skew(:,:,i)*v(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
%                         zeros(3,1),-(dv_skew(:,:,i)+w_skew(:,:,i)*v_skew(:,:,i)-v_skew(:,:,i)*w_skew(:,:,i)),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
                    robot.regressor.A(:,:,i) = [dv(:,i)+w_skew(:,:,i)*v(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
                        zeros(3,1),-vec2skewSymMat((dv(:,i)+w_skew(:,:,i)*v(:,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_ = zeros(54,90);U_row = zeros(6,90);TT = eye(6,6);
                for i = 1:ndof
                    % tricky
                    for j = i:ndof
                        if(j == i)
                            TT = eye(6,6);
                            U_row(:,10*(j-1)+1:10*j) = TT*robot.regressor.A(:,:,j);
                        else
                            TT = TT*Adjoint(RpToTrans(R(:,:,j),P(:,:,j)));
                            U_row(:,10*(j-1)+1:10*j) = TT*robot.regressor.A(:,:,j);
                        end
                    end
                    U_(6*(i-1)+1:6*i,:) = U_row;
                    U_row = zeros(6,90);
                end
                robot.regressor.U = U_;
%                 delta_ = zeros([ndof,6*ndof]);
                delta_ = zeros([9,6*9]);
                for i = 1:ndof
                    delta_(i,6*i) = 1;

                    %FIXME: use link type, hack now
                    if(i==ndof)
                        delta_(i,6*i) = 0;
                        delta_(ndof,6*(ndof-1)+3) = 1;
                    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
    otherwise
        disp('Bad opt.KM_method!')
        return;
end