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

[Jin Siang]

Hi 재욱 임 ,

First of all, you did some error at the inertia tensor section. From what I have tested, when you use the "loadrobot()" command, it actually automatically convert the inertia with respect to body frame, so you do not need to convert again and can straight use the moment of inertia directly. However, there is still disecrepancy which until now I have no idea why is it happening. There might be something that is missing, such as additional transformation at the jacobian (I am suspecting the jacobian should be respect to center of link and not geometric jacobian) or rotation matrix.. Hmmm..

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!

[Jin Siang]

Hi @Debadipto,

The "loadrobot()" command actually automatically convert the inertia with respect to body frame. However, there is still disecrepancy which until now I have no idea why is it happening. There might be something that is missing, such as additional transformation at the jacobian (I am suspecting the jacobian should be respect to center of link and not geometric jacobian) or rotation matrix.... Do you have any idea? I am using the Euler-Lagrange formulation as mentioned above of this topic.