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

조회 수: 18 (최근 30일)
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

답변 (1개)

Debadipto
Debadipto 2024년 2월 1일
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!

카테고리

Help CenterFile Exchange에서 Robotics에 대해 자세히 알아보기

제품


릴리스

R2021b

Community Treasure Hunt

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

Start Hunting!

Translated by