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
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.jlof aRobotZoo.DubinsCartrying to reach the origin (code below). The run ofAltro.solve!reportsSOLVE_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 defaultsolver.opts.constraint_tolerance == 1.0e-6, I am surprised that the solver claims that the solve was successful. I guess that theBoundsConstraintthat 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 expectALTROto 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?
Trajectory for tight bounds on u
Trajectory for relaxed bounds on u (abs(u) <= 10)