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.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];
                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,1],'real');
            qd_sym = sym('qd%d',[ndof+1,1],'real');
            q2d_sym = sym('qdd%d',[ndof+1,1],'real');
            q_sym(ndof+1) = 0;
            qd_sym(ndof+1) = 0;
            q2d_sym(ndof+1) = 0;
            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];
                    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(ndof)=q_sym;
            %init vd
            robot.vd = robot.d; 
            robot.vd(ndof)=q_sym;
            %init accd
            robot.accd = robot.d;
            robot.accd(ndof)=q_sym;
        end
        % Dynamics parameters
        link_mass = [17.42,7.7,2.42,5.16,2.22,1.78,2.32,2.92 0.1];
        %TODO in process, seems axis_of_rot useless 
        axis_of_rot(:,1) = [0;0;1];
        axis_of_rot(:,2) = [0;0;1];
        axis_of_rot(:,3) = [0;0;1];
        axis_of_rot(:,4) = [0;0;1];
        axis_of_rot(:,5) = [0;0;1];
        axis_of_rot(:,6) = [0;0;1];
        axis_of_rot(:,7) = [0;0;1];
        axis_of_rot(:,8) = [0;0;1];
        axis_of_rot(:,9) = [0;0;1];
        % 画图
        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];
        % 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]);
        % 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;