How to implement this formulation symbolically (eg. in CasADi)

[aditya-shirwatkar]

Hi, great work!
Sorry if this sounds too naive, I'm trying to implement this trajectory optimization formulation that you proposed in python using the CasADi framework which requires the problem to be written symbolically.

But I don't understand how to apply the dynamics and kinematics constraint. Since we have a set of polynomials for the end effector and forces whose durations keep changing, How can we apply the constraints at equal intervals in time?

Suppose I add a constraint on a symbolic polynomial "Pi" with duration "Ti" at time t = ti (ti < Ti). But after few iterations during optimization the durations of the polynomial changes such that (ti > Ti). Then the polynomial on which the constraint should be enforced becomes "Pi+1".
Now since the framework is symbolic we can't do the changes? So how should one tackle the problem?

In the below image, the black dashed lines represent the constraint. And since the durations change and so does the polynomial at which the constraint is applicable, I'm not able to understand how to implement this in the symbolic framework

[awinkler]

Hi! That's a good question that I unfortunately don't have a direct answer to. You're right, the polynomial that's affecting the constraint changes as the duration get optimized. Therefore, you can't really know in advance which optimization variables affect this constraint, anyone of the polynomials might. See here the discrete id being calculated:

towr/towr/src/node_spline.cc

Line 66 in e125b35

std::tie(id, t_local) = GetLocalTime(t_global, GetPolyDurations());

(Side note: only for the dynamic and range-of-motion constraint that are equally spaced in time. Many other constraints are directly enforced at the polynomial junctions, and move together as the durations change, so those should be easier to formulate symbolically).

So I'm not sure how you could implement this symbolically, but the function above that calculates this discrete ID would be the one to focus on. Maybe there's a way to say: All node values might affect the specific constraint at time t, just the affect of those nodes far away from the constraint are zero?

[Mathieu-Geisert]

Hi,
On the same subject, I think this problem already exists with IPOPT. According to my tests and the IPOPT documentation, the structure of the Jacobian needs to be constant [1] and IPOPT starts to give weird results when a constraint switches from one spline to the other.
@awinkler Do you initialize IPOPT with a specific structure somewhere? If not, I'd suggest to add non-zero values in the Jacobian for the n next/previous splines w.r.t the active one at least during the IPOPT initialization.

[1] "Once defined, this nonzero structure MUST remain constant for the entire optimization procedure. This means that the structure needs to include entries for any element that could ever be nonzero,
not only those that are nonzero at the starting point." https://projects.coin-or.org/Ipopt/browser/stable/3.2/Ipopt/doc/documentation.pdf?format=raw

[awinkler]

Yes, you're right, I just checked the code to find where I'm tackling this issue already. This is because IPOPT requires the sparsity structure beforehand, see this discussion. They way it's currently solved is very generously initializing all elements to be non-zero; so similar to what you suggested @Mathieu-Geisert. I'm assuming this makes it less performant, but at least won't mess up the algorithmic structure.

towr/towr/src/phase_durations.cc

Line 153 in e125b35

return jac.sparseView(1.0, -1.0);

On a more general note, optimizing over the durations remains tricky. Convergence is slower from my experience and not as robust, so there's definitely room for investigation and discussion on how to set up this structure correctly.