Diagnose Infeasibility of Simple 3-DoF Trajectory Optimization

I am trying to write a simple optimization program which finds the best initial launch angle and velocity to arrive at a target location. I have written - using this mathworks tutorial as a guide - a program which I believe ought to do this.

First, I define and simulate for a randomly chosen initial guess the trajectory of a 3-DoF aircraft model (and define some constants).

% Constants.
g = 9.81;  % gravity
dt = 1e-1;
tspan = 0:dt:5;
e = [50,0]; % target loc.
degs = pi/180; % conversion.
a = 30*degs; % a is angle of launch in rads.
s = 50; % velocity of vehicle at launch.
x = 0;
h = 0;
y0 = [s,a,x,h]';

[~,Y] = ode45(@(t,y) dfu(t,y),tspan,y0);
figure
hold on
plot(Y(:,3),Y(:,4),'k')
plot(e(1),e(2),'bx')

Next, I set up the optimization program and determine the solution.

% Setup Optimization Problem
r = optimvar('r',1,'LowerBound',0,'UpperBound',90*degs);
v = optimvar('v',1,'LowerBound',0,'UpperBound',100);
f = fcn2optimexpr(@RtoODE,r,v,tspan,[v,r,x,h]);
obj = 0;
prob = optimproblem("Objective",obj);
prob.Constraints.terminalx = f(end,3) == e(1);
prob.Constraints.terminaly = f(end,4) == e(2);
r0.r = a;
r0.v = s;
options = optimoptions(@fmincon,'Display','iter');
[rsol,~] = solve(prob,r0,'Options',options);

Solving problem using fmincon.
                                            First-order      Norm of
 Iter F-count            f(x)  Feasibility   optimality         step
    0       3    0.000000e+00    4.520e+01    0.000e+00
    1       6    0.000000e+00    4.176e+01    0.000e+00    4.799e-01
    2       9    0.000000e+00    4.060e+01    0.000e+00    2.824e-01
    3      12    0.000000e+00    4.031e+01    0.000e+00    1.569e-01
    4      15    0.000000e+00    4.023e+01    0.000e+00    7.196e-02
    5      18    0.000000e+00    4.021e+01    0.000e+00    3.717e-02
    6      21    0.000000e+00    4.020e+01    0.000e+00    2.057e-02
    7      24    0.000000e+00    4.020e+01    0.000e+00    1.345e-02
    8      27    0.000000e+00    4.020e+01    0.000e+00    1.094e-02
    9      30    0.000000e+00    4.019e+01    0.000e+00    1.021e-02
   10      33    0.000000e+00    4.019e+01    0.000e+00    1.001e-02
   11      36    0.000000e+00    4.019e+01    0.000e+00    9.964e-03
   12      39    0.000000e+00    4.019e+01    0.000e+00    9.949e-03
   13      42    0.000000e+00    4.019e+01    0.000e+00    9.946e-03
   14      45    0.000000e+00    4.018e+01    0.000e+00    9.941e-03
   15      48    0.000000e+00    4.018e+01    0.000e+00    9.933e-03
   16      51    0.000000e+00    4.018e+01    0.000e+00    9.928e-03
   17      54    0.000000e+00    4.018e+01    0.000e+00    9.938e-03
   18      57    0.000000e+00    4.018e+01    0.000e+00    9.832e-03
   19      60    0.000000e+00    4.017e+01    0.000e+00    9.925e-03
   20      63    0.000000e+00    4.017e+01    0.000e+00    1.016e-02
   21      66    0.000000e+00    4.017e+01    0.000e+00    9.898e-03
   22      69    0.000000e+00    4.017e+01    0.000e+00    9.422e-03
   23      72    0.000000e+00    4.017e+01    0.000e+00    1.026e-02
   24      75    0.000000e+00    4.017e+01    0.000e+00    9.164e-03
   25      78    0.000000e+00    4.016e+01    0.000e+00    2.072e-02
   26      81    0.000000e+00    4.011e+01    0.000e+00    2.642e-01
   27      84    0.000000e+00    4.011e+01    0.000e+00    1.351e-02
   28      87    0.000000e+00    4.010e+01    0.000e+00    1.351e-02
   29      90    0.000000e+00    4.010e+01    0.000e+00    1.287e-02
   30      93    0.000000e+00    4.010e+01    0.000e+00    7.211e-03

                                            First-order      Norm of
 Iter F-count            f(x)  Feasibility   optimality         step
   31      96    0.000000e+00    4.010e+01    0.000e+00    3.624e-03
   32      99    0.000000e+00    4.006e+01    0.000e+00    1.711e-01
   33     102    0.000000e+00    4.006e+01    0.000e+00    9.214e-03
   34     105    0.000000e+00    3.973e+01    0.000e+00    1.683e+00
   35     108    0.000000e+00    3.942e+01    0.000e+00    1.581e+00
   36     111    0.000000e+00    3.878e+01    0.000e+00    6.448e+00
   37     114    0.000000e+00    3.465e+01    0.000e+00    1.971e+01
   38     118    0.000000e+00    3.458e+01    0.000e+00    7.287e-01
   39     123    0.000000e+00    3.456e+01    0.000e+00    6.677e-01
   40     126    0.000000e+00    3.455e+01    0.000e+00    2.639e-01
   41     131    0.000000e+00    3.455e+01    0.000e+00    2.384e-01
   42     135    0.000000e+00    3.455e+01    0.000e+00    1.527e-01
   43     140    0.000000e+00    3.455e+01    0.000e+00    3.766e-02
   44     145    0.000000e+00    3.455e+01    0.000e+00    3.395e-02
   45     149    0.000000e+00    3.455e+01    0.000e+00    2.173e-02
   46     154    0.000000e+00    3.455e+01    0.000e+00    3.291e-03
   47     161    0.000000e+00    3.455e+01    0.000e+00    2.113e-04
   48     175    0.000000e+00    3.455e+01    0.000e+00    8.856e-09
   49     179    0.000000e+00    3.455e+01    0.000e+00    3.542e-09

