What's the purpose of method replaceStageCircular() and cycleProblem()?

[Zionshang]

I noticed that this two methods in mpc example:

aligator/tests/mpc-cycle.cpp

Lines 191 to 192 in 6bd7a8d

	problem.replaceStageCircular(stage);
	ddp.cycleProblem(problem, stage.createData());

I noticed that these two methods are included in every MPC problem, including Simple-Robotics/simple-mpc.

I personally think replaceStageCircular() is used to handle changes in the stage_model more efficiently. But what is the purpose of cycleProblem()? Why doesn't FDDP have such methods?

Additionally, if my stage_model remains unchanged throughout the entire horizon, can these two methods be omitted?

Please forgive me for asking such a simple question, as I have found many examples of using this two methods without commenting on the reason.

[ManifoldFR]

Hello again @Zionshang,

You are right, the purpose of problem.replaceStageCircular(stage) is to inject the stage into problem and update it, while popping off the "head" stage (the first one). It uses the C++ STL's std::rotate under the hood to cycle out the head stage. This is meant to be used in MPC.
Concretely, the problem goes from S[0] - S[1] - ... - S[N-1] to S[1] - S[2] - ... - S[N-1] - S' where S' is the new stage you've added.

The purpose of cycleProblem() is to cycle the workspace and results structs inside of the solver, and update internal data to accommodate for the new stage that's been injected (and the "head" stage being popped out).

It doesn't exist on FDDP for... reasons ? I think this is an oversight actually, whoops. We'll fix it in a future release.

Additionally, if my stage_model remains unchanged throughout the entire horizon, can these two methods be omitted?

You mean, if your problem doesn't change? Once again, when you insert a stage into an aligator problem (TrajOptProblem), it gets copied. There are two way of changing stages on a problem: 1) directly access the stage in problem.stages[index] and modify the stuff you need 2) emplace a new stage and rotate out the first stage (for MPC) problem.replaceStageCircular. If you don't want to do either of these things, you don't need to.

On a personal note, I think overall our API for this is not super clear or particularly ergonomic and I could've done better, it's definitely something we want to fix in the future.

[Zionshang]

@ManifoldFR Thank you for the comprehensive explanation - it's exceptionally clear! (･ω･)ﾉ

I still have a small problem though. o(╥﹏╥)o.

There are two way of changing stages on a problem.

When using Way 1 to change stages, is there a corresponding need to adjust the solver's workspace?

I notice that Way 2 explicitly requires calling cycleProblem() after problem.replaceStageCircular to properly update internal components like results_, workspace_, and linearSolver_. Does this imply that simply modifying stages through Way 1 might leave these critical solver internals in an inconsistent state, potentially leading to optimization failures in subsequent steps?

My concern stems from the observation that Way 1 appears to bypass the explicit reset mechanisms required in cycleProblem() .

[ManifoldFR]

When using Way 1 to change stages, is there a corresponding need to adjust the solver's workspace?

I notice that Way 2 explicitly requires calling cycleProblem() after problem.replaceStageCircular to properly update internal components like results_, workspace_, and linearSolver_. Does this imply that simply modifying stages through Way 1 might leave these critical solver internals in an inconsistent state, potentially leading to optimization failures in subsequent steps?

Yes unfortunately 😬
There's no automatic mechanism at the moment which will adjust the workspace if the user decides to do something like e.g. change a stage's dimensions (add a constraint, etc). There's no shortcut to reallocate a given stage and the corresponding buffers in the workspace and linear solver, either.
It would be great to have all that, you make a good point!

This is a big design issue in the solver.
The "fireproof" way is unfortunately reallocating the entire workspace, results and linear solver which is something I do not advise for real-time applications - although this is something @edantec had to do to the parallel linear solver due to an edge case I didn't consider.

I have an open PR where I revamp the linear solver's memory allocation strategies, but it should definitely be extended to the workspace and results structs to allow users to tear down nodes and recreate them (with new specs/dimensions) while avoiding a runtime malloc() as much as possible (by tapping into a memory arena for instance). I don't know if other solvers in the market have such a feature.

[Zionshang]

Thank you for the thorough explanation and transparency about the current limitations! I completely understand that this issue requires future development efforts.

Looking forward to seeing this enhancement in future releases. Thanks again for your time and expertise in clarifying the situation! (･ω･)ﾉ