Potential roadmap for increasing OCP solve features

[brocksam]

Just dropping some thoughts on things that could potentially be implemented in opty to improve (computational) performance and solutions. I've proposed these in an order that I think would make sense to work on them. Hopefully this acts as a starter for some discussion. If we agree that any of these are of interest we can open separate issues.

A. Add support the Trapezoidal method

opty currently supports the Backwards Euler and Midpoint methods. Both of these are implicit Runge-Kutta methods based on a specific set of quadrature points related to the Legendre polynomials; the Backwards Euler method is the first method from the Radau IIA family and the Midpoint method is the first method from the Gauss I family. Both of these are collocation schemes. In simple terms this means that the numerical method ensures that the approximating quadrature matches both the value of the underlying time function (state or control) and its derivative. The collocation condition (matching the derivative at the quadrature points) is enforced in different places for different schemes. The Backwards Euler method enforces the collocation condition at only the RHS of each stage while the Midpoint method enforces it at only the midpoint of each stage.

A third family of related collocation schemes is the Lobatto IIIA family. It's based on Lobatto quadrature points and enforces the collocation condition at both the LHS and RHS of each stage. The lowest order Lobatto IIIA method is the Trapezoidal method. Implementing this would be relatively straightforward as we could reuse the majority of the Backwards Euler method code. This would close #412.

B. Generalise constraints/Jacobian functions

opty currently formulates the constraints and Jacobian of the OCP differently depending on which integration method is selected. This means that we can't reuse cached C code if the integration method changes. It also potentially limits the ability to change the uniformity of discretisation in the future, e.g. have stages of different widths of stages using different orders of collocation method. This is important for efficient discretisation and mesh refinement, which could significantly improve computational performance for larger problems, i.e. allowing wide stages for smooth parts of the solution but making the mesh more dense where the solution is more nonlinear.

Currently, the Jacobian is formed by constructing the collocation constraint for a generic stage symbolically and then differentiating this with respect to the various variables in the problem. Instead, we could differentiate just the equations of motion (and any other equations that form the constraint equations of the OCP) with respect to all of the types of variable in the OCP and then use sparse matrix operations to transform this into the required Jacobian. The approach is detailed in Patterson, Michael A., and Anil V. Rao. "Exploiting sparsity in direct collocation pseudospectral methods for solving optimal control problems." Journal of Spacecraft and Rockets 49.2 (2012): 354-377.

C. Add support for higher-order Gauss I, Radau IIA (and Radau IA), and Lobatto IIIA methods

Higher order collocation methods are more computationally efficient than lower order ones as they allow the state/control of the OCP to be approximated to the same accuracy with fewer discretisation points. Implementing the first few higher order methods for each family of Legendre-Gauss quadrature would be relatively straightforward as the coefficients are well known:

• Gauss I: https://en.wikipedia.org/wiki/List_of_Runge–Kutta_methods#Gauss–Legendre_methods
• Radau IIA: https://en.wikipedia.org/wiki/List_of_Runge–Kutta_methods#Radau_IIA_methods
• Radau IA: https://en.wikipedia.org/wiki/List_of_Runge–Kutta_methods#Radau_IA_methods
• Lobatto IIIA: https://en.wikipedia.org/wiki/List_of_Runge–Kutta_methods#Lobatto_IIIA_methods

If the constraint/Jacobian functions have been generalised, it would also be possible to implement even higher-order methods by programmatically generating the coefficients. The maths for this is well described in Butcher, John Charles. Numerical methods for ordinary differential equations. John Wiley & Sons, 2016.

D. Add support for exact second-order mode by computing the Lagrangian Hessian

IPOPT is significantly more efficient and stable if it can operate in second-order mode and use an exact Hessian. For an OCP, this required formulating and evaluating the Lagrangian Hessian for the problem. This can be pretty expensive as it requires taking an additional derivative and this is for an expression that already contains the Jacobian. However, this can be done efficiently by exploiting sparsity using the approach described again by Patterson, Michael A., and Anil V. Rao. "Exploiting sparsity in direct collocation pseudospectral methods for solving optimal control problems." Journal of Spacecraft and Rockets 49.2 (2012): 354-377.

E. Add support for computing mesh error after a successful NLP solve

