Feedforward + PI Control Algorithm Yields Different Results

Hello there, hope you are all doing well.

I wrote a couple of days ago in order to let you know about a program of mine not working as expected. I just found a chance to try what you all said and I am suspecting about a bug in my FeedForward + PI Control Algorithm. Before giving out the code I want to tell you exactly what my code is expected to accomplish.

This the structure of the function:

[Vd, V, Je, u_theta_dot, Xerr, Xerr_integral] = FeedbackControl(X, Xd, Xd_next, Kp, Ki, delta_t, thetaList, Xerr_integral)

Inputs;

1. X -> Actual End-Effector Configuration Tse,
2. Xd -> Desired/Reference End-Effector Configuration Tse_d,
3. Xd_next -> Desired/Reference End-Effector Configuration at the next
4. time step, Tse_d1,
5. Kp and Ki-> Gains
6. delta_t -> Timestep between configurations,
7. thetaList -> Current actual joint angles,
8. Xerr_integral -> Used in order to update the integral of Xerr.

Outputs;

1. Vd -> Resulting Feed-forward twist
2. V -> Commanded End-Effector twist
3. Je -> Jbase(theta) and Jarm(theta) concatanated
4. u_theta_dot -> controls vector (u and theta_dot)
5. Xerr -> Configuration Error
6. Xerr_integral -> Used to pass as a parameter in the next_step

Here are the required Math Equations:

[Xerr] = log(inverse(X) * Xd) -> This needs to be converted to a vector.

Quick Note: My function uses some functions from Modern Robotics Library, which can be found (on Github) here.

With all of these, here is my function implementation:

function [Vd, V, Je, u_theta_dot, Xerr, Xerr_integral] = FeedbackControl(X, Xd, Xd_next, Kp, Ki, delta_t, thetaList, Xerr_integral)

%% Arm properties.
Blist = [0 0 1 0 0.033 0; 0 -1 0 -0.5076 0 0; 0 -1 0 -0.3526 0 0; 0 -1 0 -0.2176 0 0; 0 0 1 0 0 0]';
l = 0.47/2;
w = 0.30/2;
r = 0.0475;
F = (r/4) * [-1/(l + w), 1/(l + w), 1/(l + w), -1/(l + w); 1 1 1 1; -1 1 -1 1];
sizee = size(F);
m = sizee(2);
zeross = zeros(1, m);
F6 = [zeross; zeross; F; zeross];

Tb0 = [1 0 0 0.1662; 0 1 0 0; 0 0 1 0.0026; 0 0 0 1];
M0e = [1 0 0 0.033; 0 1 0 0; 0 0 1 0.6546; 0 0 0 1];
T0e = FKinBody(M0e, Blist, thetaList);
Tbe = Tb0 * T0e;
Teb = TransInv(Tbe);
Jbase = Adjoint(Teb) * F6;
Jarm = JacobianBody(Blist, thetaList);
Je = [Jbase, Jarm];
psInv = pinv(Je);

Xerr_bracket = MatrixLog6(TransInv(X) * Xd);
Xerr = se3ToVec(Xerr_bracket);
Xerr_integral = Xerr_integral + (delta_t * Xerr);
Xerr_integral = round(Xerr_integral, 3);
Vd_bracket = (1/delta_t) * MatrixLog6(TransInv(Xd) * Xd_next);
Vd = se3ToVec(Vd_bracket);

V = (Adjoint(TransInv(X) * Xd) * Vd) + (Kp * Xerr) + Ki * Xerr_integral;


u_theta_dot = psInv * V;
u_theta_dot = round(u_theta_dot, 3);
Xerr = round(Xerr, 3);
V = round(V, 3);
Vd = round(Vd, 3);
Je = round(Je, 3);

end

Last couple of lines are used to round the results to 3 decimals places.

When I test my function with pre-calculated parameters, these are the expected results:

Here is my test script:

clc
clear

Blist = [0 0 1 0 0.033 0; 0 -1 0 -0.5076 0 0; 0 -1 0 -0.3526 0 0; 0 -1 0 -0.2176 0 0; 0 0 1 0 0 0]';
Tb0 = [1 0 0 0.1662; 0 1 0 0; 0 0 1 0.0026; 0 0 0 1];
M = [1 0 0 0.033; 0 1 0 0; 0 0 1 0.6546; 0 0 0 1];
thetaList = [0 0 0.2 -1.6 0]';
Xd = [0 0 1 0.5; 0 1 0 0; -1 0 0 0.5; 0 0 0 1];
Xd_next = [0 0 1 0.6; 0 1 0 0; -1 0 0 0.3; 0 0 0 1];
X = [0.170 0 0.985 0.387; 0 1 0 0; -0.985 0 0.170 0.570; 0 0 0 1];

Xerr_bracket = MatrixLog6(TransInv(X) * Xd);
Xerr = se3ToVec(Xerr_bracket);

Kp = 0 * eye(6);
Ki = 0 * eye(6);

delta_t = 0.01;

[Vd, V, Je, u_theta_dot, Xerr, Xerr_integral] = FeedbackControl(X, Xd, Xd_next, Kp, Ki, delta_t, thetaList, Xerr)

My Je, and Vd outputs are exactly right. However, the other outputs have some decimal errors. The differences do not have anything to do with the fact that I rounded the numbers up. In fact, the expected results were rounded up and that was the reason I did too, in the first place. I do not think that the expected results are incorrect, so that is why I am suspicious of my function implementation. Can you help me with that?