IRDYn/get_robot_R1000.m

function robot = get_robot_R1000(file,opt)
switch opt.robot_def
    case 'direct'
        ndof = 9;
        robot.ndof = ndof;
        % Kinematics parameters
        if(opt.Isreal)
            switch opt.KM_method
                case 'SDH'
                case 'MDH'
                    robot.theta = [0,0,0,0,-pi/2,0,-pi/2,-pi/2,0];
                    robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.05896];
                    robot.d = [0,0,0,0,0,0.28,0.40,0,0];
                    robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
                    robot.link_type = ['R','R','R','R','R','R','R','R','P'];
                case 'SCREW'
                    robot.theta = zeros(1,9);
                    robot.theta(5) = pi/3;
                    robot.theta(7) = pi/3;
                otherwise
                    disp('Bad opt.KM_method!')
                    return;
            end
        else
            % Create symbolic generilized coordiates, their first and second deriatives
            q_sym = sym('q%d',[ndof,1],'real');
            qd_sym = sym('qd%d',[ndof,1],'real');
            q2d_sym = sym('qdd%d',[ndof,1],'real');

            robot.theta = q_sym;
            robot.dtheta = qd_sym;
            robot.ddtheta = q2d_sym;
            %R1000 ISA
            robot.theta(ndof) = 0;
            robot.dtheta(ndof) = 0;
            robot.ddtheta(ndof) = 0;
            switch opt.KM_method
                case 'SDH'
                case 'MDH'
                    robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493];
                    robot.d = [0,0,0,0,0,0.28,0.40,0,-0.3157];
                    robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
                    robot.link_type = ['R','R','R','R','R','R','R','R','P'];
                otherwise
                    disp('Bad opt.KM_method!')
                    return;
            end
            robot.d = sym(robot.d);
            robot.d(ndof)=q_sym(ndof);
            %init vd
            robot.vd = robot.d;
            robot.vd(ndof)=q_sym(ndof);
            %init accd
            robot.accd = robot.d;
            robot.accd(ndof)=q_sym(ndof);
        end
        % Dynamics parameters
        link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2];
        %TODO in process, seems axis_of_rot useless
        axis_of_rot(:,1) = [0;0;1];
        axis_of_rot(:,2) = [0;-1;0];
        axis_of_rot(:,3) = [0;-1;0];
        axis_of_rot(:,4) = [0;-1;0];
        axis_of_rot(:,5) = [0;0;1];
        axis_of_rot(:,6) = [1;0;0];
        axis_of_rot(:,7) = [0;0;-1];
        axis_of_rot(:,8) = [0;-1;0];
        axis_of_rot(:,9) = [0;0;0];
        % 画图
        %         com_pos(:,1) = [0;0;0.122];
        %         com_pos(:,2) = [0.373;0;0];
        %         com_pos(:,3) = [0.188;0;0];
        %         com_pos(:,4) = [0.05;0;-0.05];
        %         com_pos(:,5) = [0;0.13;0];
        %         com_pos(:,6) = [0;0.028;0.049];
        %         com_pos(:,7) = [0;0;0.102];
        %         com_pos(:,8) = [0;0.06;0];
        %         com_pos(:,9) = [0;0;0];

        com_pos(:,1) = [9.7435250e+01  8.3517655e-01  1.2246547e+02]'*10^-3;
        com_pos(:,2) = [3.7345395e+02  4.4313141e-02 -5.5328829e+01]'*10^-3;
        com_pos(:,3) = [1.8811711e+02  4.9225523e-04 -7.9651429e+00]'*10^-3;
        com_pos(:,4) = [5.4048685e+01  5.8463901e+01 -5.0915631e+00]'*10^-3;
        com_pos(:,5) = [1.3028528e+02  4.8953539e-02  4.6198421e+01]'*10^-3;
        com_pos(:,6) = [4.9059639e+01  5.9928547e-02 -2.8858572e+01]'*10^-3;
        com_pos(:,7) = [2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3;
        com_pos(:,8) = [-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
        com_pos(:,9) = [-9.6776846e-02  1.4179201e-01 -3.4242667e+01]'*10^-3;

        com_pos_R1(:,1)=[9.7435250e+01  8.3517655e-01  1.2246547e+02]'*10^-3;
        com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'*10^-3;
        com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'*10^-3;
        com_pos_R2(:,2)=[ -1.2654643e+02  9.6371171e+01 -4.4094190e-02]'*10^-3;
        com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'*10^-3;
        com_pos_R2(:,3)=[-2.6188289e+02  2.8348571e+00 -4.9225523e-04 ]'*10^-3;
        com_pos_R1(:,4)=[ 5.4048685e+01  5.8463901e+01 -5.0915631e+00 ]'*10^-3;
        com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00  5.2908437e+01]'*10^-3;
        com_pos_R1(:,5)=[1.3028528e+02  4.8953539e-02  4.6198421e+01]'*10^-3;
        com_pos_R2(:,5)=[-9.5814715e+01  4.8953539e-02 -1.2301579e+01 ]'*10^-3;
        com_pos_R1(:,6)=[4.9059639e+01  5.9928547e-02 -2.8858572e+01]'*10^-3;
        com_pos_R2(:,6)=[-4.7403608e+00  5.9928547e-02  6.2741428e+01]'*10^-3;
        com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3;
        com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00  2.0257876e+02 ]'*10^-3;
        com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
        com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01  1.4875409e+01]'*10^-3; % don't use
        com_pos_R1(:,9)=[0 0 0]'*10^-3;

        % the inertia tensor wrt the frame oriented as the body frame and with the
        % origin in the COM
        %         link_inertia(:,:,1) = diag([1,1,1]);
        %         link_inertia(:,:,2) = diag([1,1,1]);
        %         link_inertia(:,:,3) = diag([1,1,1]);
        %         link_inertia(:,:,4) = diag([1,1,1]);
        %         link_inertia(:,:,5) = diag([1,1,1]);
        %         link_inertia(:,:,6) = diag([1,1,1]);
        %         link_inertia(:,:,7) = diag([1,1,1]);
        %         link_inertia(:,:,8) = diag([1,1,1]);
        %         link_inertia(:,:,9) = diag([1,1,1]);
        link_inertia(:,:,1) = [[1.1212091e+05 -1.1694021e+04 -4.7186593e+04];...
            [-1.1694021e+04  2.3289532e+05 -3.0395414e+02];...
            [-4.7186593e+04 -3.0395414e+02  2.0033875e+05]]*10^-6;
        link_inertia(:,:,2) = [[3.7926328e+04 -9.0569033e+01  4.7526575e+04];...
            [-9.0569033e+01  2.9714754e+05  6.8396715e+00];...
            [4.7526575e+04  6.8396715e+00  2.8138392e+05]]*10^-6;
        link_inertia(:,:,3) = [[4.4513887e+03  1.9981964e-01 -3.0303891e+02];...
            [1.9981964e-01  6.7952039e+04 -8.8585864e-02];...
            [-3.0303891e+02 -8.8585864e-02  6.9958344e+04]]*10^-6;
        link_inertia(:,:,4) = [[1.1642351e+04  2.2997175e+03  2.9159431e+03];...
            [2.2997175e+03  2.6031269e+04 -1.3518384e+02];...
            [2.9159431e+03 -1.3518384e+02  2.4694742e+04]]*10^-6;
        link_inertia(:,:,5) = [[3.0930544e+03  8.3558814e-01 -2.8169092e+03];...
            [8.3558814e-01  1.2796446e+04 -3.3666469e+00];...
            [-2.8169092e+03 -3.3666469e+00  1.2128856e+04]]*10^-6;
        link_inertia(:,:,6) = [[3.6635776e+03 -7.0081461e+00  2.2392870e+00];...
            [-7.0081461e+00  1.8152305e+03 -2.4828765e+02];...
            [2.2392870e+00 -2.4828765e+02  3.4602935e+03]]*10^-6;
        link_inertia(:,:,7) = [[1.3662652e+04 -3.6340953e+00  4.4011670e-01];...
            [-3.6340953e+00  1.3222824e+04 -4.3625500e+02];...
            [ 4.4011670e-01 -4.3625500e+02  2.2500397e+03]]*10^-6;
        link_inertia(:,:,8) = [[1.5162872e+03  -4.6245133e+01  4.4556929e+03];...
            [-4.6245133e+01  5.9901606e+04  6.0350548e+0];...
            [4.4556929e+03  6.0350548e+0  5.8819998e+04]]*10^-6;
        link_inertia(:,:,9) = [[1.1730106e+03  3.3834506e+0  4.6097678e+01];...
            [3.3834506e+0  1.7996852e+03  5.2088778e+0];...
            [4.6097678e+01  5.2088778e+0  1.3960734e+03]]*10^-6;
        % verify if link_inertia is issymmetric
        for i =  1:ndof
            if(issymmetric(link_inertia(:,:,i))==false)
                fprintf('Bad definition of inertia matrix %d\n',i)
                return;
            end
        end
        % manipulator regressor
        for i = 1:ndof
            robot.m(i) = link_mass(i);
            robot.axis(:,i) = axis_of_rot(i);
            robot.com(:,i) = com_pos(:,i);
            robot.I(:,:,i) = link_inertia(:,:,i);
            robot.mc(:,i) = link_mass(i)*com_pos(:,i);
            % the inertia tensor wrt the frame oriented as the body frame and with the
            % origin in the Joint i
            com_vec2mat = vec2skewSymMat(com_pos(:,i));
            robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia(:,:,i)-...
                link_mass(i)*com_vec2mat*com_vec2mat);
            robot.pi(:,i) = [robot.m(i);robot.mc(:,i);robot.I_vec(:,i)];
        end
    case 'urdf'
        robot = parse_urdf(file);
    case 'mat'
        robot = [];
        disp('TODO mat robot define options!')
    otherwise
        robot = [];
        disp('Bad robot define options!')
        return
end
%Gravity
gravity = [0;0;9.8];
robot.gravity = gravity;