petercorke / robotics-toolbox-matlab

Robotics Toolbox for MATLAB
GNU Lesser General Public License v2.1
1.26k stars 440 forks source link

Panda's DH parameter of Link 7 is argubly wrong #129

Open Zheng-JIA opened 1 year ago

Zheng-JIA commented 1 year ago

According to https://frankaemika.github.io/docs/control_parameters.html, the link 7's parameter "d" should be zero, which was however defined to be 0.107 in mdl_panda(). In the following, I'm writing to verify the correctness of mdl_panda().

In mdl_panda().m, the robot is created using the function revolutemdh(). After reading rne_mdh.m, I understand that the "d" parameter of link $i$ is used for defining the transformation matrix

${}^{i-1}{T}_i$

between frame $i-1$ and $i$. For example, "L1" in mdl_panda() gives us ${}^{0}T_1$, consistent with numbering of links in https://frankaemika.github.io/docs/control_parameters.html. But "L7" however is not consistent as "d" is 0.107. I wasn't sure if anywhere I didn't understand correctly using the function revolutemdh and I was wondering if the inertia matrix and center of mass parameters were referred to $i-1$ or $i$-frame when using the function revolutemdh, since in the description of Link.m, it says in line 673, "T=L.A(Q)" tells the transformation from current frame to the next frame. I was confused current frame with $i$ and next frame with $i+1$ for a while until I found that this line of description of current and next frames are not relevant at all to the frame numbering...

Anyway, I found that the inertia matrix and center of gravity should be referred in the frame $i$ because:

Line 134 of rne_mdh.m implied that the angular velocity w was calculated and referred in the frame $i$.

w = ${}^{i}\omega_i$

and from line 154 to 157, the center of mass and inertia matrix were used to operate with this ${}^{i}\omega_i$. It only makes sense two vectors referred in the same frame performing the cross product and so on.

So if Link7, or L7, is created with d=0.107, then it means it defines a wrong transformation matrix

${}^{6}{T}_7$

and a wrong frame 7. Since the center of gravity and inertia matrix were referred to frame 7, it gives wrong results.

I verified this by writing a simple matlab file, just comparing the joint torque given random q, qd, qdd with https://github.com/marcocognetti/FrankaEmikaPandaDynModel/tree/master/matlab/dyn_model_panda, with d=0 and d=0.107. When d=0, there was less error.

qlims = [[-2.8973 2.8973];
    [-1.7628 1.7628];
    [-2.8973 2.8973];
    [-3.0718 -0.0698];
    [-2.8973 2.8973];
    [-0.0175 3.7525];
    [-2.8973 2.8973]];
mdl_panda % modify d=0 or d=0.107 in L7
error = zeros(1,100);
for i = 1:100
    q = qlims(:,1) + (qlims(:,2) - qlims(:,1)).*rand(7,1);
    dq = -0.5 + 1*rand(7,1);
    ddq = -2 + 4*rand(7,1);
    temp = get_MassMatrix(q)*ddq + ...
            get_CoriolisVector(q, dq) + ...
            get_GravityVector(q);
    tau_1(:, i) = temp.';
    tau_2(:, i) = panda.rne(q',dq',ddq');
    error(i) = norm(temp.' - panda.rne(q',dq',ddq'));
end
figure
plot(tau_1(1,:))
hold on
plot(tau_2(1,:))
figure
plot(error)