IRDYn/get_velocity.m

function robot = get_velocity(robot, opt)
switch opt.KM_method
    case 'SCREW'
        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=[];
        for i = 1:robot.ndof
            Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
            Glist = cat(3, Glist, Gb);
            % ?
%             mc = robot.TW(1:3,1:3,i)*robot.m(i)*robot.com(:,i);
%             robot.pi(2:4,i) = mc;
        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,:);
        robot.vel.v = V(4:6,:);
        robot.vel.dw = Vd(1:3,:);
%         robot.vel.dv = Vd(4:6,:);
        robot.vel.dv=[zeros(2,robot.ndof);-robot.gravity(end)*ones(1,robot.ndof)];
    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;
                v0 = zeros(3,1);dv0 = zeros(3,1);
                link_type = robot.link_type;
                % init q
                q = robot.theta;
                qd = robot.dtheta;
                qdd = robot.ddtheta;   
                for i = 1:robot.ndof
                    switch link_type(i)
                        case 'R'
                            %Do nothing
                        case 'P'
                            q(i) = robot.d(i);
                            qd(i) = robot.vd(i);
                            qdd(i) = robot.accd(i);
                    end
                end
                R = robot.R;
                P = robot.t;
                % 1-n外推公式 参考robotics toolbox
                %第一关节
                switch link_type(1)
                    case 'R' %revolute
                        w(:,1) = R(:,:,1)' * w0 + qd(1) * Z;
                        v(:,1) = R(:,:,1)' * v0 + cross(w0,P(:,1));
                        dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, qd(1) * Z) + qdd(1) * Z;
                        dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0, cross(w0, P(:,1))) + dv0);
                    case 'P' %prismatic
                        w(:,1) = R(:,:,1)' * w0;
                        v(:,1) = R(:,:,1)' * (Z*qd(1)+v0) + cross(w0, P(:,1));
                        dw(:,1) = R(:,:,1)' * dw0;
                        dv(:,1) = R(:,:,1)' * (cross(dw0, P(:,1)) + cross(w0, cross(w0, P(:,1))) + dv0)+...
                            2*cross(R(:,:,1)' * w0, Z * qd(1)) + Z * qdd(1);
                end
                %后面n-1关节
                for i = 1:robot.ndof
                    switch link_type(i)
                        case 'R' %revolute
                            w(:,i+1) = R(:,:,i+1)' * w(:,i) + qd(i+1) * Z ;
                            v(:,i+1) = R(:,:,i+1)' * v(:,i) + cross(w(:,i), P(:,i+1));
                            dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), qd(i+1) * Z)+ qdd(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));
                        case 'P' %prismatic
                            w(:,i+1) = R(:,:,i+1)' * w(:,i);
                            v(:,i+1) = R(:,:,i+1)' * (Z*qd(:,i)+v(:,i)) + cross(w(:,i), P(:,i+1));
                            dw(:,i+1) = R(:,:,i+1)' * dw(:,i);
                            dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i))+...
                                2*cross(R(:,:,i+1)' * w(:,i), Z * qd(:,i)) + Z * qdd(:,i);
                    end
                end
                robot.vel.w = w;
                robot.vel.v = v;
                robot.vel.dw = dw;
                robot.vel.dv = dv;
            otherwise
                disp('Bad opt.Vel_method!')
                return;
        end
    otherwise
        disp('Bad opt.KM_method!')
        return;
end