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)