Anybody can check my code to build mass matrix of 6 DOF manipulator, especially UR10?

Hello,

I'm trying to build mass matrix for UR10 of Universal Robot.

I think you might know that the mass matrix M can be calculated using the equation below

• : mass of link
• : velocity Jacobian matrix ()
• : angular velocity Jacobian matrix ()
• : the inertia tensor of the collection of body elements, output as a 3-by-3 matrix relative to the centered frame. The centered frame is a whose axes are aligned with those of Base's Reference Frame (=world frame)

So I loaded UR10's urdf file using loadrobot

robot = loadrobot("universalUR10");
robot.DataFormat = 'column';

and using robotics system toolbox, I coded like this

% # of link
numOfLink = numel(robot.Bodies);

% mass of each link
linkMass = cellfun(@(link) link.Mass, robot.Bodies);

% LBF means Link Body Fixed Frame, which is link attached reference frame 
% LBF_centeredMOI is the inertia tensor of the collection of body elements, output as a 3-by-3 matrix 
% relative to the centered frame. The centered frame is a frame whose origin coincides with the center of mass and 
% whose axes are aligned with those of LBF
LBF_centeredMOI = cellfun(@buildLBFCenteredMOI, robot.Bodies, 'UniformOutput', false);

% isRevoluteJoint = cellfun(@checkIfRevoluteJoint, robot.Bodies);

jacobian = cell(1, numOfLink);
jacobianV = cell(1, numOfLink);
jacobianW = cell(1, numOfLink);

xForm_0 = cell(1, numOfLink);
M = cell(1, numOfLink);

config = robot.homeConfiguration;
massMatrix = zeros(6, 6);
for i=1:numOfLink

    jacobian{i} = robot.geometricJacobian(config, robot.BodyNames{i});
    jacobianW{i} = jacobian{i}(1:3, :);
    jacobianV{i} = jacobian{i}(4:6, :);

    xForm_0{i} = robot.getTransform(config, robot.BodyNames{i});

    R = xForm_0{i}(1:3, 1:3);
    M{i} = linkMass(i)*jacobianV{i}'*jacobianV{i} + jacobianW{i}'*R*LBF_centeredMOI{i}*R'*jacobianW{i};
     
    massMatrix = massMatrix + M{i};
end

massMatrix
robot.massMatrix(config)

------------------------------------------------------------------------------------------------------------

function LBF_centeredMOI = buildLBFCenteredMOI(link)
    
    LBF_CoMofLink = link.CenterOfMass;
    mass = link.Mass;
    
    inertiaArray = link.Inertia; % [Ixx Iyy Izz Iyz Ixz Ixy]
    LBF_MOI = diag(inertiaArray(1:3)) + [0, inertiaArray(6), inertiaArray(5)
                                           inertiaArray(6), 0, inertiaArray(4)
                                           inertiaArray(5), inertiaArray(4), 0];

    LBF_centeredMOI = LBF_MOI + mass*(LBF_CoMofLink*LBF_CoMofLink'*eye(3) - LBF_CoMofLink'*LBF_CoMofLink);
   
end

However, the result of {massMatrix} is different from {robot.massMatrix}

• massMatrix :

• robot.massMatrix

What do you think of the problem is?

Thank you

Answer 1

[Debadipto]

Hi @재욱 임,

The discrepancy in the values of massMatrix is most probably an expected difference. It arises from the fact that URDF specifies the inertia with respect to the Center of Mass, while the RigidBody object defines inertia with respect to the frame origin.

I would request you to go through the following MATLAB answer post for a detailed insight into the issue:

https://www.mathworks.com/support/search.html/answers/1602190-why-the-robot-bodies-inertia-is-different-from-urdf.html?fq%5B%5D=asset_type_name:answer&fq%5B%5D=category:robotics/manipulator-modeling&page=1

If the issue still persists or you have specific concerns regarding the results of the computation, please feel free to contact MathWorks Technical Support:

https://www.mathworks.com/support/contact_us.html?s_tid=hp_ff_s_support

Hope this helps!