Write our own ALM method

[msparapa]

I've been running into some issues with scipy's SQP method dealing specifically with the number of constraints and number of parameters. It's too strict and refuses to solve optimization problems where the # of parameters equals the # of constraints, which is the case with indirect methods. We should write an Augmented Lagrange Multiplier method, maybe driven by SQP. The idea is then pseudospectral and collocation are driven by ALM. SciPy's solvers will then not directly "see" any of the constraints since the constraints are adjoined with new parameters. Another benefit from this is that, when we use direct methods, the output numerical value of the lagrange multipliers should match that of the costates from indirect methods.

[msparapa]

Update on this. I made my own SQP method which is available here. I did this for a few reasons

1. The FORTRAN77-based SciPy code doesn't output KKT multipliers. It can though some modification of their _minimize_slsqp routine, but would require us to carry around that modified function OR get the changes published upstream in SciPy itself which, based on some emails I've found from the devs, isn't likely.

• Note on this: Since KKT multipliers will eventually be output, we could potentially use this to get a "close enough" guess for initial costate values for when we hop over to indirect methods.

2. The SQP code I wrote allows for constraints imposed on the dual (KKT) space. In indirect methods, the dual space is just the costates, but in direct methods, the KKT multipliers take on the values from the costates (up to some scaling) because of the covector mapping principle. From what I've seen, the Ross-Fahroo direct methods require this feature to implement the closure conditions, which are basically just the additional hamiltonian + costate boundary conditions from indirect methods. I have no idea how well this feature works. The Ross-Fahroo methods don't even seem like true direct methods though. In my opinion they're a hybrid method.

3. Lots of SQP and QP solvers throw a fit if you have num_params < num_constraints, or even num_params = num_constraints. Makes sense for the QP subproblem but I don't know why SQP solvers throw a fit. The SQP I wrote does LU decomposition on the linearized equality constraints and only activates the linearly independent constraints. This is to try and ALWAYS have a full rank set of constraints in the QP sub problem. This feature prevented us from using the same NLP solver definitions for direct and indirect methods since with indirect methods, we usually have equivalent number of constraints and parameters.