28 lines
1.4 KiB
Matlab
28 lines
1.4 KiB
Matlab
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]; |