The final position of Nonlinear optimization of trajectory is not on the desired position

Specific Domains Optimization (Mathematical)
, , ,
7

Try this:

using JuMP, Ipopt, Plots

function run_mpc(initial_position, initial_velocity, initial_angle)
    model = Model(Ipopt.Optimizer)
    set_silent(model)
    T, Δt = 20, 0.1
	desired_position = [-1, 0]
    @variables(model, begin
        position[1:2, 1:T]
        velocity[1:2, 1:T]
		acceleration[1:2, 1:T]
		-90 <= angle[1:T] <= 90
		-3 <= thrust[1:T] <= 3
    end)
	@NLconstraints(model, begin
        [t=1:T], acceleration[1, t] == thrust[t] * sind(angle[t])
        [t=1:T], acceleration[2, t] == thrust[t] * cosd(angle[t]) - 1.635
    end)
    @constraints(model, begin
        [t=2:T, j=1:2], velocity[j, t] == velocity[j, t-1] + acceleration[j, t-1] * Δt
        [t=2:T, j=1:2], position[j, t] == position[j, t-1] + velocity[j, t-1] * Δt
        [i=1:2], position[i, 1] == initial_position[i]
        [i=1:2], velocity[i, 1] == initial_velocity[i]
        angle[1] == initial_angle
    end)
    @objective(
        model,
        Min,
        100 * sum((desired_position[i] - position[i, T])^2 for i in 1:2) +
        sum(velocity[i, T]^2 for i in 1:2)
    )
    optimize!(model)
    return value.(position), value.(velocity), value.(acceleration), value.(angle)
end

begin
	q, v, ang = [1.0, 0.0], [-2.0, 2.0], 45
	Δt = 0.1
	anim = @animate for i in 1:100
        # Run MPC
	    q_plan, v_plan, u_plan, ang_plan = run_mpc(q, v, ang)
        # Plot solution
	    plot([q[1]], [q[2]], marker=(:rect, 10), xlim=(-2, 2), ylim=(-2, 2))
		plot!([q[1]], [q[2]], marker=(:cross, 18, :grey))
	    plot!(q_plan[1, :], q_plan[2, :], linewidth=5, arrow=true, c=:orange)
        # Update positions
	    global q .= q_plan[:, 2]
	    global v .= v_plan[:, 2]
		global ang = ang_plan[2]
	end
	gif(anim, "NLmpc_angle.gif")
end

NLmpc_angle

1 Like