Quasi newton method for NLP code problem

Hello, Can some one tell whats wrong with the following program ? I tried to change x and evaluate V but I got some difficulties to run. Is there any built in function to solve PRSopt_QN with quasi-newton ?

function V = PRSopt_QN(radius,alpha,beta) 
L=1;     
z=0.7071068; 
ts=1/2;
t=0:ts:1;
for k=1:length(t)
    xi_2=beta(k);
    rp=radius(k);
    for j=1:length(t)
        xi_1=0;
        xi_2=alpha(j);
        xi_3=beta(j);
        th(j)=-0.2*cos(2*pi*t(j));
        psi(k)=0.2*sin(2*pi*t(k)); 
        phi(j)=atan2(sin(psi(k))*sin(th(j)),(cos(psi(k))+cos(th(j))));
        R=Rot('y',th(j))*Rot('x',psi(k))*Rot('z',phi(j));
        x(k)=1/2*rp*(-cos(phi(j))*cos(psi(k))+cos(phi(j))*cos(th(j))+sin(phi(j))*sin(psi(k))*sin(th(j)));
        y(k)=-rp*cos(psi(k))*sin(phi(j));
        zc(k)=z;
        delta(k)=sqrt(x(k)^2+y(k)^2)
        p=[x(k);y(k);zc(k)];
        a1(:,k)=R*[rp;0;0];
        a2(:,k)=R*[rp*cos(xi_2);rp*sin(xi_2);0];
        a3(:,k)=R*[rp*cos(xi_3);rp*sin(xi_3);0];
        
        r1=p+R*[rp;0;0];
        r2=p+R*[rp*cos(xi_2);rp*sin(xi_2);0];
        r3=p+R*[rp*cos(xi_3);rp*sin(xi_3);0];
        P=1/3*(r1+r2+r3);
        g1=inv(Rot('z',0))*r1; 
        g2=inv(Rot('z',xi_2))*r2;
        g3=inv(Rot('z',xi_3))*r3;
        % leg length 
        b1(k)=g1(1)+sqrt(L^2-g1(3)^2);
        b2(k)=g2(1)+sqrt(L^2-g2(3)^2);
        b3(k)=g3(1)+sqrt(L^2-g3(3)^2);

        % passive koint value 
        sin_th21=(g1(1)-b1(k))/L;
        cos_th21=g1(3)/L;
        th21(k)=atan2(sin_th21,cos_th21);
        
        sin_th22=(g2(1)-b2(k))/L;
        cos_th22=g2(3)/L;
        th22(k)=atan2(sin_th22,cos_th22);
        
        sin_th23=(g3(1)-b3(k))/L;
        cos_th23=g3(3)/L;
        th23(k)=atan2(sin_th23,cos_th23);
        
      
        pr1(k)=norm(r1-p); 
        pr2(k)=norm(r3-p); 
        pr3(k)=norm(r3-p);  
     
        r12(k)=norm(r1-r2);
        r23(k)=norm(r3-r2);
        r31(k)=norm(r3-r1);
        Link1(k)=norm(r1-[b1(k);0;0]);
        Link2(k)=norm(r2-Rot('z',xi_2)*[b2(k);0;0]);
        Link3(k)=norm(r3-Rot('z',xi_3)*[b3(k);0;0]);
        
        s21=[0;1;0];                      
        s22=[-sin(xi_2);cos(xi_2);0];   
        s23=[-sin(xi_3);cos(xi_3);0];
        
        
        Constraint1(k)=(p+a1(:,k))'*s21;
        Constraint2(k)=(p+a2(:,k))'*s22;
        Constraint3(k)=(p+a3(:,k))'*s23;
        
              
        Gc=[s21',cross(s21,a1(:,k))';s22',cross(s22,a2(:,k))';s23',cross(s23,a3(:,k))'];
        M=[eye(6)-transpose(Gc)*pinv((transpose(Gc)))]; 
        st(:,k)=[0.2*cos(2*pi*t(k))*0;0.2*sin(2*pi*t(k))*0;2*cos(2*pi*t(k))*0;2*cos(2*pi*t(k));2*sin(2*pi*t(k));-0.02*cos(2*pi*t(k))*0]; % Desired task velocity
        stm(:,k)=M*st(:,k) 
        wx(k)=st(4,k);
        wy(k)=st(5,k);
        vz(k)=stm(3,k);
        
        C1=[-sin(xi_1) cos(xi_1) -(a1(1)*cos(xi_1)+a1(2)*sin(xi_1)) ; -sin(xi_2) cos(xi_2) -(a2(1)*cos(xi_2)+a2(2)*sin(xi_2)) ; -sin( xi_3) cos( xi_3) -(a3(1)*cos( xi_3)+a3(2)*sin( xi_3)) ];
        C2=[-a1(3)*cos(xi_1) -a1(3)*sin(xi_1) ;-a2(3)*cos(xi_2) -a2(3)*sin(xi_2);-a3(3)*cos( xi_3) -a3(3)*sin( xi_3)]
        V(:,k)=(inv(C1)*C2)*[wx(k);wy(k)];
        Stm(:,k)=M*[V(1,k);V(2,k);vz(k);wx(k);wy(k);V(3,k)] 
        
        constraint2(:,k)=Gc*Stm(:,k) 
        constraint1(:,k)=Gc*stm(:,k)  
        con1(k)=constraint1(1,k);
        con2(k)=constraint1(2,k);
        con3(k)=constraint1(3,k);  
        
    end 
