MATLAB: The result of Robotic System Toolbox are different with RTB(Robotics Toolbox for MALTAB)

Question

I built a simple two DoF robot with Robotic System Toolbox and set the transform matrix using DH parameters. But the result has a big difference with result of RTB toolbox (developed by Peter Corke).

When I use Modified DH parameters, the results are the same. Does anybody know why? Is something wrong with my code?

The Two DOF robot is

[m1,m2] = deal(20,20);  %Mass of links
[l1,l2] = deal(0.5,0.5);  %Length of links
lc1 = l1/2;             %Center of Gravity
lc2 = l2/2;
I1 = 1;     %inertia
I2 = 1;
%%  this section is a robot model built by RTB(Robotics Toolbox for MALTAB) using DH parameters
L(1) = Revolute('d', 0, 'a', l1, 'alpha', 0, 'I',[0;0;I1;0;0;0], 'r', [-lc1; 0; 0], 'm', m1);   %the inertia is relative to COG
L(2) = Revolute('d', 0, 'a', l2, 'alpha', 0, 'I',[0;0;I2;0;0;0], 'r', [-lc2; 0; 0], 'm', m2);
two_Link= SerialLink(L, 'name', 'Puma 560');
two_Link.gravity = [0,0,0];
two_Link.nofriction;
%% this section is a robot model built by robotic system toolbox
dhparams = [l1, 0, 0, 0;
            l2, 0, 0, 0];
Two_rigid = robotics.RigidBodyTree;
Two_rigid.DataFormat = 'column';
link1 = robotics.RigidBody('link1');
link2 = robotics.RigidBody('link2');  
jnt1 = robotics.Joint('joint1','revolute');
jnt2 = robotics.Joint('joint2','revolute');   
setFixedTransform(jnt1,dhparams(1,:),'dh');
setFixedTransform(jnt2,dhparams(2,:),'dh'); 
link1.Joint = jnt1;
link2.Joint = jnt2;
link1.Mass = m1;
link1.Inertia = [0; m1*lc1^2; I1+m1*lc1^2; 0;0;0];      % the inertia is change to body frame
link1.CenterOfMass = [-lc1; 0; 0];
link2.Mass = m2;
link2.Inertia = [0; m2*lc2^2; I2+m2*lc2^2; 0;0;0];
link2.CenterOfMass = [-lc2; 0; 0];
addBody(Two_rigid,link1,'base');   
addBody(Two_rigid,link2,'link1');
%% results of two toolboxes
q1 = pi/6;
q2 = pi/12;
q = [q1;q2];
qd = [0.1;0.1];
C_L = coriolis(two_Link, q', qd');
M_L = inertia(two_Link,q');     %% mass matrix computed by RTB
M_r = massMatrix(Two_rigid,q);  %% mass matrix computed by RST

The results are

Obviously, the results has a big difference.

Answer 1

Hi, Xiyong,

Thanks for posting this question. What you described is indeed a bug in the internal code for computing dynamics quantities ONLY for robots specified with DH parameters. The issue, however, does NOT happen for robots specified with MDH parameters or robots specified directly with homogeneous transformation matrices. Unlike MDH or Tform robots, for DH robots, the joint frame and body frame do not collocate in general, so an additional spatial transformation is needed when propagating velocity/acceleration related quantities during the dynamics calculation, and that is currently missing.

The development team is currently working on the official fix, and you should check back on this post later for updates. If you need the fix sooner to unblock your workflow, please do not hesitate to reach out to the MATLAB tech support team to start a new case (remember to mention this post!), and we can get you a hot fix based on the MATLAB version you have.

Thanks,

Yiping