Robotic Toolbox, how to reduce the error in pose estimation?

I'm trying to write a program to control a robot with 5 degrees of freedom, in particular I would like to make sure that the robot's end effector follows a certain trajectory maintaining a defined orientation, which changes from point to point of the trajectory. I'm trying to use the Robotic toolbox included in matlab, for the calculation of the inverse kinematics use the BFGSGradientProjection algorithm with the predefined parameters, however at the output I always get an estimate of the joint angles with a PoseErrorNorm around 0.5 and an ExitFlag = 2, which means that the algorithm has reached the maximum iteration limit. By increasing this limit, the algorithm returns ExitFlag = 3 indicating that the computation time has exceeded the limit. Using the other algorithm you still get the same results.

Can anyone give me some advice to improve the results? I include the code, the vector "newPoint" is a nx3 vector with the x y z coordinates of the point to be reached, the vector newNormal is a 3xn vector instead contains the versors that indicate the orientation of the end effector to be maintained during the path.

close all;
clc;

robot = rigidBodyTree('DataFormat','column','MaxNumBodies',6);

L1 = 223/300;
L2 = 222/300;
L3 = 222/300;
L4 = 200/300;


body = rigidBody('link1');
joint = rigidBodyJoint('joint1', 'revolute');  
setFixedTransform(joint,trvec2tform([0, 0, 0]));
joint.JointAxis = [0 0 1];
joint.PositionLimits = [-pi/4 pi/4];
body.Joint = joint;
addBody(robot, body, 'base');


body = rigidBody('link2');
joint = rigidBodyJoint('joint2','revolute');
beta = 0;             
tform1 = [        1,          0,          0, 0;     % X
                  0, cos(beta),  -sin(beta), 0;
                  0, sin(beta),   cos(beta), L1;
                  0,          0,          0, 1];
              
beta = pi/2;
tform2 = [cos(beta),          0,  sin(beta),  0;    % Y
                  0,          1,          0,  0;
         -sin(beta),          0,  cos(beta),  0;
                  0,          0,          0,  1];
beta = pi/2;             
tform3 = [cos(beta), -sin(beta),          0,  0;     %Z
          sin(beta),  cos(beta),          0,  0;
                  0,          0,          1,  0;
                  0,          0,          0,  1];  
              
setFixedTransform(joint, tform1*tform2*tform3);
joint.JointAxis = [0 0 1];
joint.PositionLimits = [-pi/2 pi/2];
body.Joint = joint;
addBody(robot, body, 'link1');


body = rigidBody('link3');
joint = rigidBodyJoint('joint3','revolute');
beta = 0;             
tform1 = [        1,          0,          0, 0;     % X
                  0, cos(beta),  -sin(beta), L2;
                  0, sin(beta),   cos(beta), 0;
                  0,          0,          0, 1];
            
beta = pi/2;             
tform3 = [cos(beta), -sin(beta),          0, 0;     %Z
          sin(beta),  cos(beta),          0, 0;
                  0,          0,          1, 0;
                  0,          0,          0, 1];  
              
setFixedTransform(joint, tform1*tform3);
joint.JointAxis = [0 0 1];
joint.PositionLimits = [-pi/2 pi/2];
body.Joint = joint;
addBody(robot, body, 'link2');


body = rigidBody('link4');
joint = rigidBodyJoint('joint4','revolute');
beta = 0;             
tform1 = [        1,          0,          0, L3;     % X
                  0, cos(beta),  -sin(beta), 0;
                  0, sin(beta),   cos(beta), 0;
                  0,          0,          0, 1];
beta = pi/2;
tform2 = [cos(beta),          0,  sin(beta),  0;     % Y
                  0,          1,          0,  0;
         -sin(beta),          0,  cos(beta),  0;
                  0,          0,          0,  1];
beta = -pi/2;             
tform3 = [cos(beta), -sin(beta),          0,  0;     % Z
          sin(beta),  cos(beta),          0,  0;
                  0,          0,          1,  0;
                  0,          0,          0,  1];  
              
setFixedTransform(joint, tform1*tform2*tform3);
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'link3');


body = rigidBody('link5');
joint = rigidBodyJoint('joint5','revolute');
beta = pi/2;             
tform1 = [        1,          0,          0, 0;     % X
                  0, cos(beta),  -sin(beta), 0;
                  0, sin(beta),   cos(beta), 0;
                  0,          0,          0, 1];
              
setFixedTransform(joint, tform1);
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'link4');


body = rigidBody('tool');
joint = rigidBodyJoint('fix1','fixed');

beta = -pi/2             
tform1 = [        1,          0,          0, 0;     % X
                  0, cos(beta),  -sin(beta), L4;
                  0, sin(beta),   cos(beta), 0;
                  0,          0,          0, 1];
              
beta = -pi/2;             
tform3 = [cos(beta), -sin(beta),          0, 0;             %Z
          sin(beta),  cos(beta),          0, 0;
                  0,          0,          1, 0;
                  0,          0,          0, 1];  
              
setFixedTransform(joint, tform1*tform3);
body.Joint = joint;
addBody(robot, body, 'link5');



showdetails(robot);

show(robot);

% t = (0:0.2:10)'; % Time
% count = length(t);
% center = [0.6 0.1 0.1];
% radius = 0.15;
% theta = t*(2*pi/t(end));
% points = center + radius*[cos(theta) sin(theta) zeros(size(theta))];

wayPoints = newPoint(1:4,:);
count = length(wayPoints);

wayPoints(:,2) = wayPoints(:,2) + 100;

q0 = homeConfiguration(robot);
ndof = length(q0);
qs = zeros(count, ndof);


wayPoints = wayPoints./300;

%%

% Angoli di eulero per ZYX

a = newNormal(1,:);
b = [0 0 1];
angle = atan2(norm(cross(a,b)), dot(a,b));

eeOrientation = [0, pi+angle, 0];


% wayPoint(X Y Z fi_z fi_y fi_x)

wayPoints = [wayPoints(1,1:3) eeOrientation];

initTargetPose = eul2tform(wayPoints(1,4:6));
initTargetPose(1:3,end) = wayPoints(1,1:3)';

weights = [1, 1, 1, 1, 1, 1];
endEffector = 'tool';

% Solve for q0 such that the manipulator begins at the first waypoint
ik = inverseKinematics('RigidBodyTree',robot);
[qsol,solInfo] = ik(endEffector,initTargetPose,weights,q0);


%%

% %ik = inverseKinematics('RigidBodyTree', robot);
% qInitial = q0; % Use home configuration as the initial guess
% for i = 1:count
%     % Solve for the configuration satisfying the desired end effector
%     % position
%     point = wayPoints(i,:);
%     d = [-1  0  0  point(1,1);
%           0  0  1  point(1,2);
%           0 -1  0  point(1,3);
%           0  0  0           1 ];
%     qSol = ik(endEffector,d,weights,qInitial);
%     % Store the configuration
%     qs(i,:) = qSol;
%     % Start from prior solution
%     qInitial = qSol;
%     toc
% end
% toc

figure
show(robot,qsol);
hold on;