IRDYn/get_robot.m

function robot = get_robot(file,opt)
switch opt.robot_def
    case 'direct'
        ndof = 2;
        robot.ndof = ndof;
        % Kinematics parameters
        if(opt.Isreal)
            switch opt.KM_method
                case 'SDH'
                case 'MDH'
                    robot.theta = zeros([1,ndof]);
                    robot.a = [0,1,1];
                    robot.d = [0,0,0];
                    robot.alpha = [0,0,0];
                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;
            switch opt.KM_method
                case 'SDH'
                case 'MDH'
                    robot.a = [0,1,1];
                    robot.d = [0,0,0];
                    robot.alpha = [0,0,0];
                otherwise
                    disp('Bad opt.KM_method!')
                    return;
            end
        end
        % Dynamics parameters
        link_mass = [1,1];
        axis_of_rot(:,:,1) = [0;0;1];
        axis_of_rot(:,:,2) = [0;0;1];
        com_pos(:,:,1) = [1/2;0;0];
        com_pos(:,:,2) = [1/2;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]);
        % 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*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);
            robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia-...
                link_mass(i)*com_vec2mat*com_vec2mat);
            robot.pi(:,i) = [robot.I_vec(:,i); robot.mc(:,i); robot.m(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;