Automatically generate constraints for trajectory optimization

Question

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
    %   defectsGrad = [nState, nTime-1, nDecVars] = gradient of defects


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