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: .. raw:: html
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. .. raw:: html
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 ---------------- .. list-table:: Where each status comes from and what to inspect :header-rows: 1 :widths: 20 24 30 26 * - 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 ---------------------- .. raw:: html

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.