I've been trying to learn how to solve simple acceleration-limited trajectory planning problems. I'm working in C++ and I've been using the Eigen library to do linear systems solving. I'm doing the nonlinear-programming part myself because that's part of what I want to learn.
Here's a blog post with some animated GIFs that show the sort of problem I'm working with. It's a one-dimensional trajectory made of two cubic segments, so the start, midpoint and endpoint positions are fixed, the start and end velocities are fixed, and there's a limit on maximum acceleration. The free variables are the durations of the two segments, and the midpoint velocity. I'm trying to minimize the total time of the maneuver.
For movement in a line like this the acceleration constraints can be broken down into separate constraints on minimum and maximum acceleration at each end of the two cubics, for a total of eight constraints. This works pretty well; I wrote an interior-point solver (using the Boyd and Vandenberghe book) and it seems to converge reliably.
I want to extend the problem to two dimensions, which would make the acceleration limit be a disc. For a first step I stayed in the one-dimensional problem, but replaced my eight constraints with four constraints based on squared acceleration at the segment endpoints.
However, with the set of constraints based on squared acceleration it's pretty easy to come up with problem states that the solver won't be able to make progress from. I think this is because the constraint gradient info is not helpful if you're on the side of the constraint function where the objective moves away from the constraint edge? It's also possible I'm just getting my math wrong.
Is there a better way to pose and solve this type of problem? Perhaps I should be using an active-set method, so the problematic constraints aren't in effect until the solution gets over to the side where they should apply?