Converged to an infeasible point.

fmincon stopped because the size of the current step is less than
the value of the step size tolerance but constraints are not
satisfied to within the value of the constraint tolerance.


Consider enabling the interior point method feasibility mode.

y0 = [rsol.v,rsol.r,x,h]';

% Model Production.
[~,Y] = ode45(@(t,y) dfu(t,y),tspan,y0);
plot(Y(:,3),Y(:,4),'r')

% Output check.
angle     = rsol.r/degs

angle = 78.6686

velocity  = rsol.v

velocity = 79.7992

downrange = Y(end,3)

downrange = 78.3958

altitude  = Y(end,4)

altitude = 268.5932

targetx   = e(1)

targetx = 50

targety   = e(2)

targety = 0

Obviously, this is determining the problem to be infeasible. But I do not suspect this is the case. Just manually tuning some values returns successful solutions to within the required parameters.

Here are the additional functions required to run this program:

function dydt = dfu(~,y)
g = 9.81;
dydt = zeros(4,1);
dydt(1) = -g*sin(y(2));
dydt(2) = -(g*cos(y(2)))/y(1);
dydt(3) = y(1)*cos(y(2));
dydt(4) = y(1)*sin(y(2));
end

function solpts = RtoODE(r,v,tspan,y0)
sol = ode45(@(t,y)dfu(t,y),tspan,y0);
solpts = deval(sol,tspan);
solpts = solpts(3:4,:);
end

I was wondering if perhaps anyone can find my mistake, or offer general advice on simulating and solving optimization problems for this purpose.

Thank you.

[Alan Weiss]

I am not sure that you found the most relevant documentation. These examples might be closer to what you are attempting:

Discretized Optimal Trajectory, Problem-Based

Optimize an ODE in Parallel

That said, I have not read your code in detail to try to debug it. The code might be fine, with the problem being a difficult one for fmincon to satisfy. In particular, the discretized formulation can be superior from a numerical stability point of view.

Alan Weiss

MATLAB mathematical toolbox documentation

[Benjamin Thompson]

Possibly due to the choice of states here. Try using x, y, x-dot, and y-dot as the state variables. Then calculate the initial launch angle and velocity from the results of the optimization.

[Ayden Clay]

Thank you both for the help. I'll follow through the more relevant documentation.

Answer 1

[Bjorn Gustavsson]

Why not write the equation of motion component-by-component instead:

function dydtd2ydt2 = dfu(~,y)
g = 9.81;
dydtd2ydt2 = zeros(4,1);
dydy = y(3:4);
d2ydt2 = [0;-g];
dydtd2ydt2 = [dydt(:);d2ydt2];
end

Then you look at the ballode-example for how to catch the ground-impact with event-detection. That will definitely give you the postition of the particle as y(2) (presumably vertical) is equal to zero. From that you can determine how far from your target you landed and that is your function to fit. From there fmincon should be able to do the job. Have look at the help, documentation and code for/of ballode.

HTH

[Ayden Clay]

Thank you for your response! Yes, I can see how they're equivalent! Is there any benefit in that formulation?

I will check out the code you have mentioned. So I'll calculate the distance required and minimize the miss distance to landing instead? I'm more than happy to do that. I'm just working on this problem as a pre-cursor to a more advanced problem in the future. So I'm just looking to understand how the process works for now.

Thank you for your help.

[Bjorn Gustavsson]

@Ayden Clay: In my opinion the main benefit is that you have a "reasonably simple to interpret" system in terms of the acceleration in the two cardinal directions - which instantly leads to the corresponding accelerations - and that is all that is necessary to write out the equations-of-motion. The formulation you use the magnitude of the velocity and the angle of the velocity - to get that right I'd have to do some thinking about how rapidly the velocity turns, and thinking is hard work that is easy to get wrong...

