Why the result of Robotic System Toolbox are different with RTB(Robotics Toolbox for MALTAB)?

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.

[Walter Roberson]

my understanding is that they use different coordinate systems.

[siyong xu]

Thanks for your comment.

Only the 'inertia' property are relative to different coordinate systems. RTB is relative to center of gravity, while the RST is relative to body frame. And I have unified the cooridinate sytems in my code.

[Karsh Tharyani]

Thank you for your question. In order to aid you better in diagnosing this issue, I recommend that you please reach out to Technical Support https://www.mathworks.com/support/contact_us.html with a reproducible (you can use the one in this post) of your issue. The appropriate team of this feature will be able to answer your query.

Answer 1

[Yiping Liu]

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

[siyong xu]

Hi Yiping.

Thanks for your answer.

Recently, I am work on a algorithm that computes the derivatives of joint torque (that is, high order inverse dynamics) based on the Robotics System Toolbox. And I refer to the code of inversedynamics function.

It looks like that I found the issue that you said: Line 164 of RigidbodyTreeDynamics.m file

vB(:,i) = vJ(:,i) + X{i}*vB(:,pid);

Is the vJ(:,i)=S*qdoti should be transformed to body i frame, just like this?

Tdh = robot.Bodies{i}.Joint.ChildToJointTransform;
Tdhinv = robotics.manip.internal.tforminv(Tdh);
X{i} = robotics.manip.internal.tformToSpatialXform(Tdhinv)*vJ(:,i)+X{i}*vB(:,pid);

And the condition' parent is base' and the acceleration computeing are same as above.

Do I understand correctly?

[Yiping Liu]

I think the idea you are getting at is correct, but the source code will actually need a couple of updates in different locations.

[siyong xu]

Okay,Thank you very much.