MATLAB: Gathering values while running fmincon

fminconpeter corkestore value

I ran this code with manipul([4, 5, 8]) because I would like to optimize the a_new values.
When I tried like this, I got the following errors.
Undefined function or variable 'all_new_1'.
Error in manipul>rfs (line 29)
    if isempty(all_new_1)
Error in fmincon (line 536)
      initVals.f = feval(funfcn{3},X,varargin{:});
Error in manipul (line 4)
     a_new = fmincon(@rfs, a_initial, [], [],[],[],[],[], [], options)
Caused by:
    Failure in initial objective function evaluation. FMINCON cannot continue.
function a_new = manipul(a_initial)
       options = optimoptions('fmincon','Display','iter');
       a_new = fmincon(@rfs, a_initial, [], [],[],[],[],[], [], options)
       L(1) = Link([0 0 a_new(1,1) 0]);
       L(2) = Link([0 0 a_new(1,2) 0]);
       L(3) = Link([0 0 a_new(1,3) 0]);
       newR = SerialLink(L);
       T_1 = transl([15,20,0]); 
       T_2 = transl([20,25,-5]);
       T_3 = transl([16,8,0]);
       T_4 = transl([13,5,0]);
       T_5 = transl([25,3,0]);
       q_new_1 = newR.ikcon(T_1);  % Inverse kinematics solution with given disred position 1

       q_new_2 = newR.ikcon(T_2);  % Inverse kinematics solution with given disred position 2

       q_new_3 = newR.ikcon(T_3);  % Inverse kinematics solution with given disred position 3

       q_new_4 = newR.ikcon(T_4);  % Inverse kinematics solution with given disred position 4

       q_new_5 = newR.ikcon(T_5);  % Inverse kinematics solution with given disred position 5

end
function costFun = rfs(a_initial)
      persistent all_q_new_1 all_q_new_2 all_q_new_3 all_q_new_4 all_q_new_5
      if isempty(all_new_1)
        all_q_new_1 = {}; all_q_new_2 = {}; all_q_new_3 = {}; all_q_new_4 = {}; all_q_new_5 = {};
      end
      if nargin == 0
        costFun = {all_q_new1, all_q_new2, all_q_new_3, all_q_new_4, all_q_new_5};
        all_q_new_1 = {}; all_q_new_2 = {}; all_q_new_3 = {}; all_q_new_4 = {}; all_q_new_5 = {};
        return
      end
L(1) = Link([0 0 a_initial(1,1) 0]);
L(2) = Link([0 0 a_initial(1,2) 0]);
L(3) = Link([0 0 a_initial(1,3) 0]);
R = SerialLink(L,'name', 'My Robot');
% Expressing the desired position in homogeneous matrix
       T_1 = transl([15,20,0]); 
       T_2 = transl([20,25,-5]);
       T_3 = transl([16,8,0]);
       T_4 = transl([13,5,0]);
       T_5 = transl([25,3,0]);
q_new_1 = R.ikcon(T_1);  % Inverse kinematics solution with given disred position 1
q_new_2 = R.ikcon(T_2);  % Inverse kinematics solution with given disred position 2
q_new_3 = R.ikcon(T_3);  % Inverse kinematics solution with given disred position 3
q_new_4 = R.ikcon(T_4);  % Inverse kinematics solution with given disred position 4
q_new_5 = R.ikcon(T_5);  % Inverse kinematics solution with given disred position 5
T_real_1 = R.fkine(q_new_1); % Forward kinematics solution / initial position for the end effector
T_real_2 = R.fkine(q_new_2);
T_real_3 = R.fkine(q_new_3);
T_real_4 = R.fkine(q_new_4);
T_real_5 = R.fkine(q_new_5);
% Error between initial position and 5 desired position
costFun = (T_real_1.t(1,1) - T_1(1,4))^2 + (T_real_1.t(2,1) - T_1(2,4))^2 + (T_real_1.t(3,1) - T_1(3,4))^2 +...
    (T_real_2.t(1,1) - T_2(1,4))^2 + (T_real_2.t(2,1) - T_2(2,4))^2 + (T_real_2.t(3,1) - T_2(3,4))^2 + ...
    (T_real_3.t(1,1) - T_3(1,4))^2 + (T_real_3.t(2,1) - T_3(2,4))^2 + (T_real_3.t(3,1) - T_3(3,4))^2 + ...
    (T_real_4.t(1,1) - T_4(1,4))^2 + (T_real_4.t(2,1) - T_4(2,4))^2 + (T_real_4.t(3,1) - T_4(3,4))^2 + ...
    (T_real_5.t(1,1) - T_5(1,4))^2 + (T_real_5.t(2,1) - T_5(2,4))^2 + (T_real_5.t(3,1) - T_5(3,4))^2;
      all_q_new_1{end+1} = q_new_1;
      all_q_new_2{end+1} = q_new_2;
      all_q_new_3{end+1} = q_new_3;
      all_q_new_4{end+1} = q_new_4;
      all_q_new_5{end+1} = q_new_5;
end

Best Answer

        if isempty(all_new_1)
should be
        if isempty(all_q_new_1)
Related Question