In simulation problems, we have always one stage, and
problem_options.impose_terminal_phyisical_time = 1;
problem_options.stagewise_clock_constraint = 1;
lead to the very same result, but we may have the same terminal constraint twice (also one relaxed, other as hard, depending on the relaxation settings, hence once we run the integrator, problem_options.stagewise_clock_constraint should always be false.

Looks fine except the few minor aestetics comments. As mentioned earlier, having a minimal two contacts example (like the OCP with two discs, described in some of the examples) would be helpful to verify correctness.

I added the three cart problem as a test of the multicontact implementation. It seems to work.

In simulation problems, we have always one stage, and problem_options.impose_terminal_phyisical_time = 1; problem_options.stagewise_clock_constraint = 1; lead to the very same result, but we may have the same terminal constraint twice (also one relaxed, other as hard, depending on the relaxation settings, hence once we run the integrator, problem_options.stagewise_clock_constraint should always be false.

Yep this is now checked in Options.preprocess

Thanks for addressing all my comments. We are almost ready to merge.

I have a few final comments:

• simulation: Gauss Legendre leads to infeasible problems both with Stewart and Heaviside and both elastic and inelastic impacts, with GL size of algebraic e.g. theta and t_grid not consistent.
• on discs example Heaviside not-lifted much slower than lifted (are the initializations consistent - if yes, then just the nonlinearity makes the difference)
• disc example Heaviside with n_s = 1 breaks the plotting for some reason.
• tf_simulation example and inelastic impacts, the plots of the the algebraic variables is quite strange (I tried Radau, as it converges)

• simulation: Gauss Legendre leads to infeasible problems both with Stewart and Heaviside and both elastic and inelastic impacts, with GL size of algebraic e.g. theta and t_grid not consistent.

The bug that was causing infeasibility is fixed however size mismatch is expected as we do not include the rhs theta/alpha as a variable in the mpcc.

• on discs example Heaviside not-lifted much slower than lifted (are the initializations consistent - if yes, then just the nonlinearity makes the difference)

Initialization for $\alpha$ is consistent between both.

• disc example Heaviside with n_s = 1 breaks the plotting for some reason.

With heuristic step equilibration the solver finds a solution where each other h is zero. This breaks our plotting code.

• tf_simulation example and inelastic impacts, the plots of the the algebraic variables is quite strange (I tried Radau, as it converges)

Does not seem strange to me. the theta multipliers and beta seem to be correct.