A discretisation is only useful if it accurately approximates the state and control of an OCP. This can only really be determined by analysing the solution once a transcribed OCP has been numerically solved. There are lots of different approaches to determining the accuracy of a transcription that have been proposed. I think the simplest and most intuitive is the one described by Patterson, Michael A., William W. Hager, and Anil V. Rao. "A ph mesh refinement method for optimal control." Optimal Control Applications and Methods 36.4 (2015): 398-421. It looks at the solution and sees how well a polynomial of degree one greater than that used in the initial discretisation would approximate the solution for each stage. Doing something like this would open the door for implementing mesh refinement in the future.

[moorepants]

This is all very attractive and, yes, I'd be interested improving opty with these ideas. Opty is an implementation of the simplest collocation method based on me reading Ton's various implementations he shared with me versus me studying the state of the art in collocation methods. I keep using it because I understand it, versus it being the best tool for the job. Deciding if expanding opty is a good idea to pursue, begs the question of whether I should study your dissertation and pycollo, and then give my efforts there. For example, is figuring out how to get pycollo to work with big EoMs a shorter solution path to get all what you list above for free?

[moorepants]

On "A. Add support for the trapezoidal method".

I think my current implementation may not allow methods that require evaluating the differential equations more than once at a node. Trapezoidal integration is:

$$ y(t_{i+1}) = y(t_i) + (t_{i+1} - t_i) \frac{1}{2} [f(t_i, y(t_i)) + f(t_{i+1}, y(t_{i+1}))] $$

I'm not sure evaluation of f() with two different arguments works. Looks like I tried to figure out the trapezoidal method in #16 but abandoned it as I didn't see how it fit with the design. opty is, at least partially, fast because I never invert the mass matrix.

[moorepants]

I played around with the trapezoidal rule equation a bit today and I do not see a way to map it back to a differential equation of the form $f(\dot{y}, y, t) = 0$. There will always be an inverted mass matrix present if there are more than one $f()$ in the equation. I'm doubting further that the opty approach can work with any higher order integration methods. We rely on never inverting the mass matrix to make the differential equations explicit and this is a large reason why the evaluation of the equations of motion and the Jacobian can be performant for very large equations of motion.

[tvdbogert]

Yes, the trapezoidal method does not seem to be applicable to implicit differential equations.

A few other thoughts:

1. B.J. Fregly has been working with GPOPS (by Anil Rao, whom Sam cited), which can do higher order collocation methods but requires explicit dynamics. He found a way to use GPOPS with implicit dynamics $f(x,\dot{x},u)=0$. If I remember correctly, introduce an additional trajectory variable $y = \dot{x}$. The explicit system dynamics is then simply: $\dot{x} = y$, which makes y an unknown input. Then you add a path constraint based on the original system dynamics: $f(x,y,u) = 0$. It almost seems too simple. You may be able to find details in his publications. I am not 100% sure that this gives you an actual benefit of higher order integration.
2. You can certainly get higher order transcriptons for dx/dt, for instance, central difference, which would transcribe the implicit dynamics like this: $f(x,\dot{x},u)=0 \rightarrow f(x_k,\frac{x_{k+1} - x_{k-1}}{2h},u_k)=0 $. This would be a 2nd order method. There are formulas with more points too, e.g. https://en.wikipedia.org/wiki/Five-point_stencil.
3. You could also consider collocation methods that can be directly applied to second-order implicit dynamics, avoiding the velocity states. For example: $f(x,\dot{x},\ddot{x},u)=0 \rightarrow f(x_k,\frac{x_{k+1} - x_{k-1}}{2h},\frac{x_{k+1}-2x_k+x_{k-1}}{h^2},u_k)=0 $

These multipoint methods will require some additional work to deal with boundary conditions and periodicity conditions. In the case of periodicity, it is straightforward. For initial and final conditions (if you have them), you would probably need a lower order collocation at the first and last few time steps.

The 3-point and 5-point formulas are symmetric in time, which also has the advantage of energy conservation. You may have seen that Backward Euler introduces artificial damping, because it's not symmetric in time. See Noether's theorem for a mathematical justification.

Most of my experience has been with gait models. Ground reaction forces can change very quickly, which always leads me to think that lower order methods are best. In my experience, ODE solving with explicit dynamics tends to be faster with low order methods (e.g. Matlab RK methods ODE23 vs ODE45) for such highly nonlinear systems.

Disclaimer: I am not an expert on collocation methods. Betts' book may have some useful information.