# Automatically generate constraints for trajectory optimization

This is a follow up to my previous post here

I'm interested in performing trajectory optimization from the problem mentioned in abov link.

I want to supply the following as dynamical constraints to MATLAB's fmincon.

$$\frac{d \phi}{dt} = -M^TDM\phi + W \hspace{1cm}(1)$$

$$\phi$$ is a vector with variables [ A B C D E F].

I've been referring to this article that illustrates how trapezoidal collocation is done

For example, if $$\phi$$ is a scalar

$$\dot\phi = f \hspace{1cm} (2)$$

integrating the above from time $$t_k$$ to $$t_{k+1}$$ and approximating using trapezoidal quadrature gives,

$$\phi_{k+1} = \phi_k + \frac{1}{2}h_k(f_{k+1} + f_k) \hspace{1cm} (3)$$

This will translate into a set of non-linear equality constraints (ref) for k = 0 to N-1.

I would like to know how to generate these constraints automatically. Is it possible to generate these constraints automatically and pass it as a handle to fmincon. Because it is possible to manually write for a single variable but for multivariable it gets complex.

I also looked at a repository available here that can be used in MATLAB. However, I am not sure if this can be used to generate constraints.

Also, I would like to understand how equation (3) should actually be written as an equality constraint. Should one solve the ode system in (1) and use the values of $$\phi$$ at the collocation points to evaluate the RHS of equation (3)? What happens to RHS?

Any suggestions/explanations for the above questions will be really useful.

EDIT: I tried following the example code given here to understand how ceq (equality constraint) has to be implemented

For example, in the code provided in the above link

function [ c, ceq ] = double_integrator_constraints( x )


is given.

Could someone explain how the input argument, x, is computed? Also, I don't understand how ydesiredend has to be mentioned in equality constraints.

In my problem, only the initial conditions are defined for the dynamical constraints and the terminal conditions are not available. In that case , I am not sure how the equality constraints have to be defined.

EDIT2: Update: I could solve this problem using fminunc and lsqnonlin.

Dhat0 = %input vector
% fun   = @objfun;
% [Dhat,fval] = fminunc(fun, Dhat0)

%% lsqnonlin
Dhat = lsqnonlin(@(Dhat) objfun(Dhat),Dhat0)

function f = objfun(Dhat)

%% Integrator settings
tspan = %tspan
options = odeset('abstol', 1e-10, 'reltol', 1e-9);

%% generate exact solution
phi0 = % initial condition vector
[t, phi]  = ode15s(@(t,phi) exact(t,phi), tspan , phi0 ,options);

%% generate approximate solution

[t, phi_tilde]  = ode15s(@(t,phi_tilde) approx(t,phi_tilde, Dhat), tspan , phi0 ,options);

%% objective function for fminunc
% diff = (phi - phi_tilde).*(phi - phi_tilde);
% f = sum(diff, 'all')

%% objective function for lsqnonlin
f  = phi - phi_tilde
end


I am still interested in understanding how the constrained optimization problem can be solved. I'm trying to set up the problem this way.

I would like to understand how noncol should be called from fmincon.

nonlcon = @defects;
Dhat= fmincon(@objfun,Dhat0,A,b,Aeq,beq,lb,ub,nonlcon)


For my system, A,b,Aeq,beq,lb,ub = []

But, I am not sure from where to pass these arguments for defects(dt,x,f).

function [c ceq] = defects(dt,x,f)
% ref: https://github.com/MatthewPeterKelly/OptimTraj
% This function computes the defects that are used to enforce the
% continuous dynamics of the system along the trajectory.
%
% INPUTS:
%   dt = time step (scalar)
%   x = [nState, nTime] = state at each grid-point along the trajectory
%   f = [nState, nTime] = dynamics of the state along the trajectory
%
% OUTPUTS:
%   defects = [nState, nTime-1] = error in dynamics along the trajectory

nTime = size(x,2);

idxLow = 1:(nTime-1);
idxUpp = 2:nTime;

xLow = x(:,idxLow);
xUpp = x(:,idxUpp);

fLow = f(:,idxLow);
fUpp = f(:,idxUpp);

% This is the key line:  (Trapazoid Rule)
defects = xUpp-xLow - 0.5*dt*(fLow+fUpp);
ceq = reshape(defects,numel(defects),1);
c = [];
end
end


Any suggestions will be really helpful

• What you have is what's called an "optimal control problem". You might want to take a look at books and articles on the subject. It is not efficient to use a standard optimization method such as fmincon for these kinds of problems, but there are very efficient optimal control algorithms. – Wolfgang Bangerth Mar 20 at 22:39
• @WolfgangBangerth Thank you. I've been referring to chapters 3 and 4 of the book by Betts and articles by Kelly. to start with I have a simple system with 6 variables, so I'd prefer to solve it using fmincon to understand how the problem has to be set up. I'll definitely read more on other efficient algorithms. The problem now is, I am unsure about how the constraints have to be set up for solving in fmincon. – Natasha Mar 21 at 5:27
• @WolfgangBangerth The other doubt that I have is, the control variables in my system i.e $\hat{D}$ are not functions of time. I'd like to understand if this can still be solved as an optimal control problem. – Natasha Mar 21 at 5:51
• If you are imposing all these constraints, do you even expect there to be a solution in the case with noise? You are basically trying to exactly fit random noise with 6 degrees of freedom – whpowell96 Mar 21 at 6:29
• @whpowell96 I am not sure if I can find a solution with all these constraints. But I really want to try if imposing the condition works when noise term is set to zero. The problem ow is, I don't know how to formulate the equality constraints (in the code) using equation(1) when W=0. I understand I have to use the trapezoidal rule, but I am stuck as to how this should be implemented in the code. – Natasha Mar 21 at 6:38