fmincon Смещение одного выхода и минимизация целевой функции

У меня есть решатель обратной кинематики. Я хочу сместить значение угла одного сустава так, чтобы оно хотя бы приблизилось к определенному значению, при котором этот угол приведет к тому, что звено будет плоским или параллельным земле. Скрипт работает хорошо, но моя проблема в том, что fmincon останавливает итерацию, я думаю, как только достигается значение по умолчанию TolFun. Значение смещения составляет около ±0.17 радиан или ±10 градусов.

Я пытался использовать очень маленькое значение TolFun, например, 1e-12, но когда итерация сходится, угол, который я хочу сместить, все равно не достигает желаемого значения или не приближается к нему. Я пробовал добавлять пороговое значение fval и использовать цикл while, но, как мне кажется, критерий остановки TolFun все еще превосходит это, так как при запуске скрипта почти достигаются аналогичные сходимости. Мне нужна помощь в том, как реализовать смещение. Вот фрагмент кода:

function A = transformation_matrix(theta, alpha, a, d)
        A = [[cos(theta), -cos(alpha) * sin(theta), sin(alpha) * sin(theta), a * cos(theta)];
             [sin(theta), cos(alpha) * cos(theta), -sin(alpha) * cos(theta), a * sin(theta)];
             [0, sin(alpha), cos(alpha), d];
             [0, 0, 0, 1]];
    end

function position = forward_kinematics(joint_angles)
    theta2 = joint_angles(1);
    theta3 = joint_angles(2);
    theta4 = joint_angles(3);
    theta5 = joint_angles(4);
    theta6 = joint_angles(5);

    A1 = transformation_matrix(0, 0, 0, 148);
    A2 = transformation_matrix(theta2, pi/2, 0, 152);
    A3 = transformation_matrix(theta3, 0, 250, 0);
    A4 = transformation_matrix(theta4, 0, 160, 0);
    A5 = transformation_matrix(theta5 + pi/2, pi/2, 0, 0);
    A6 = transformation_matrix(theta6, 0, -75, 160);

    position_matrix = A1 * A2 * A3 * A4 * A5 * A6 * [0; 0; 0; 1];
    %threshold = 1e-8;  % Малый порог для округления до нуля
    %position_matrix(abs(position_matrix) < threshold) = 0;  % Установить маленькие значения в ноль
    position = position_matrix(1:3);
end

function error = objective_function(joint_angles, target, initial_guess, is_first_run)
    theta3 = joint_angles(2);
    theta4 = joint_angles(3);
    theta5 = -(theta3 + theta4);

    position = forward_kinematics(joint_angles);
    error = norm(position - target);
    bias_theta5 = abs(joint_angles(4) - theta5);
    disp('Смещение угла 5:');
    disp(bias_theta5);
    error += 1 * bias_theta5 % смещение для приближения угла 5 к значению, параллельному начальной координате Y
    if ~is_first_run
        error += 0.1 * norm(joint_angles - initial_guess);  % Штраф за отклонение от начального предположения
    end
end

function error = objective_function(joint_angles, target, initial_guess, is_first_run)
    theta3 = joint_angles(2);
    theta4 = joint_angles(3);
    theta5 = -(theta3 + theta4);

    position = forward_kinematics(joint_angles);
    error = norm(position - target);
    bias_theta5 = abs(joint_angles(4) - theta5);

    disp('Смещение угла 5:');
    disp(bias_theta5);

    error += 1 * bias_theta5;  % Смещение для приближения угла 5 к значению, параллельному начальной координате Y

    if ~is_first_run
        error += 0.1 * norm(joint_angles - initial_guess);  % Штраф за отклонение от начального предположения
    end
end

function [joint_angles, exitflag] = inverse_kinematics(target_position, initial_guess, is_first_run)
    options = optimset('Display', 'final', 'MaxIter', 2000);  % Пропускаем 'TolFun'

    % Ограничения для начальных значений первых четырех углов
    initial_lb = [-(5/6) * pi; -(1/6) * pi; -(11/18) * pi; -(1/2) * pi];
    initial_ub = [ (5/6) * pi;  (5/9) * pi;  0;           (1/2) * pi];

    if is_first_run
        lb = initial_lb;  % Используем начальные нижние границы
        ub = initial_ub;  % Используем начальные верхние границы
    else
        % Обновляем границы на основе конвергированных углов суставов
        lb = initial_lb - initial_guess(1:4);  % Вычитаем конвергированные значения
        ub = initial_ub - initial_guess(1:4);  % Вычитаем конвергированные значения
    end

    initial_guess_4 = initial_guess(1:4);
    %max_iter = 100;  % Устанавливаем максимальное количество итераций
    fval_threshold = 0.17;  % Критерий остановки для fval
    iter = 0;
    converged = false;

    while ~converged %&& iter < max_iter
        % Оптимизируем theta2, theta3, theta4
        [joint_angles_4, fval, exitflag] = fmincon(@(j) objective_function([j; 0], target_position, initial_guess, is_first_run), ...
                                                     initial_guess_4, [], [], [], [], lb, ub, [], options);

        % Проверяем критерий остановки по fval
        if fval < fval_threshold
            converged = true;  % Мы достигли желаемого fval
        end

        initial_guess_4 = joint_angles_4;  % Обновляем начальное предположение для следующей итерации
        %iter = iter + 1;
    end

    joint_angles = [joint_angles_4; 0];  % Устанавливаем theta 6 в ноль

    % Отображаем результат
    if converged
        fprintf('Сошлось с fval: %f\n', fval);
    else
        fprintf('Достигнуто максимальное количество итераций без достижения желаемого fval.\n');
    end
end