setting multiple non-convex constraint in the same problem

[Jushan-Randy-Chen]

Hi,

The dubin car tutorial is very helpful, and I am trying to run a centralized three-quadrotor trajectory optimization problem based on that template. In my case, I have a large state vector with the states of all three quadrotors (same case for the input). Each drone has 6 states and 3 inputs, so the combined state is 18-dimensional and the input is 9-dimensional. However, I am unsure about how to set the inter-drone collision constraint. I need three non-convex constraints in the case of three quadrotors since there is a minimum allowed distance between each drone. Should I just include all three constraints into a list like the following?:

# nonconvex obstacle cosntraint:
r_0 = 0.5
drone_width = 0.2
Δr_0 = drone_width/2
E_xy = [1 0 0;0 1 0; 0 0 1]

s(t, x, u, p) = [(r_0+Δr_0)^2-(E_xy*x[0:3]-E_xy*x[6:9])'*(E_xy*x[0:3]-E_xy*x[6:9]);
                 (r_0+Δr_0)^2-(E_xy*x[0:3]-E_xy*x[12:15])'*(E_xy*x[0:3]-E_xy*x[12:15]);
                 (r_0+Δr_0)^2-(E_xy*x[6:9]-E_xy*x[12:15])'*(E_xy*x[6:9]-E_xy*x[12:15])];

Moreoever, I am trying to implement quadratic tracking cost as the objective function:

Q = I(18) * 5
R = I(9) * 0.1
Qf = I(18) * 100
Γ(x, u, p) = (x-x_f)'*Q*(x-x_f) + u'*R*u;
ϕ(x,p) = (x-x_f)'*Qf*(x-x_f)


wrap(func) = (t, k, x, u, p, pbm) -> func(x, u, p)
problem_set_running_cost!(pbm, alg, wrap(Γ))
problem_set_terminal_cost!(pbm,alg, wrap(ϕ))

But I keep getting this error for the terminal cost:

MethodError: no method matching problem_set_terminal_cost!(::TrajectoryProblem, ::Symbol, ::var"#19#20"{typeof(ϕ)})
Closest candidates are:
  problem_set_terminal_cost!(::TrajectoryProblem, ::Union{Nothing, Function}) at /mnt/c/Users/Randy666/Documents/projects/ICON_lab/SCPToolbox.jl/src/parser/problem.jl:365

Stacktrace:
 [1] top-level scope
   @ In[40]:3
 [2] eval
   @ ./boot.jl:373 [inlined]
 [3] include_string(mapexpr::typeof(REPL.softscope), mod::Module, code::String, filename::String)
   @ Base ./loading.jl:1196

[sandeepkumar575]

I think the error is in usage of the function problem_set_terminal_cost! ? There is no 'alg' argument input needed for this function. Had to figure it out from one of the examples in the repository.