Computing the Jacobian matrix of robot centroid.

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.

Answer 1

[Yiping Liu]

Hi, Sheng,

Thanks for posting this question. What you described is indeed a bug in the internal code for computing centroidal momentum for robots specified with DH parameters (i.e. a missing spatial transformation on the joint's motion subspace, as for DH robots, unlike MDH robots and robots specified directly with homogeneous transformation matrices, the moving joint frame does NOT collocate with the body frame).

The development team is working on the official fix for this issue related to the DH robots. Meanwhile, if this is blocking your workflow, please reach out to the MathWorks tech support team to start a new tech support case (remember to mention this post), and we can then get a hot fix for you based on your MATLAB version.

Thanks,

Yiping