simple trajectory optimization seems to fail with inequality constraint

[HiroIshida]

Problem

With the help of the quickstart.jl, I defined a simple path-planning problem in configuration space (not in the phase space). The snippets in pasted in the last of this issue. From the result of this optimization, the solver seems to be aware of the inequality constraint, but it just jumps over the circle, which usually does not happens with other trajectory optimization algorithms (like the one usign SQP). The resulting path is shown in below for your information.

After that, I noticed that maybe I have to put some input constraints because the formulation of the DDP is different from the ordinal trajectory optimization as input is involved, so I added the following constraint (please de-comment-out the corresponding part in the snippet).

u_bnd = [0.1, 0.1]
bnd = BoundConstraint(n,m, u_min=-u_bnd, u_max=u_bnd)
add_constraint!(conSet, bnd, 1:N-1)

However the result is like

It would be really helpful if you point out the mistakes in my snippets to obtain a proper result.

Finally, I'm really impressed the result reported in the ICRA paper. Releasing a such a well-structured open-source software must take a lot of effort, and I appreciate a lot to your effort.

Snippet to reproduce this issue

using Altro
using TrajectoryOptimization
using StaticArrays, LinearAlgebra
using RobotDynamics
using PyPlot

struct PointModel <: AbstractModel end
function RobotDynamics.dynamics(model::PointModel, x, u)
    return u
end
size(model::PointModel) = (2, 2)
RobotDynamics.state_dim(model::PointModel) = 2
RobotDynamics.control_dim(model::PointModel) = 2
model = PointModel()
n,m = size(model)

N = 21
tf = 5.
dt = tf/(N-1)

x0 = @SVector zeros(n)
xf = @SVector ones(n)
u0 = @SVector fill(0.0,m)
U0 = [u0 for k = 1:N-1]

Q = 0.0*Diagonal(@SVector ones(n))
Qf = 0.0*Diagonal(@SVector ones(n))
R = 10.0 * Diagonal(@SVector ones(m))
obj = LQRObjective(Q,R,Qf,xf,N)

circle = TrajectoryOptimization.CircleConstraint(2, [0.5], [0.5], [0.2])

conSet = ConstraintList(n,m,N)
goal = GoalConstraint(xf)
add_constraint!(conSet, goal, N)
add_constraint!(conSet, circle, 1:N)

# with input constraint
#u_bnd = [0.1, 0.1]
#bnd = BoundConstraint(n,m, u_min=-u_bnd, u_max=u_bnd)
#add_constraint!(conSet, bnd, 1:N-1)

prob = Problem(model, obj, xf, tf, x0=x0, constraints=conSet)
initial_controls!(prob, U0)

opts = SolverOptions(
    cost_tolerance_intermediate=1e-2,
    penalty_scaling=10.,
    penalty_initial=1.0
)
altro = ALTROSolver(prob, opts)
@time solve!(altro);
X = states(altro)
U = controls(altro)

xs = [x[1] for x in X]
ys = [x[2] for x in X]
scatter(xs, ys, c=:blue, s=10)
plt.gcf().gca().add_artist(plt.Circle((0.5, 0.5), 0.2, fill=false))

[zacmanchester]

I think you've managed to hit a few implementation/non-convexity quirks with your problem setup by making everything perfectly symmetric. Try using a slightly different initial guess. Random noise should work. you could also just put in a fixed initial u that is not in the [1, 1] direction to break the symmetry of the problem. That should encourage finding a solution that goes around the circle rather than right through the middle.

[HiroIshida]

Thanks for your reply. I made some modification in the previous code including breaking-symmetry and random noise in the initial guess of the control input. Now, the path successfully avoids the obstacle, however the path is not yet shortest path. (latter half of the path can be improved) It would be grateful if you could give me advice to obtain a optimal path.

using Altro
using TrajectoryOptimization
using StaticArrays, LinearAlgebra
using RobotDynamics
using PyPlot

struct PointModel <: AbstractModel end
function RobotDynamics.dynamics(model::PointModel, x, u)
    return u
end
size(model::PointModel) = (2, 2)
RobotDynamics.state_dim(model::PointModel) = 2
RobotDynamics.control_dim(model::PointModel) = 2
model = PointModel()
n,m = size(model)

N = 101
tf = 5.
dt = tf/(N-1)

x0 = @SVector zeros(n)
xf = @SVector ones(n)
u0 = SVector{2, Float64}([0.0, 0.001] + rand(2)*0.01)
U0 = [u0 for k = 1:N-1]

Q = 0.0*Diagonal(@SVector ones(n))
Qf = 0.0*Diagonal(@SVector ones(n))
R = 100.0 * Diagonal(@SVector ones(m))
obj = LQRObjective(Q,R,Qf,xf,N)

r = 0.3
x = 0.3
y = 0.6
circle = TrajectoryOptimization.CircleConstraint(2, [x], [y], [r])

conSet = ConstraintList(n,m,N)
goal = GoalConstraint(xf)
add_constraint!(conSet, goal, N)
add_constraint!(conSet, circle, 1:N)

prob = Problem(model, obj, xf, tf, x0=x0, constraints=conSet)
initial_controls!(prob, U0)

opts = SolverOptions(
    cost_tolerance_intermediate=1e-2,
    penalty_scaling=10.,
    penalty_initial=1.0
)
altro = ALTROSolver(prob, opts)
@time solve!(altro);
X = states(altro)
U = controls(altro)

xs = [x[1] for x in X]
ys = [x[2] for x in X]
scatter(xs, ys, c=:blue, s=10)
plt.gcf().gca().add_artist(plt.Circle((x, y), r, fill=false))

[zacmanchester]

It looks like you're not solving to very tight tolerances. In particular, setting cost_tolerance_intermediate to such a large value means the solver will return before it has reached a very low cost. Try the default settings.

[HiroIshida]

Thanks! Changing cost_tolerance_intermediate must be the first thing I should have tried. Sorry for this silly question. Changing the tolerance to the default value, and tuning the R scaling (to 0.01) finally fixes the problem.