Non Linear MPC optimization of a 2 dimensional drone

I am trying to simulate a drone on a 2-dimensional lunar surface. The drone can apply thrust the z-axis of the body, and the drone can change the angle of its body from -90 degrees to +90 degrees. The first planned acceleration in the y direction that the MPC function gives is a negative value that exceeds the the lunar accel_g, which I set to be 1.635 m/s^2; thus, the drone cancels out the initial velocity really quickly. This should not happen since I set the constraints of body angle in such that the thrust will never be able to reduce the vertical velocity: vertical velocity of the drone should be reduced only by the lunar gravity. I can not find what is wrong with the code. ** is there a way I can apply rotation to the marker of the plot? I want to change the cross marker so that it can represent the changes in attitude. **

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 = 3 # Max Thrust / Mass
max_pitch_angle = 90
accel_g = 1.635 # 1/6 of Earth G
des_pos = [-1,0]

@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[1:num_time_steps] <= max_pitch_angle
    0 <= accel_Thr[1:num_time_steps] <= max_acceleration_Thr
end

# Dynamics constraints
@NLconstraint(model, [i=2:num_time_steps, j=[1]], acceleration[j, i] == accel_Thr[i-1]*sind(angle[i-1]))

@NLconstraint(model, [i=2:num_time_steps, j=[2]], acceleration[j, i] == (accel_Thr[i-1]*cosd(angle[i-1]))-accel_g)

@NLconstraint(model, [i=2:num_time_steps, j=1:2],
            velocity[j, i] == velocity[j, i - 1] + (acceleration[j, i - 1]) * Δt)
@NLconstraint(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)
@NLobjective(model, Min, 
    100 * sum((des_pos[i]-position[i, num_time_steps])^2 for i in 1:2)+ sum(velocity[i, num_time_steps]^2 for i in 1:2))

# Initial conditions:
@NLconstraint(model, [i=1:2], position[i, 1] == initial_position[i])
@NLconstraint(model, [i=1:2], velocity[i, 1] == initial_velocity[i])
@NLconstraint(model, angle[1] == initial_angle)

optimize!(model)
return value.(position), value.(velocity), value.(acceleration), value.(angle[2:end])

end;

begin
# The robot's starting position and velocity
q = [1.0, 0.0]
v = [-2.0, 2.0]
ang = 45
Δt = 0.1

# Recording Position, Acceleration, Attitude, Planned Positions
qs_x = []
q开发者_运维问答s_y = []
as_x = []
as_y = []
angs = []
q_plans = []
u_plans = []

anim = @animate for i in 1:90 # This determies the number of MPC to be run
    # Plot the current position & Attitude
    plot(label = "Drone",[q[1]], [q[2]], marker=(:rect, 10), xlim=(-2, 2), ylim=(-2, 2))
    plot!(label = "Body Axis",[q[1]], [q[2]], marker=(:cross, 18, :grey))
    push!(qs_x,q[1])
    push!(qs_y,q[2])
    
    # 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!(label = "Opt. Path", q_plan[1, :], q_plan[2, :], linewidth=5, arrow=true, c=:orange)
    # Draw the planned acceleration
    plot!(label = "Opt. Accel",u_plan[1, 1:2], u_plan[2, 1:2], linewidth=3, arrow=true, c=:red)
    
    # Save Acceleration & Angle Data to csv
    u = u_plan[:, 1]
    push!(as_x, u[1])
    push!(as_y, u[2])
    push!(angs, ang)
    push!(u_plans, u_plan)
    
    # Apply the planned acceleration&Attitude and simulate one step in time
    global ang = ang_plan[1]
    global v += u * Δt
    global q += v * Δt
end
gif(anim, "~/Downloads/NLmpc_angle.gif", fps=60)

end