Recreating z trajectory from paper

[Minimartian]

Hi, I am trying to recreate the 6.2 trajectory example from your paper using this repo but I am not getting the same joint angles. Perhaps you could help? I have GC = 3, arm angle = 58.5882, K = 0.1, α = 5, and the robot moves along z correctly from the start pose, to the desired pose but the result of the inverse kinematics seems off (I have attached the output). Essentially i have attempted to implement the control scheme in Fig 10:

jl = deg2rad([170 120 170 120 170 120 170]);
rconf = 3;
psi = deg2rad(58.5882);
k = 0.1;
a2 = 5;
i = 1;

joints_current = deg2rad([-5.4101 -26.4986 -48.1542 -61.6500 152.6198 114.4466 8.1812]);
[ pose_desired, ~, ~, ~] = ForwardKinematics( joints_current );

step = 100;
t = eye(4);
t(:,4) = [0, 0, -(0.25/ (step-1)), 0]';
linear_movement = (pose_desired*t);


while(i < step)

    %Compute intervals using desired pose
    [ ~, s_mat, w_mat ] = InverseKinematics( pose_desired, psi, rconf );
    allow = PsiLimits(rconf, s_mat, w_mat, jl);

    for j=1:length(allow)/2

        inf = allow(j*2-1); %Lower limit of the feasible interval
        sup = allow(j*2); %Upper limit of the feasible interval

        if(psi>=inf && psi<=sup)
            current_psi = psi + (k*((sup-inf)/2)) * (exp(-a2*((psi-inf)/(sup-inf))) - (exp(-a2*((sup-psi)/(sup-inf)))));
            break;
        end

    end

    [ joints_current, ~, ~] = InverseKinematics( pose_desired, current_psi, rconf );

    psi = current_psi; 
    pose_current = pose_desired;
    pose_desired(:,4) = pose_current(:,4) - linear_movement(:,4);

    i = i + 1;

pt). Thank you.