function [massMatrix,totalMass,gravityForce] = calculateMassMatrix(dynamicBasicInfo,...
                                                            toolMass,jacoInfo)
                                                        
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 计算机器人系统的质量矩阵，
% dynamicBasicInfo: 机械臂的基本信息
% jacoInfo: 雅可比矩阵
% toolMass: 器械的质量
%
% author:li.jiang
% e-mail:li.jiang@csrbtx.com
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
g = -9.8;
massMatrix = zeros(9,9); % init massMatrix to be zeros
totalMass = 0;
gravityForce = zeros(9,1);
for i=1:9-1
    tempMatrix = [dynamicBasicInfo.link(i).mass*eye(3) zeros(3,3);
                  zeros(3,3)   dynamicBasicInfo.link(i).inertialMatrix];
    massMatrix = massMatrix + jacoInfo.link(i).jaco'*tempMatrix*jacoInfo.link(i).jaco;
    totalMass = totalMass+dynamicBasicInfo.link(i).mass;
    gravityForce = gravityForce + jacoInfo.link(i).jaco'*[0;0;g*dynamicBasicInfo.link(i).mass;0;0;0];
end
tempMatrix = [(dynamicBasicInfo.link(9).mass+toolMass)*eye(3) zeros(3,3);
              zeros(3,3)   dynamicBasicInfo.link(9).inertialMatrix];
massMatrix = massMatrix + jacoInfo.link(9).jaco'*tempMatrix*jacoInfo.link(9).jaco;
totalMass = totalMass+dynamicBasicInfo.link(9).mass+toolMass;
gravityForce = gravityForce + jacoInfo.link(9).jaco'*[0;0;g*(dynamicBasicInfo.link(9).mass+toolMass);0;0;0];