Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Query solver success #23

Open
lassepe opened this issue Mar 26, 2021 · 0 comments
Open

Query solver success #23

lassepe opened this issue Mar 26, 2021 · 0 comments

Comments

@lassepe
Copy link

lassepe commented Mar 26, 2021

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

image

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

image

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant