function robot = get_velocity(robot, opt)
switch opt.KM_method
    case 'SDH'
    case 'MDH'
        switch opt.Vel_method
            case 'Direct'
                Z = [0,0,1]';
                w0 = zeros(3,1); dw0 = zeros(3,1);dv0 = robot.gravity;
                % w = zeros(3,number);
                % dw = zeros(3,number);
                % dv = zeros(3,number);
                theta  = robot.theta;
                dtheta = robot.dtheta;
                ddtheta = robot.ddtheta;
                R = robot.R;
                P = robot.t;
                % 1-n外推公式
                %第一关节
                w(:,1) = R(:,:,1)' * w0 + dtheta(1) * Z;
                dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, dtheta(1) * Z) + ddtheta(1) * Z;
                dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0,cross(w0, P(:,1))) + dv0);
                %后面n-1关节
                for i = 1:robot.ndof
                    w(:,i+1) = R(:,:,i+1)' * w(:,i) + dtheta(i+1) * Z ;
                    dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), dtheta(i+1) * Z)+ ddtheta(i+1) * Z;
                    dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i));
                end
                robot.vel.w = w;
                robot.vel.dw = dw;
                robot.vel.dv = dv;
            otherwise
                disp('Bad opt.Vel_method!')
                return;
        end
    otherwise
        disp('Bad opt.KM_method!')
        return;
end