Possible bug with coneprog?

I am wondering if this is a bug in coneprog or expected behavior. The example used to reproduce this is from https://www.mathworks.com/help/optim/ug/discretized-optimal-trajectory-cone-programming.html . When the code below is run as is then the expected result is outputted. However, when a "dummy" constraint is added (by uncommenting the two lines in the setupproblem function) that has nothing to do with the original it changes the answer signicantly.

Thanks in advance for any responses.

clc,clear,close all
p0 = [0 0 0];
pF = [5 10 3];
trajprob = setupproblem(20);
opts = optimoptions('coneprog','Display','iter');
[sol,fval,eflag,output] = solve(trajprob,options=opts)
plottrajandaccel(sol,p0,pF)

function trajectoryprob = setupproblem(T,air)
    if nargin == 1
        air = false;
    end
    N = 50;
    g = [0 0 -9.81];
    p0 = [0 0 0];
    pF = [5 10 3];
    Amax = 25;
    t = T/N;
    p = optimvar("p",N,3);
    v = optimvar("v",N,3);
    a = optimvar("a",N-1,3,"LowerBound",-Amax,"UpperBound",Amax);
    trajectoryprob = optimproblem;
    s = optimvar("s",N-1,"LowerBound",0,"UpperBound",3*Amax);
    trajectoryprob.Objective = sum(s)*t;
    scons = optimconstr(N-1);
    for i = 1:(N-1)
        scons(i) = norm(a(i,:)) <= s(i);
    end
    acons = optimconstr(N-1);
    for i = 1:(N-1)
        acons(i) = norm(a(i,:)) <= Amax;
    end
    vcons = optimconstr(N+1,3);
    vcons(1,:) = v(1,:) == [0 0 0];
    if air
        vcons(2:N,:) = v(2:N,:) == v(1:(N-1),:)*exp(-t) + t*(a(1:(N-1),:) + repmat(g,N-1,1));
    else
        vcons(2:N,:) = v(2:N,:) == v(1:(N-1),:) + t*(a(1:(N-1),:) + repmat(g,N-1,1));
    end
    vcons(N+1,:) = v(N,:) == [0 0 0];
    pcons = optimconstr(N+1,3);
    pcons(1,:) = p(1,:) == p0;
    if air
        pcons(2:N,:) = p(2:N,:) == p(1:(N-1),:) + t*(1+exp(-t))/2*v(1:(N-1),:) + t^2/2*(a(1:(N-1),:) + repmat(g,N-1,1));
    else
        pcons(2:N,:) = p(2:N,:) == p(1:(N-1),:) + t*v(1:(N-1),:) + t^2/2*(a(1:(N-1),:) + repmat(g,N-1,1));
    end
    pcons((N+1),:) = p(N,:) == pF;
    trajectoryprob.Constraints.acons = acons;
    trajectoryprob.Constraints.scons = scons;
    trajectoryprob.Constraints.vcons = vcons;
    trajectoryprob.Constraints.pcons = pcons;
    
    %%%%%%%%% NEW DUMMY VARIABLE AND CONSTRAINT %%%%%%%%%
%     test_var = optimvar("test_var",N,1);
%     trajectoryprob.Constraints.test_cons = test_var >= 0;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end

function plottrajandaccel(sol,p0,pF)
figure
psol = sol.p;
plot3(psol(:,1),psol(:,2),psol(:,3),'rx')
hold on
plot3(p0(1),p0(2),p0(3),'ks')
plot3(pF(1),pF(2),pF(3),'bo')
hold off
view([18 -10])
xlabel("x")
ylabel("y")
zlabel("z")
legend("Steps","Initial Point","Final Point")
figure
asolm = sol.a;
nasolm = sqrt(sum(asolm.^2,2));
plot(nasolm,"rx")
xlabel("Time step")
ylabel("Norm(acceleration)")
end

Answer 1

[Alan Weiss]

I think that you have identified a problem with the default options for trajectory problems: the optimality tolerance should be chosen smaller than default. If you run the problem with a tighter optimality tolerance, ther results look better, and look pretty much the same with or without your dummy variables.

p0 = [0 0 0];
pF = [5 10 3];
trajprob = setupproblem(20);
opts = optimoptions('coneprog','Display','iter','OptimalityTolerance',1e-10);
[sol,fval,eflag,output] = solve(trajprob,options=opts)

