When calculating rigid body center Jacobian matrix, if ‘mdh’ rule is used to establish rigid body tree, ‘centerOfMass’ can calculate the correct centroid position and centroid Jacobian matrix. But if ‘dh’ rule is used to establish rigid body tree, ‘centerOfMass’ can calculate the correct centroid position, but the Jacobian matrix is not correct.
For example:
m1 = 1.0;
L1 = 1;
link2m_mass = m1;
link2m_CenterOfMass = [-0.5 0 0];
link2m = rigidBodyTree;
dhparams = [L1 0 0 0];
linkbody(1) = rigidBody("link2m"+1);
link2mJnt(1) = rigidBodyJoint("link2mjnt"+1, 'revolute');
setFixedTransform(link2mJnt(1),dhparams(1,:),'dh');
linkbody(1).Joint = link2mJnt(1);
linkbody(1).CenterOfMass = link2m_CenterOfMass(1,:);
linkbody(1).Mass = link2m_mass(1,1);
addBody(link2m,linkbody(1),"base");
link2m.DataFormat = 'row';
q = [0];
[CoM,CoMJ] = centerOfMass(link2m,q);
The result shows that CoM = [0.5,0,0] '; CoMJ = [0, - 0.5,0]', CoM is correct, but CoMJ is wrong, CoMJ should be [0,0.5,0] '.
But if we use the 'mdh' rule
m1 = 1.0;
L1 = 1;
link2m_mass = m1;
link2m_CenterOfMass = [-0.5 0 0];
link2m = rigidBodyTree;
dhparams = [L1 0 0 0];
linkbody(1) = rigidBody("link2m"+1);
link2mJnt(1) = rigidBodyJoint("link2mjnt"+1, 'revolute');
setFixedTransform(link2mJnt(1),dhparams(1,:),'mdh');
linkbody(1).Joint = link2mJnt(1);
linkbody(1).CenterOfMass = link2m_CenterOfMass(1,:);
linkbody(1).Mass = link2m_mass(1,1);
addBody(link2m,linkbody(1),"base");
link2m.DataFormat = 'row';
q = [0];
[CoM,CoMJ] = centerOfMass(link2m,q);
The result shows that CoM = [0.5,0,0] '; CoMJ = [0, - 0.5,0]', CoM and CoMJ are all correct.
I don't know why. By looking at the prototype of the centerOfMass , it is found that CoMJ is the last three lines of cmm divided by the total mass, and cmm is obtained by centroidalMomentumMatrix, in which the compositeRigidBodyInertia is used.
