CircleConstraint ignored

[FedeClaudi]

Hi,

Thanks for making all these packages freely available, they're amazing.

I've starting looking through the code to get an idea of how to use it. The idea is to eventually solve a minimum time maneuvering problem with a bicycle model going through the a race track. For now, I've started looking at the various examples/problems scripts changing things to get an idea of how the different pieces fit together. However, I'm having an issue were a set of CircleConstraint items get ignored and the solver's solution goes right through the obstacle.

I use this function to generate the obstacles

function make_constraints(n)
    # constraints
    r = 0.1
    s1 = 10

    circles_escape = NTuple{3,Float64}[]
    for y in range(2,stop=3,length=s1)
        push!(circles_escape,(5.,y,r))
    end

    n_circles_escape = length(circles_escape)

    circles_escape
    x,y,r = collect(zip(circles_escape...))
    x = SVector{n_circles_escape}(x)
    y = SVector{n_circles_escape}(y)
    r = SVector{n_circles_escape}(r)

    return CircleConstraint(n,x,y,r)
end

and this to generate the optimization problem:

function BicycleCar(scenario=:parallel_park, ;N=101)
    # Model
    model = RobotZoo.BicycleModel()
    n,m = size(model)

    opts = SolverOptions(
        iterations=5000,
        penalty_initial=1e3,
    )

    # Scenario
    tf = 1.0
    x0 = SA_F64[2.5, 2.5, 0, 0]
    xf = SA_F64[7.5, 2.5, 0, 0]

    # Objective
    Q = Diagonal(SA[1,1,1e-2,1e-2])
    R = Diagonal(SA[1e0,1e0])
    Qf = Diagonal(SA_F64[1,1,1,1])*100
    obj = LQRObjective(Q,R,Qf,xf,N)

    # Collision constraints
    obs = make_constraints(n)

    # Constraints
    bnd = [ 20, 20, Inf, deg2rad(180)]
    bnd = BoundConstraint(n, m, x_min=-bnd, x_max=bnd)

    cons = ConstraintList(n,m,N)
    add_constraint!(cons, obs, 2:N-1)
    add_constraint!(cons, bnd, 1:N-1)
    add_constraint!(cons, GoalConstraint(xf), N)

    # Problem
    prob = Problem(model, obj, xf, tf, x0=x0, constraints=cons)
    initial_controls!(prob, SA[-0.1,0.0])
    rollout!(prob)

    return prob, opts
end

and then to solve and plot the results:

prob, opts = BicycleCar()
altro = ALTROSolver(prob, opts, infeasible=false)
solve!(altro)

Everything works fine, the solver converges and find a solution that matches the goal and start states, but it ignores the boundaries completely:

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: 16
    Solve Time: 2.929289 (ms)

  Covergence
    Terminal Cost: 17.16872651645381
    Terminal dJ: 8.698917989846677e-5
    Terminal gradient: 9.356137321932584e-5
    Terminal constraint violation: 7.453802197687764e-7
    Solve Status: SOLVE_SUCCEEDED

Am I missing something in how to add this kind of constraints to the problem, or is there some option in the solver that I should be changing?

Any help would be greatly appreciated!

[FedeClaudi]

Adding some more info.

I can make the dubin car escape problem almost work if I pass infeasible=true to the solver (although the solution is always identical to the initial guess, even when I change it, which makes me think it's not actually solving anything).

However for the bicycle model toy problem infeasible=true remains stuck at the start:

I've tried playing around with most of the parameters but it didn't affect the results in any significant way.

Thanks,
Fede

[FedeClaudi]

For reference, solving with ipopt gives a solution that goes around the obstacle, even though it's not great and it's very slow.

using Ipopt
using MathOptInterface
const MOI = MathOptInterface


# Copy problem to avoid modifying the original problem
prob_nlp = copy(prob)

# Add the dynamics and initial conditions as explicit constraints
TrajectoryOptimization.add_dynamics_constraints!(prob_nlp)

# Reset our initial guess
initial_controls!(prob_nlp, U₀)
rollout!(prob_nlp)

# Create the NLP
nlp = TrajOptNLP(prob_nlp, remove_bounds=false, jac_type=:vector);

optimizer = Ipopt.Optimizer()
optimizer.options = Dict("max_iter" => 20000, "print_level" => 4)

TrajectoryOptimization.build_MOI!(nlp, optimizer)
MOI.optimize!(optimizer)
MOI.get(optimizer, MOI.TerminationStatus())

X = hcat(Vector(states(nlp))...,)
U = hcat(Vector(controls(nlp))...,)

# plot stuff
plt = plot(X[1, :], X[2, :], xlabel="x position", ylabel="y position", markershape=:circle, label="bike traj")
plot!(plt, prob.constraints, label="goal")

display(
    plt
)

[zhao-mz]

I have some questions about solving the minimum time problem with Altro. Does Altro's "problem" structure provide an interface for delta_h? I don't seem to have found it. Have you solved your minimum time problem?