[Ayden Clay]

I understand. Thank you for the help!

[Bjorn Gustavsson]

Happy that it helped.

[Ayden Clay]

I've tried to recreate my problem with the new discrete version linked above but am struggling to get the same output as before. I'm now getting an error about operands needing to be convertible to logical forms.

function [sol,fval,eflag,output] = DiscreteOptimization(T)
% [sol,fval,eflag,output]
N = 50;
g = 9.81;
degs = pi/180;
x0 = [50 15*degs 0 0]';
xN = [0 0 50 0]';
t = ceil(T/N);
x = optimvar('x',4,N); % v, g, x, h.
u = optimvar('u',2,N-1); % a and r.

% Define problem.
prob = optimproblem;

% Convert nonlinear functions to optimizable.
% integ = fcn2optimexpr(@RK,x,u,t,'OutputSize',[4,1]); % Ignore for now.
dydt = fcn2optimexpr(@dfu,x,u,'OutputSize',[4,1]);

% Control constraints.
ucons = optimconstr(2,N-1);
for i = 1:N-1
    ucons(1,i) = norm(u(1,i)) <= 2;
    ucons(2,i) = norm(u(2,i)) <= 5*degs;
end

% State constraints.
xcons = optimconstr(4,N+1);
xcons(:,1)      = x(:,1) == x0;
xcons(:,N+1)    = x(:,N) == xN;

% Dynamic constraints.
for i = 2:N
xcons(:,i) = x(:,i) == x(:,i-1) + dydt*t;
end

% Optimization:
prob.Constraints.ucons = ucons;
prob.Constraints.xcons = xcons;
[sol,fval,eflag,output] = solve(prob);
end

function dydt = dfu(y,u)
g = 9.81;
dydt = zeros(1,4);
dydt(1) = -g*sin(y(2)) + u(1);
dydt(2) = -(g*cos(y(2)))/y(1) + u(2);
dydt(3) = y(1)*cos(y(2));
dydt(4) = y(1)*sin(y(2));
end

[Alan Weiss]

I corrected a few minor errors in your syntax. I also changed sin to sind and cos to cosd in the dfu function; was that the right thing to do? I still get no feasible solution, and I'm not sure why.

function [sol,fval,eflag,output] = DiscreteOptimization(T)
N = 50;
g = 9.81;
degs = pi/180;
x0 = [50 15*degs 0 0]';
xN = [0 0 50 0]';
% t = ceil(T/N); % This looks like a mistake to me
t = T/N;
x = optimvar('x',4,N); % v, g, x, h.
ulb = repmat([-2;-5*degs],1,N-1); % Bound for u
uub = -ulb;
u = optimvar('u',2,N-1,'LowerBound',ulb,'UpperBound',uub); % a and r.

% Define problem.
prob = optimproblem;

% Convert nonlinear functions to optimizable.
% integ = fcn2optimexpr(@RK,x,u,t,'OutputSize',[4,1]); % Ignore for now.
dydt = fcn2optimexpr(@dfu,x,u,'OutputSize',[4,1]);

% % Control constraints. % I simply put bounds, which is more efficient
% ucons = optimconstr(2,N-1);
% for i = 1:N-1
%     ucons(1,i) = norm(u(1,i)) <= 2;
%     ucons(2,i) = norm(u(2,i)) <= 5*degs;
% end

% State constraints.
xcons = optimconstr(4,N+1);
xcons(:,1)      = x(:,1) == x0;
xcons(:,N+1)    = x(:,N) == xN;

% Dynamic constraints.
for i = 2:N
xcons(:,i) = x(:,i) == x(:,i-1) + dydt*t;
end

% Optimization:
% prob.Constraints.ucons = ucons;
prob.Constraints.xcons = xcons;
starti = zeros(4,N); % Linear interpolation for initial point
for i = 1:4
    starti(i,:) = linspace(x0(i),xN(i),N); % linear interpolation
end
start.x = starti;
start.u = zeros(size(u));
opts = optimoptions("fmincon","MaxFunctionEvaluations",1000*N);
[sol,fval,eflag,output] = solve(prob,start,"Options",opts);
end


function dydt = dfu(y,u)
g = 9.81;
dydt = zeros(4,1); % Needed because the output size is 4-by-1
dydt(1) = -g*sind(y(2)) + u(1);
dydt(2) = -(g*cosd(y(2)))/y(1) + u(2);
dydt(3) = y(1)*cosd(y(2));
dydt(4) = y(1)*sind(y(2));
end

Alan Weiss

MATLAB mathematical toolbox documentation

[Ayden Clay]

Thank you for correcting my syntax, the change to cosd and sind isn't necessary as I convert everything to radians by multiplying by "degs". Sorry thats my way of converting because then I can say "Oh it's 40 degrees" or "40*degs". I've converted it back and it's exceeding function evaluation limits, probably just a difficult problem for it to solve, but useful to see a working program. Thank you very much!