Convergence Logic Tree

This page helps trace an AL-iLQR solve from the innermost line-search decision to the final AL-iLQR status. Use it when trajOpt reports a failure, when constraints are satisfied but the inner iLQR status is not Converged, or when a trajectory appears unchanged despite a successful return.

The three layers are:

Inner: line search / forward pass Middle: iLQR subproblem Outer: AL-iLQR constraints

How To Read The Tree

Start at the top of the outer layer. Each AL-iLQR outer iteration calls one iLQR subproblem. Each iLQR iteration repeatedly tries a backward pass and a forward-pass line search while increasing regularization. The forward pass is the only place where the state and control trajectory are actually accepted and overwritten.

Outer Layer

AL-iLQR loop

Goal: satisfy all inequality constraints, then return a trajectory only if the inner solve made real progress or converged.

1. Initialize augmented terms

   Collect each constraint vector c_k. Initialize lambda_aug and mu_aug with the same per-timestep sizes.

2. Call the iLQR subproblem

   Run the middle layer with the current augmented Lagrangian penalties.

   enter middle layer
3. Did iLQR return a non-recoverable failure?

   Current recoverable statuses are MaxIterations and RegularizationExceeded, because either may still leave an improved trajectory.

   Yes

   Return ALILQRStatus::InnerFailed.

   No

   Measure the maximum positive constraint violation.

4. Is max_c <= constraint_tol?

   This is the AL feasibility test. It is checked after every iLQR subproblem, even if the iLQR status is not Converged.

   Yes, and iLQR converged or accepted at least one forward-pass step

   Return ALILQRStatus::Converged. Constraint satisfaction is valid because the trajectory was optimized or the inner cost tolerance was reached.

   Yes, but iLQR accepted zero steps

   Return ALILQRStatus::InnerFailed. This blocks false positives where the warm start was feasible but the optimizer never improved the trajectory.

   No

   Update lambda_aug and mu_aug, then start another AL outer iteration.

5. Outer iteration budget exhausted

   If constraints never satisfy the tolerance, return ALILQRStatus::MaxOuterIterations.

Middle Layer

iLQR subproblem

Goal: locally reduce the augmented trajectory cost by alternating backward and forward passes.

1. Start iLQR iteration

   Set reg = reg_init. Telemetry starts with accepted_steps = 0.

2. Does the backward pass factorize successfully?

   The Riccati step requires a positive-definite regularized Q_uu.

   No

   Increase regularization by reg_scale and retry the same iLQR iteration.

   Yes

   Build feedback gains K, feedforward terms d, and expected cost terms deltaV.

3. Run forward-pass line search

   Try to roll out a new trajectory using K, d, and decreasing line-search step sizes.

   enter inner layer
4. Did the forward pass accept a rollout?
   No

   Increase regularization by reg_scale and retry from the backward pass.

   Yes

   Overwrite X and U. Increment accepted_steps, store last_delta_J, and store final_cost.

5. Is abs(J_prev - J_new) <= cost_tol?
   Yes

   Return ILQRStatus::Converged and true.

   No

   Continue to the next iLQR iteration. The trajectory may still be useful because a forward pass was accepted.

6. Middle-layer terminal statuses

   RegularizationExceeded means all regularization retries failed. MaxIterations means the cost tolerance was not reached before the iLQR iteration budget ended.

Inner Layer

Forward pass and line search

Goal: find one acceptable rollout that decreases cost consistently with the expected model reduction.

1. Try line-search step alpha = 1, 1/2, 1/4, ...

   Initialize X_bar and U_bar from the current nominal trajectory.

2. Does the backward pass predict descent for this step?

   The predicted cost change is dV(alpha) = alpha*dV0 + alpha^2*dV1 from the backward pass. A trial is only meaningful when the model predicts descent (dV(alpha) < -1e-16); when it predicts ascent or no change, the acceptance ratio z flips sign and a cost-increasing rollout could otherwise be accepted.

   No (predicted ascent / degenerate)

   Reject this alpha without rolling out and keep backtracking. If every alpha is rejected this way, the forward pass fails and regularization escalates — persistent ascent predictions usually mean stale or indefinite curvature (check the cost Hessian mode and regularization settings, not the dynamics).

   Yes

   Roll out the trial.

3. Is the rollout dynamically valid?

   Reject the trial if the state is non-finite, the timestep is invalid, dynamics throw, the next state is invalid, or quaternion normalization fails.

   No

   Reject this alpha and try the next smaller step.

   Yes

   Compute nominal cost plus augmented Lagrangian penalty terms.

4. Are augmented-constraint vector sizes consistent?

   The line search requires lambda_aug[k], mu_aug[k], and c_k to have matching sizes.

   No

   Return forward-pass failure immediately.

   Yes

   Evaluate the line-search acceptance ratio.

5. Does the line-search ratio satisfy beta1 <= z <= beta2?

   With ilqr.ls_strict_decrease enabled, acceptance additionally requires a strict merit decrease (J_new < J_prev).

   Yes

   Accept the rollout, overwrite X and U, return forward-pass success.

   No

   Reject this alpha and keep searching.

6. All line-search trials rejected

   Return forward-pass failure. The middle layer will usually increase regularization and retry.

Status Reference

Where each status comes from and what to inspect

Layer	Status or return	Meaning	First places to inspect
Inner	forwardPass == true	A line-search trial accepted and updated the trajectory.	Accepted alpha, J_prev, J_new, deltaV, constraint penalties.
Inner	forwardPass == false	No line-search step was accepted, or augmented constraint vector sizes mismatched.	State validity, dynamics exceptions, beta1/beta2, deltaV, lambda_aug and mu_aug sizes.
Middle	ILQRStatus::Converged	The latest accepted rollout reached abs(J_prev - J_new) <= cost_tol.	last_delta_J and cost_tol.
Middle	ILQRStatus::MaxIterations	iLQR accepted steps but never reached the cost tolerance before max_iters.	accepted_steps, last_delta_J, max_iters, cost scaling.
Middle	ILQRStatus::RegularizationExceeded	Backward or forward pass kept failing until reg > reg_max.	Whether accepted_steps is zero, backward-pass factorization, line-search rejection pattern, reg_init/reg_scale/reg_max.
Outer	ALILQRStatus::Converged	Constraints are within tolerance and the iLQR subproblem either converged or accepted at least one rollout.	max_constraint_violation, accepted_steps, final trajectory quality.
Outer	ALILQRStatus::InnerFailed	The inner solve failed in a way AL-iLQR cannot safely accept, or constraints were satisfied with zero accepted trajectory updates.	Warm-start feasibility, accepted_steps == 0, first backward/forward failure reason.
Outer	ALILQRStatus::MaxOuterIterations	AL penalty updates did not drive constraints below constraint_tol before the outer iteration budget ended.	Constraint component traces, penalty scaling, active constraints, actuator limits.

Common Diagnosis Paths

Constraints satisfied, but iLQR did not converge

This can be acceptable if accepted_steps > 0. It means the trajectory was optimized, but the inner cost-delta tolerance was not the final stopping reason.

Constraints satisfied with zero accepted steps

Treat this as unsafe. The warm start may have been feasible, but the optimizer did not produce a new trajectory.

Regularization exceeded after accepted steps

The latest refinement attempt stalled, but previous accepted steps may still have produced a valid AL solution if constraints are now below tolerance.

Max outer iterations

The middle layer may be working, but AL penalty updates are not enforcing at least one active constraint quickly enough.