Gathering values while running fmincon

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

Answer 1

[Walter Roberson]

        if isempty(all_new_1)

should be

        if isempty(all_q_new_1)

[Yongmin Cho]

You said that I can call all q values by typing rfs()?

[Walter Roberson]

Yes. You will want to assign the results to a variable.

[Yongmin Cho]

Can I not use all q values in the main function? With the q values, I would like to make a simulation.

[Walter Roberson]

all_q = rfs();
q_new_1 = all_q{1};
q_new_2 = all_q{2};
q_new_3 = all_q{3};
q_new_4 = all_q{4};
q_new_5 = all_q{5};

With the code I posted previously, each of these variables will be a cell array, as I did not want to read through the robotics documentation to prove that each of the values would absolutely positively be a scalar. If you are sure that the values will all be scalars, then in the series of lines similar to

        all_q_new_1{end+1} = q_new_1;

change the {} to (), so like

        all_q_new_1(end+1) = q_new_1;

This is only valid if q_new_1 and the other variables are absolutely certain to be scalars.

[Yongmin Cho]

If it is in vector form, it would be great So I want to put the value in a 1x3 matrix for each all_q_new for each iteration.

[Walter Roberson]

? You appear to be working with 5 variables, q_new_1 through q_new_5 . I do not know which elements would go into the 1 x 3 ?