How to model a 2 dimensional drone using JUMP, and lpopt

I want to model a drone? (consider a drone) that has a fixed thrust. It needs to control its body angle and thrust value to balance itself against the gravity to arrive at the desired destination. I might have made some mistakes in the code, and its making error. The following is a diagram of the model, and the code. # Side question: Should I use non linear? I don’t know how to use NLconstraint…

MethodError: no method matching length(::UndefInitializer)

function run_mpc(initial_position, initial_velocity, initial_angle)
    
    model = Model(Ipopt.Optimizer)

    Δt = 0.1
    num_time_steps = 20 # Change this -> Affects Optimization
    max_acceleration_Thr = 10 # Max Thrust / Mass
	max_pitch_angle = 90
	accel_g = 1.635 # 1/6 of Earth G

    @variables model begin
        position[1:2, 1:num_time_steps]
        velocity[1:2, 1:num_time_steps]
		acceleration[1:2, 1:num_time_steps]
		-max_pitch_angle <= angle[undef, 1:num_time_steps] <= max_pitch_angle
		-max_acceleration_Thr <= accel_Thr[undef, 1:num_time_steps] <= max_acceleration_Thr
    end

    # Dynamics constraints
	@constraint(model, [i=1:num_time_steps, j=[1]], acceleration[j, i] == accel_Thr[i-1]*sin(angle))
	
	@constraint(model, [i=1:num_time_steps, j=[2]], acceleration[j, i] == accel_Thr[i-1]*cos(angle)-accel_g)
	
    @constraint(model, [i=2:num_time_steps, j=1:2],
                velocity[j, i] == velocity[j, i - 1] + (acceleration[j, i - 1]) * Δt)
    @constraint(model, [i=2:num_time_steps, j=1:2],
                position[j, i] == position[j, i - 1] + velocity[j, i - 1] * Δt)

    # Cost function: minimize final position and final velocity
	# For Moving to [-2,0] with min. vertical velocity,
	# sum(([-2,0]-position[:, end]).^2)+ sum(velocity[[2], end].^2)
    @objective(model, Min, 
        100 * sum(position[:, end].^2)+ sum(velocity[:, end].^2))

    # Initial conditions:
    @constraint(model, position[:, 1] .== initial_position)
    @constraint(model, velocity[:, 1] .== initial_velocity)
	@constraint(model, angle[1] .== initial_angle)

    optimize!(model)
    return value.(position), value.(velocity), value.(acceleration), value.(angle)
end;
begin
	# The robot's starting position and velocity
	q = [1.0, 0.0]
	v = [-2.0, 2.0]
	ang = 45
	Δt = 0.1

	qs_x = []
	qs_y = []
	as_x = []
	as_y = []
	angs = []
	
	anim = @animate for i in 1:80 # This determies the number of MPC to be run
	    # Plot the current position
		# Attitude of the vehicle is not considered
	    plot([q[1]], [q[2]], marker=(:rect, 10), xlim=(-2, 2), ylim=(-2, 2))
		push!(qs_x,q[1])
		push!(qs_y,q[2])
		plot!([q[1]], [q[2]], marker=(:cross, 18, :grey))
	    
	    # Run the MPC control optimization
	    q_plan, v_plan, u_plan, ang_plan = run_mpc(q, v, ang)
	    
	    # Draw the planned future states from the MPC optimization
	    plot!(q_plan[1, :], q_plan[2, :], linewidth=5, arrow=true, c=:orange)
	    
	    # Save Acceleration & Angle Data to csv
	    u = u_plan[:, 1]
		global ang = ang_plan
		push!(as_x, u[1])
		push!(as_y, u[2])
		push!(angs, ang_plan)
		
		# Apply the planned acceleration and simulate one step in time
	    global v += u * Δt
	    global q += v * Δt
	end
	gif(anim, "~/Downloads/mpc_halt_maneuver.gif", fps=60)
end

Should I use non linear? I don’t know how to use NLconstraint…

Yes. If your equations are not linear, e.g., they contain sin(theta), then you must use @NLconstraint.

The JuMP documentation has a section on nonlinear optimization: Nonlinear Modeling · JuMP.

There are also tutorials. Here’s a similar one about the optimal control of a rocket:
https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/rocket_control/

MethodError: no method matching length(::UndefInitializer)

This is coming from angle[undef, 1:num_time_steps]. Why is there undef there? You likely just need angle[1:num_time_steps].

Just an aside, be careful here as you probably want sind(angle) since your angles appear to be in degrees, not radians.

1 Like

I used angle[undef, 1:num_time_steps], and it’s giving me

BoundsError: attempt to access 20-element Vector{JuMP.VariableRef} at index [0]

I’m guessing there’s an issue with angle and accel_Thr?
So I changed the angle to angle[I-1] in the @NLconstraint.
But I still get the same error message.
The rest of the error message is the following.

getindex@array.jl:861[inlined]
macro expansion@Other: 1834[inlined]
(::Main.workspace#6.var"#6#15"{Vector{JuMP.VariableRef}, Vector{JuMP.VariableRef}, Matrix{JuMP.VariableRef}, JuMP.Model})(::Int64, ::Int64)@Other: 304
#44@container.jl:114[inlined]
iterate@generator.jl:47[inlined]
collect(::Base.Generator{JuMP.Containers.VectorizedProductIterator{Tuple{Base.OneTo{Int64}, Vector{Int64}}}, JuMP.Containers.var"#44#45"{Main.workspace#6.var"#6#15"{Vector{JuMP.VariableRef}, Vector{JuMP.VariableRef}, Matrix{JuMP.VariableRef}, JuMP.Model}}})@array.jl:724
map@abstractarray.jl:2878[inlined]
container@container.jl:114[inlined]
container@container.jl:75[inlined]
macro expansion@Other: 2372[inlined]
run_mpc(::Vector{Float64}, ::Vector{Float64}, ::Int64)@Other: 20
macro expansion@Local: 23[inlined]
macro expansion@Local: 238[inlined]
top-level scope@Local: 14

BoundsError: attempt to access 20-element Vector{JuMP.VariableRef} at index [0]

You have:

@constraint(model, [i=1:num_time_steps, j=[1]], acceleration[j, i] == accel_Thr[i-1]*sin(angle))

Here i=1:num_time_steps, but you’re calling accel_Thr[i-1]. When i=1, this is calling accel_Thr[0], which doesn’t exist.

Perhaps you meant

@constraint(model, [i=2:num_time_steps, j=[1]], acceleration[j, i] == acceleration[j, i-1] + accel_Thr[i-1]*sin(angle[i-1]))

In no case should you use undef inside a JuMP macro.