Robotics System Toolbox, massMatrix() keeps returning a row of zeros
1 view (last 30 days)
Show older comments
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);
0 Comments
Answers (1)
Abhijeet
on 25 Oct 2023
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.
0 Comments
See Also
Categories
Find more on Code Generation in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!