end 
return;
end 



%% 
function Rotation = Rot(char,a)

c=cos(a); s=sin(a);
switch char
    case 'x' 
      Rotation =[1,  0,  0;
               0,  c, -s;
               0,  s,  c];
    case 'y'
     Rotation = [c,  0,  s;
               0,  1,  0;
              -s,  0,  c];
    case 'z'
     Rotation = [c, -s,  0;
               s,  c,  0;
               0,  0,  1];
end

Answer 1

[Matt J]

Is there any built in function to solve PRSopt_QN with quasi-newton ?

Yes, fminunc is a quasi-newton solver,

https://www.mathworks.com/help/optim/ug/fminunc.html#responsive_offcanvas

[HN]

Thank you Matt J .

Does this function support a vector output ? It keeps giving me the following error for

PRSopt_QN(radius,alpha,beta) % given above  

fun = @PRSopt_QN
x0 = [0.25, deg2rad(120),deg2rad(120)]';
x = fminunc(fun,x0(1),x0(2),x0(3),options)


Unable to perform assignment because dot indexing is not supported for variables of this type.

Error in fminunc (line 240)
options.GradObj = optimget(options,'GradObj',defaultopt,optimgetFlag);

[Matt J]

All input/outputs assume the unknowns to be organized as a single vector.

fun = @(x)PRSopt_QN(x(1),x(2),x(3));

x0 = [0.25, deg2rad(120),deg2rad(120)]';

x = fminunc(fun,x0,options),

[HN]

Thank you Matt J

It still gives the following error message.

Error using PRSopt_QN
Too many output arguments.

Error in @(x)PRSopt_QN(x(1),x(2),x(3))

Error in fminunc (line 313)
        [f,GRAD] = feval(funfcn{3},x,varargin{:});

Caused by:
    Failure in initial objective function evaluation. FMINUNC cannot continue.

line 313 is

[f,GRAD] = feval(funfcn{3},x,varargin{:});

[Matt J]

I do not know what options you have given to fminunc, but you should not select SpecifyObjectiveGradient=true, unless you intend to supply your own gradient calculation.

[HN]

Sorry for the delay Matt J

options = optimoptions('fminunc','Algorithm','trust-region','SpecifyObjectiveGradient',false);
fun = @(x)PRSopt_QN(x(1),x(2),x(3));
x0 = [0.25, deg2rad(120),deg2rad(120)]';
x = fminunc(fun,x0,options)

Then, I got the following error

Error using fminunc (line 182)
Options Algorithm = 'trust-region' and SpecifyObjectiveGradient = false conflict.

[Matt J]

Why not just use default options?

[HN]

Matt J , I just revoke the function like this and gave me the error of indexing. Is it because of the input x0?

x = fminunc(fun,x0)

delta =

    0.0025


stm =

    0.0647
   -0.0870
         0
    1.9929
    0.0016
    0.0490


V =

    0.0647
   -0.0870
    0.0490


constraint1 =

   1.0e-16 *

   -0.3469
    0.2949
    0.2949

Index exceeds the number of array elements (1).

Error in PRSopt_QN (line 11)
        xi_3=beta(j);

Error in @(x)PRSopt_QN(x(1),x(2),x(3))

Error in fminunc (line 304)
        f = feval(funfcn{3},x,varargin{:});

Caused by:
    Failure in initial objective function evaluation. FMINUNC cannot continue.
    
    

Here is where the fminunc fails

switch funfcn{1}
case 'fun'
    try
        f = feval(funfcn{3},x,varargin{:});
    catch userFcn_ME
        optim_ME = MException('optim:fminunc:ObjectiveError', ...
            getString(message('optim:fminunc:ObjectiveError')));
        userFcn_ME = addCause(userFcn_ME,optim_ME);
        rethrow(userFcn_ME)
    end

[Matt J]

It has nothing to do with fminunc. Calling fun(x0) resulted in an error.

[HN]

Yes, thats right Matt J . But I tried to understand what makes the function incompaible to the fminunc.

Would you please take a minute to try the function for arbitrary or given intial values ?I am still trying to figure out but no luck.

Thank you.

[Matt J]

Here is what I get, when I run fun(x0)

>>fun(x0)

Index exceeds the number of array elements (1).

Error in test>PRSopt_QN (line 21)
        xi_2=alpha(j);

