Query solver success

[lassepe]

First of all, thank you for sharing your code for Altro.jl. I am really excited to have a fast pure Julia solver for trajectory opimization.

I set up a simple toy example with TrajectoryOptimizaiton.jl of a RobotZoo.DubinsCar trying to reach the origin (code below).
The run of Altro.solve! reports SOLVE_SUCCEDED. At the same time, however, the terminal constraint violation is quite high and the resulting path does not seem dynamically feasible (see first trajectory below). Since I have the default solver.opts.constraint_tolerance == 1.0e-6, I am surprised that the solver claims that the solve was successful. I guess that the BoundsConstraint that I set above renders the problem infeasible; at least with relaxed bounds yield a more realistic trajectory. (see second trajectory below). Thus, I don't expect ALTRO to be able to solve this problem, I only need to know that it was not able to solve it.

TL;DR How can I query the solution success in the sense of KKT conditions?

---

import Altro
import Random
import RobotZoo
using LinearAlgebra: Diagonal
using TrajectoryOptimization:
    BoundConstraint, ConstraintList, GoalConstraint, LQRObjective, Problem, add_constraint!
using StaticArrays: SA
using Altro: ALTROSolver
using VegaLite: @vlplot

model = RobotZoo.DubinsCar()
n, m = size(model)
N = 20
tf = 1.0

x0 = SA[1.0, 1.0, 0.0]
xf = SA[0.0, 0.0, 0.0]

Q = Diagonal(SA[1.0, 1.0, 1.0])
R = Diagonal(SA[1.0, 1.0])

objective = LQRObjective(Q, R, Q, xf, N)
constraints = ConstraintList(n, m, N)
add_constraint!(constraints, GoalConstraint(xf), N)
add_constraint!(constraints, BoundConstraint(n, m; u_min = -1, u_max = 1), 1:(N - 1))

problem = Problem(model, objective, xf, tf; x0, constraints)
solver = ALTROSolver(problem)

Altro.solve!(solver)

X = Altro.states(solver)
U = Altro.controls(solver)

[(; x = x[1], y = x[2], heading = x[3]) for x in X] |>
@vlplot(
    width = 800,
    height = 800,
    x = "x:q",
    y = "y:q",
    angle = {"heading:q", scale = {domain = [-pi, pi], range = [270, -90]}}
) +
@vlplot(mark = {:line, point = true}) +
@vlplot(mark = {:point, shape = :wedge, color = :red})

Trajectory for tight bounds on u

SOLVE COMPLETED
 solved using the ALTRO Solver,
 part of the Altro.jl package developed by the REx Lab at Stanford and Carnegie Mellon Universities

  Solve Statistics
    Total Iterations: 41
    Solve Time: 10.220067 (ms)

  Covergence
    Terminal Cost: 1.4219497031315713
    Terminal dJ: -0.00017837842054335695
    Terminal gradient: 2.1440340016395658e-10
    Terminal constraint violation: 0.033280018914020215
    Solve Status: SOLVE_SUCCEEDED

Trajectory for relaxed bounds on u (abs(u) <= 10)

  Solve Statistics
    Total Iterations: 27
    Solve Time: 0.819603 (ms)

  Covergence
    Terminal Cost: 4.218872417412097
    Terminal dJ: -2.056174750464379e-7
    Terminal gradient: 0.0023927528035217935
    Terminal constraint violation: 9.743034595527823e-10
    Solve Status: SOLVE_SUCCEEDED