function [jacoInfo] = calculateJacobian(robotRigidInfo,...
                                        rotInfo,...
                                        theta)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 计算机器人各个轴臂的雅可比矩阵，
% dynamicBasicInfo: 机械臂的基本信息
% rotInfo: 各个轴臂的姿态信息
% theta: 转角
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
temp = struct;
temp.jaco = zeros(6,9);
jacoInfo = struct;
jacoInfo.link  = ([temp temp temp temp temp temp temp temp temp]);
jacoInfo.motor = ([temp temp temp temp temp temp temp temp temp]);
for i=1:9
    jacoInfo.link(i).jaco            = zeros(6,9);
    jacoInfo.motor(i).jaco           = zeros(6,9);
    comToRotCenter =  -rotInfo.link(i).rot*robotRigidInfo.link(i).R1;
    for j=i:-1:1
        if robotRigidInfo.link(j).jointAxis>3
            rotAxis=zeros(3,1);
            rotAxis(robotRigidInfo.link(j).jointAxis-3) = 1;
            % movement
            jacoInfo.link(i).jaco(1:3,j) = -vector2so3(comToRotCenter)*rotInfo.link(j).rot*rotAxis;
            if j>1
                comToRotCenter =  comToRotCenter + ...
                                  rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R2 ...
                                - rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R1;
            end
            % rotation
            jacoInfo.link(i).jaco(4:6,j) = rotInfo.link(i).rotTran*rotInfo.link(j).rot*rotAxis;
        else
            tranAxis=zeros(3,1);
            tranAxis(robotRigidInfo.link(j).jointAxis) = 1;
            % movement
            jacoInfo.link(i).jaco(1:3,j) = rotInfo.link(j).rot*tranAxis;
            if j>1
                comToRotCenter =  comToRotCenter + ...
                                  rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R2 ...
                                - rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R1 + ...
                                  rotInfo.link(j).rot*tranAxis*theta(j);  
            end
        end
        
    end
    motorRotAxis   = zeros(3,1);
    motorRotAxis(robotRigidInfo.motor(i).jointAxis-3) = 1;
    jacoInfo.motor(i).jaco = jacoInfo.link(i).jaco;
    jacoInfo.motor(i).jaco(4:6,i) = robotRigidInfo.motor(i).gearRatio*motorRotAxis; 
end