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

The starting position is set as [1.0, 0.0], and the desired position is [-1,0]. However, the object stops short before touching the desired position. The x coordinate gets close enough, but the issue is with the y coordinate. Why is this happening? Any issue with the @NLobjective?

``````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
-max_acceleration_Thr <= 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

qs_x = []
qs_y = []
as_x = []
as_y = []
angs = []

anim = @animate for i in 1:100 # 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[1]
push!(as_x, u[1])
push!(as_y, u[2])
push!(angs, ang_plan[1])

# Apply the planned acceleration and simulate one step in time
global v += u * Δt
global q += v * Δt
end
end
``````

You have a mis-match between your constraints and the simulation. Here you use the velocity in time `i-1` to update the position.

But here you use the updated velocity.

``````global v += u * Δt
global q += v * Δt
``````

You can see this because the orange line (the optimization) looks good, but the blue box doesn’t follow the orange line.

I’m not sure if I understood your comment correctly.
Let me state what I think is happening.

1. Optimization is done with the information of current position.
2. The function returns optimized position, velocity, acceleration, and angle.
3. The first value of planned(optimized) acceleration is used to update the velocity and position of the blue box, updating the “current position.”
4. goto #1 with this new “current position.”

If I only run the optimization once, and let the blue box follow that optimized path, it would follow the yellow curve exactly, but since I’m running optimization after every frame and updating the velocity & position with the optimized acceleration at that frame, the blue box wouldn’t follow the yellow path exactly?

Also, this may be irrelevant to this question, but I noticed that if I change the lower bound of acceleration of thrust to 0, which it should be. I noticed that the acceleration in y-axis goes beyond the its maximum value. The acceleration in y-axis should not exceed the accel_g, yet it does… and now, blue box reaches the desired location.

``````0 <= accel_Thr[1:num_time_steps] <= max_acceleration_Thr
``````

In the optimization, the position is updated using the velocity in the previous time-step. In the simulation, the position is updated in the current time-step. So your simulation is not following the trajectory of the optimization.

Try swapping the order of the updates:

``````global q += v * Δt
global v += u * Δt
``````

MPC runs in every frame using the “current” position and velocity; the drone’s velocity & position are updated using the optimized acceleration from MPC. But the issue I’m having is that the acceleration in y-axis exceeds lunar gravity.

I tried swapping the order, but unfortunately, it didn’t do much.

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
``````