Effacer les filtres
Effacer les filtres

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

18 vues (au cours des 30 derniers jours)
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};
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);
However, the result of {massMatrix} is different from {robot.massMatrix}
  • massMatrix :
  • robot.massMatrix
What do you think of the problem is?
Thank you

Réponses (1)

Debadipto le 1 Fév 2024
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:
If the issue still persists or you have specific concerns regarding the results of the computation, please feel free to contact MathWorks Technical Support:
Hope this helps!


En savoir plus sur Robotics dans Help Center et File Exchange




Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by