IRDYn/complie/R1000 DVT GravityModel V1/get_Kinematics_R1000_DVT.m

function robot = get_Kinematics_R1000_DVT(robot, opt)
if(opt.Isreal)
    switch opt.KM_method
        case 'SCREW'
            % Get transformation for each joint
            thetalist = robot.theta;
            Slist = robot.slist;
            % TODO:move to lib
            for j = 1:length(thetalist)
                T=robot.Home.M(:,:,j);
                for i = j: -1: 1
                    T = MatrixExp6(VecTose3(Slist(:, i) * thetalist(i))) * T;
                end
                robot.kine.TW(:,:,j) = T;
            end
            for i = 1:length(thetalist)
                if i == 1
                    robot.kine.T(:,:,i) = robot.kine.TW(:,:,i);
                else
                    robot.kine.T(:,:,i) = TransInv(robot.kine.TW(:,:,i-1))*robot.kine.TW(:,:,i);
                end
            end
            %stack result into kine structure
            robot.kine.R = robot.kine.T(1:3,1:3,:);
            robot.kine.t = robot.kine.T(1:3,4,:);
            robot.kine.Fkine = robot.kine.TW(:,:,end);
            % get the CG at the world base frame
            % FIXME: Fix this hack 
            ct=zeros(3,robot.ndof);
            Mlist_CG_Base=zeros(4,4,10);
            com_pos_R1 = robot.com_pos_R1;
            com_pos_R2 = robot.com_pos_R2;
            for i = 1:length(thetalist)
                if i == 1
                    ct(:,i) = com_pos_R1(:,i);
                elseif i< 9
                    ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
                else
                    % HACK
                    ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.23;0;0.05896]+com_pos_R1(:,i);
                end
                robot.Home.com(:,i) = ct(:,i);
                M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
                Mlist_CG_Base(:,:,i) = M_CG_Base;
            end
            robot.kine.Mlist_CG_Base = Mlist_CG_Base;
            % get the CG at the last GC frame
            Mlist_CG = zeros(4,4,9);
            for i = 1:length(thetalist)
                if i == 1
                    Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
                else
                    Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
                end
            end
            M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
            Mlist_CG = cat(3, Mlist_CG, M);
            robot.kine.Mlist_CG = Mlist_CG;

            % Get the end efforce transformation in each joint reference frame
            Mlist_ED = zeros(4,4,9);
            for i = 1:length(thetalist)
                Mlist_ED(:,:,i) = robot.kine.T(:,:,i);
            end
            M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
            Mlist_ED = cat(3, Mlist_ED, M);
            robot.kine.Mlist_ED = Mlist_ED;
        otherwise
            disp('Bad opt.KM_method!')
            return;
    end
end