Robotics System Toolbox, massMatrix() keeps returning a row of zeros

I have been trying to create a floating base version of the valkyrie robot, everything seems to be in order in the rigid body tree variable but when I try to run massMatrix(robot) it just returns a bunch of zeros.

I have attached the code below, any help would be appreciated.

valkyrie = loadrobot('valkyrie')


%% 

robot = rigidBodyTree;
robot.BaseName='World';
baseName = robot.BaseName;

% Virtual Joints
% ********************** Y axis translation********************************
body1 = rigidBody('y axis translation');
body1.Mass=0;body1.Inertia=[0,0,0,0,0,0];
addBody(robot,body1,baseName);
from0to1transform = makehgtform('yrotate',-pi/2)*makehgtform('xrotate',-pi/2);
jnt1 = rigidBodyJoint('y axis translation','prismatic');
jnt1.PositionLimits = [-100 100];
setFixedTransform(jnt1,from0to1transform);
replaceJoint(robot,'y axis translation',jnt1);
% ********************** X axis translation********************************
body2 = rigidBody('x axis translation');
body2.Mass=0;body2.Inertia=[0,0,0,0,0,0];
addBody(robot,body2,'y axis translation');
from1to2transform = makehgtform('xrotate',-pi/2)*makehgtform('zrotate',-pi/2);
jnt2 = rigidBodyJoint('x axis translation','prismatic');
jnt2.PositionLimits = [-100 100];
setFixedTransform(jnt2,from1to2transform);
replaceJoint(robot,'x axis translation',jnt2);
% ********************** Z axis translation********************************
body3 = rigidBody('z axis translation');
body3.Mass=0;body3.Inertia=[0,0,0,0,0,0];
addBody(robot,body3,'x axis translation');
from2to3transform = makehgtform('xrotate',-pi/2)*makehgtform('zrotate',-pi/2);
jnt3 = rigidBodyJoint('z axis translation','prismatic');
jnt3.PositionLimits = [-1000 1000];
setFixedTransform(jnt3,from2to3transform);
replaceJoint(robot,'z axis translation',jnt3);
% ********************** Z axis rotation********************************
body4 = rigidBody('z axis rotation');
body4.Mass=0;body4.Inertia=[0,0,0,0,0,0];
addBody(robot,body4,'z axis translation');
from3to4transform = makehgtform('xrotate',0)*makehgtform('zrotate',0);
jnt4 = rigidBodyJoint('z axis rotation','revolute');
jnt4.PositionLimits = [-100 100];
setFixedTransform(jnt4,from3to4transform);
replaceJoint(robot,'z axis rotation',jnt4);
% ********************** Y axis rotation********************************
body5 = rigidBody('y axis rotation');
body5.Mass=0;body5.Inertia=[0,0,0,0,0,0];
addBody(robot,body5,'z axis rotation');
from4to5transform = makehgtform('yrotate',pi/2)*makehgtform('xrotate',-pi/2);
jnt5 = rigidBodyJoint('y axis rotation','revolute');
jnt5.PositionLimits = [-100 100];
setFixedTransform(jnt5,from4to5transform);
replaceJoint(robot,'y axis rotation',jnt5);
%% Pelvis and Torso
% ******************* X axis rotation and Pelvis***********************
body6 = rigidBody('pelvis');
body6.Mass=8.220;
body6.CenterOfMass=[-0.003512000000000,-0.003600000000000,-0.005320000000000];
body6.Inertia=[0.098302601928000,0.084188670391680,0.118871697863680,0.003113863560000,-2.970631648000110e-04,0.002055617896000];
addCollision(body6,"mesh")
addVisual(body6,"Mesh",'pelvis.dae');
addBody(robot,body6,'y axis rotation');
from5to6transform = makehgtform('yrotate',-pi/2)*makehgtform('zrotate',-pi/2);
jnt6 = rigidBodyJoint('x axis rotation','revolute');
jnt6.PositionLimits = [-100 100];
jnt6.JointAxis=[1,0,0];
setFixedTransform(jnt6,from5to6transform);
replaceJoint(robot,'pelvis',jnt6);

Answer 1

[Abhijeet]

Hi Deepak,

I understand that you are trying to use ‘massMatrix’ function, but you are getting unexpected output.

It looks like the value is provided to ‘inertial parameter’ to each body. Usually value for ‘inertial parameter’ is received from CAD software so that ‘composite rigid body’ algorithm can work properly.

I suggest you to update the value of ‘inertial parameter’ as per the code mentioned below: -

body1.Inertia = [1e-6 0.25 0.25 0 0 0]; 

I hope this resolves the issue you were facing.