You are running a loop over j as if alpha is expected to be a vector of values. But we know that alpha is a scalar.

[HN]

Matt J , yest that is the error I am confronging as well. Is that from the initialization or the two for loops in the function ?

Thank you

[Matt J]

That depends on whether alpha was intended to be a scalar or not.

[HN]

Matt J It is 1D array which is a range of value of alpha. Beta and radius as well.

Thank you

[Matt J]

Well then the initialization is wrong. Your initial guess x0 must contain a value for all the unknowns and clearly there are more than 3 of them. Also, you should redefine fun apropriately, e .g.,

fun = @(x)PRSopt_QN(x(:,1),x(:,2),x(:,3));

or

fun = @(x)PRSopt_QN(x(1,:),x(2,:),x(3,:));

or whatever is required to pass the appropriate pieces of x0 to PRSopt_QN().

[HN]

Matt J , I edited as follows and after some iteration, it come up with the error shown. One more thing, How can I pick the optimum x ? and the minimum V out of the listed values ?

Thank you ?

 ts=5;
t=0:1/ts:1;
x1U=0.35; % in m
x1L=0.20; % in m
x2U = deg2rad(150);
x2L = deg2rad(120);
x3U = deg2rad(-150);
x3L = deg2rad(-120);
x1=x1L:(x1U-x1L)/ts:x1U;
x2=x2L:(x2U-x2L)/ts:x2U;
x3=x3L:(x3U-x3L)/ts:x3U;
>> x=[x1;x2;x3];
>> fun = @(x)PRSopt_QN(x(1,:),x(2,:),x(3,:));
>> x = fminunc(fun,x)

%% error
Error using fminunc (line 383)
Supplied objective function must return a scalar value.

[Matt J]

The final x returned by fminunc will not be a list. It will be an array xOptimal the same size as your initial x0. The idea is that fminunc will look for the array xOptimal which gives a smaller value V=fun(xOptimal) than any other input array x the same size. You can get the optimal V with the syntax,

[xOptimal,V] = fminunc(fun,x0)

Your function must return a scalar value V because only then can fminunc be able to make a comparison fun(xOptimal)<fun(x) for the purposes of the minimization.

[HN]

"Your function must return a scalar value V because only then can fminunc be able to make a comparison fun(xOptimal)<fun(x) for the purposes of the minimization." ->

Does that mean V(:,k)=(inv(C1)*C2)*[wx(k);wy(k)]; must return a scalar ? which is actually a 3x1 vector objective function.

Thank you Matt J

[Matt J]

Yes, otherwise how can it be "minimized"?. How can you make the statement "A is less than B" unless A and B are scalars?

[HN]

Matt J,

Therefore, I must take a norm of V as an output from the function ?

But, three elements of V are physically not homogeneous. Two are linear and one is angular ? How is that going to be treated in the fminunc ?

Thank you

[HN]

Matt J,

I took the a verage value of V and computed the miniimum value. Below, xOptimal is a vector that shows the three optmizatation variables. So,is v=1.0913e-16 computed at xOptimal =[0.35 ;2.6180 ;-2.6180] ? Thank you

Local minimum possible.

fminunc stopped because it cannot decrease the objective function
along the current search direction.

<stopping criteria details>

xOptimal =

  Columns 1 through 9

    0.2000    0.2150    0.2300    0.2450    0.2600    0.2750    0.2900    0.3050    0.3200
    2.0944    2.1468    2.1991    2.2515    2.3038    2.3562    2.4086    2.4609    2.5133
   -2.0944   -2.1468   -2.1991   -2.2515   -2.3038   -2.3562   -2.4086   -2.4609   -2.5133

  Columns 10 through 11

    0.3350    0.3500
    2.5656    2.6180
   -2.5656   -2.6180


V =

   1.0913e-16

[HN]

Matt J ,

Would you please help in the following issues I am experiancing ?

1. How can I set a range of values for optimization variables as a search domain?
2. No iterations while running the optimization. why is that ?
3. How can I plot the output in countrour or 3D to visiualize if the out is must be scalar ?

Thank you

                                                        First-order 
 Iteration  Func-count       f(x)        Step-size       optimality
     0          34       1.6549e-16                         0.332

Local minimum possible.

fminunc stopped because it cannot decrease the objective function
along the current search direction.

<stopping criteria details>

xOptimal =

  Columns 1 through 9

    0.1000    0.1550    0.2100    0.2650    0.3200    0.3750    0.4300    0.4850    0.5400
    2.0944    2.1468    2.1991    2.2515    2.3038    2.3562    2.4086    2.4609    2.5133
   -2.0944   -2.1468   -2.1991   -2.2515   -2.3038   -2.3562   -2.4086   -2.4609   -2.5133

  Columns 10 through 11

    0.5950    0.6500
    2.5656    2.6180
   -2.5656   -2.6180


V =

   1.6549e-16