function robot = get_velocity(robot, opt)
switch opt.KM_method
    case 'SCREW'
        % Def code
        Mlist_CG = robot.kine.Mlist_CG;
        % Get general mass matrix
        link_mass = robot.m;
        com_pos = robot.com;
        link_inertia = robot.I;
        Slist=robot.slist;
        Glist=zeros(6,6,9);
        for i = 1:robot.ndof
            Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
            Glist(:,:,i) = Gb;
            % ?
            %             mc = robot.TW(1:3,1:3,i)*robot.m(i)*robot.com(:,i);
            %             robot.pi(2:4,i) = mc;
            % hack: because we only know the com in the world frame
            % the inv of rotation is beacause we want to the com in
            % our defined frame. R*com is the wrong result
            robot.pi(2:4,i) = robot.m(i)*robot.Home.R(:,:,i)'*robot.com(:,i);
            com_vec2mat = vec2skewSymMat(robot.com(:,i)); %com
            % get joint inertial
            robot.I(:,:,i) = robot.I(:,:,i)-link_mass(i)*com_vec2mat*com_vec2mat;
            robot.I(:,:,i) = robot.Home.R(:,:,i)'*robot.I(:,:,i)*robot.Home.R(:,:,i);
            robot.I_vec(:,i) = inertiaMatrix2Vector(robot.I(:,:,i));
            robot.pi(5:end,i) = robot.I_vec(:,i);
        end
        if opt.Isreal
            % init q
            q = robot.theta;
            qd = robot.dtheta;
            qdd = robot.ddtheta;
            [V,Vd,~,~,~] = InverseDynamics_debug(q, qd, qdd, ...
                robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
        else
            % init q
            q = robot.theta;
            qd = robot.dtheta;
            qdd = robot.ddtheta;
            [V,Vd,~,~,~] = InverseDynamics_sym(q, qd, qdd, ...
                robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
        end
        % FIXME: twist is not equal to velocity
        % FIXME: Need to get the velocity represent at base frame
        robot.vel.w = V(1:3,2:end);
        robot.vel.dw = Vd(1:3,2:end);
        % We need the vel at COM but we want to know vel at joint origin
        for i = 1:robot.ndof
            robot.vel.v(:,i) = vec2skewSymMat(robot.Home.R(:,:,i)'*robot.com(:,i))*robot.vel.w(:,i)+V(4:6,i+1);
            robot.vel.dv(:,i) = vec2skewSymMat(robot.Home.R(:,:,i)'*robot.com(:,i))*robot.vel.dw(:,i)+Vd(4:6,i+1);
        end
    otherwise
        disp('Bad opt.KM_method!')
        return;
end