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