Solving problem using coneprog.
Iter           Fval  Primal Infeas    Dual Infeas    Duality Gap    Time
   1   1.960000e+01   3.333333e-01   4.000000e-01   2.110223e-02    0.05
   2   1.834298e+02   3.226790e-02   3.872148e-02   2.042774e-03    0.06
   3   4.671236e+02   3.317278e-03   3.980734e-03   2.100059e-04    0.06
   4   1.794310e+02   1.393238e-03   1.671886e-03   8.820130e-05    0.07
   5   2.110933e+02   3.292572e-04   3.951086e-04   2.084418e-05    0.07
   6   1.970150e+02   9.771943e-05   1.172633e-04   6.186295e-06    0.07
   7   1.929653e+02   2.301163e-05   2.761396e-05   1.456790e-06    0.08
   8   1.923943e+02   3.491190e-06   4.189428e-06   2.210157e-07    0.08
   9   1.923107e+02   8.565044e-07   1.027805e-06   5.422247e-08    0.08
  10   1.922943e+02   2.858454e-07   3.430144e-07   1.809593e-08    0.09
  11   1.922915e+02   2.171556e-07   2.605867e-07   1.374740e-08    0.09
  12   1.922875e+02   9.214488e-08   1.106251e-07   5.833388e-09    0.09
  13   1.922872e+02   8.633602e-08   1.036511e-07   5.465649e-09    0.10
  14   1.922855e+02   6.106756e-08   7.343547e-08   3.865986e-09    0.10
  15   1.922849e+02   5.618335e-08   6.756207e-08   3.556783e-09    0.10
  16   1.922831e+02   2.403072e-08   2.936808e-08   1.521302e-09    0.11
  17   1.922829e+02   1.899389e-08   2.321210e-08   1.202437e-09    0.11
  18   1.922825e+02   1.398995e-08   1.678817e-08   8.856555e-10    0.11
  19   1.922819e+02   8.372654e-09   1.004769e-08   5.300445e-10    0.12
  20   1.922815e+02   3.353846e-09   4.025421e-09   2.123187e-10    0.12
  21   1.922814e+02   2.234111e-09   2.684028e-09   1.414315e-10    0.12
  22   1.922813e+02   1.668567e-06   2.709764e-09   9.146051e-11    0.13
  23   1.922812e+02   5.259403e-07   5.090527e-07   2.996456e-11    0.13
  24   1.922812e+02   3.930634e-07   3.850559e-07   2.266504e-11    0.13
  25   1.922812e+02   1.436881e-07   4.543098e-07   9.073223e-12    0.14
Optimal solution found.

sol = struct with fields:
           a: [49×3 double]
           p: [50×3 double]
           s: [49×1 double]
           v: [50×3 double]
    test_var: [50×1 double]

fval = 192.2812

eflag = 
    OptimalSolution

output = struct with fields:
           iterations: 25
    primalfeasibility: 1.4369e-07
      dualfeasibility: 4.5431e-07
           dualitygap: 9.0732e-12
            algorithm: 'interior-point'
         linearsolver: 'prodchol'
              message: 'Optimal solution found.'
               solver: 'coneprog'

plottrajandaccel(sol,p0,pF)

function trajectoryprob = setupproblem(T,air)
    if nargin == 1
        air = false;
    end
    N = 50;
    g = [0 0 -9.81];
    p0 = [0 0 0];
    pF = [5 10 3];
    Amax = 25;
    t = T/N;
    p = optimvar("p",N,3);
    v = optimvar("v",N,3);
    a = optimvar("a",N-1,3,"LowerBound",-Amax,"UpperBound",Amax);
    trajectoryprob = optimproblem;
    s = optimvar("s",N-1,"LowerBound",0,"UpperBound",3*Amax);
    trajectoryprob.Objective = sum(s)*t;
    scons = optimconstr(N-1);
    for i = 1:(N-1)
        scons(i) = norm(a(i,:)) <= s(i);
    end
    acons = optimconstr(N-1);
    for i = 1:(N-1)
        acons(i) = norm(a(i,:)) <= Amax;
    end
    vcons = optimconstr(N+1,3);
    vcons(1,:) = v(1,:) == [0 0 0];
    if air
        vcons(2:N,:) = v(2:N,:) == v(1:(N-1),:)*exp(-t) + t*(a(1:(N-1),:) + repmat(g,N-1,1));
    else
        vcons(2:N,:) = v(2:N,:) == v(1:(N-1),:) + t*(a(1:(N-1),:) + repmat(g,N-1,1));
    end
    vcons(N+1,:) = v(N,:) == [0 0 0];
    pcons = optimconstr(N+1,3);
    pcons(1,:) = p(1,:) == p0;
    if air
        pcons(2:N,:) = p(2:N,:) == p(1:(N-1),:) + t*(1+exp(-t))/2*v(1:(N-1),:) + t^2/2*(a(1:(N-1),:) + repmat(g,N-1,1));
    else
        pcons(2:N,:) = p(2:N,:) == p(1:(N-1),:) + t*v(1:(N-1),:) + t^2/2*(a(1:(N-1),:) + repmat(g,N-1,1));
    end
    pcons((N+1),:) = p(N,:) == pF;
    trajectoryprob.Constraints.acons = acons;
    trajectoryprob.Constraints.scons = scons;
    trajectoryprob.Constraints.vcons = vcons;
    trajectoryprob.Constraints.pcons = pcons;
    
    %%%%%%%%% NEW DUMMY VARIABLE AND CONSTRAINT %%%%%%%%%
    test_var = optimvar("test_var",N,1,"LowerBound",-1,"UpperBound",1);
    trajectoryprob.Constraints.test_cons = test_var >= 0;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end

function plottrajandaccel(sol,p0,pF)
figure
psol = sol.p;
plot3(psol(:,1),psol(:,2),psol(:,3),'rx')
hold on
plot3(p0(1),p0(2),p0(3),'ks')
plot3(pF(1),pF(2),pF(3),'bo')
hold off
view([18 -10])
xlabel("x")
ylabel("y")
zlabel("z")
legend("Steps","Initial Point","Final Point")
figure
asolm = sol.a;
nasolm = sqrt(sum(asolm.^2,2));
plot(nasolm,"rx")
xlabel("Time step")
ylabel("Norm(acceleration)")
end

I will update the example in the documentation.

Alan Weiss

MATLAB mathematical toolbox documentation

[Nick]

Ok I understand the problem. Thanks Alan!