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

**URL:** <https://discourse.julialang.org/t/the-final-position-of-nonlinear-optimization-of-trajectory-is-not-on-the-desired-position/90782>\
**Category:** Optimization (Mathematical)\
**Tags:** question, jump, optimization, nonlinear\
**Created:** [November 24, 2022, 9:46pm UTC](https://discourse.julialang.org/t/the-final-position-of-nonlinear-optimization-of-trajectory-is-not-on-the-desired-position/90782 "2022-11-24T21:46:43Z")\
**Posts on this page:** 7\
**Page:** 1

<div class="post-metadata">

**Author:** ![elliot000319](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/elliot000319/32/36789_2.png) [@elliot000319](https://discourse.julialang.org/u/elliot000319)\
**Post date:** [November 24, 2022, 9:46pm UTC](https://discourse.julialang.org/t/the-final-position-of-nonlinear-optimization-of-trajectory-is-not-on-the-desired-position/90782/1 "2022-11-24T21:46:43Z")

</div>

![NLmpc_angle](https://global.discourse-cdn.com/julialang/original/3X/7/2/725ff8dc119f3a5c89517391061b8bcce38c4ccd.gif)  
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?

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

```

```julia
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
	gif(anim, "~/Downloads/NLmpc_angle.gif", fps=60)
end

```

---

<div class="post-metadata">

**Author:** ![odow](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/odow/32/28685_2.png) [@odow](https://discourse.julialang.org/u/odow)\
**Post date:** [November 24, 2022, 10:40pm UTC](https://discourse.julialang.org/t/the-final-position-of-nonlinear-optimization-of-trajectory-is-not-on-the-desired-position/90782/2 "2022-11-24T22:40:58Z")

</div>

> [@elliot000319](#):
>
> `position[j, i] == position[j, i - 1] + velocity[j, i - 1] * Δt`

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.

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

---

<div class="post-metadata">

**Author:** ![elliot000319](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/elliot000319/32/36789_2.png) [@elliot000319](https://discourse.julialang.org/u/elliot000319)\
**Post date:** [November 25, 2022, 8:05am UTC](https://discourse.julialang.org/t/the-final-position-of-nonlinear-optimization-of-trajectory-is-not-on-the-desired-position/90782/3 "2022-11-25T08:05:10Z")

</div>

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?

---

<div class="post-metadata">

**Author:** ![elliot000319](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/elliot000319/32/36789_2.png) [@elliot000319](https://discourse.julialang.org/u/elliot000319)\
**Post date:** [November 25, 2022, 8:41am UTC](https://discourse.julialang.org/t/the-final-position-of-nonlinear-optimization-of-trajectory-is-not-on-the-desired-position/90782/4 "2022-11-25T08:41:10Z")

</div>

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.

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

```

![NLmpc_angle](https://global.discourse-cdn.com/julialang/original/3X/4/9/4951c8ae01546a5966cee72bfe84f047851efecb.gif)

---

<div class="post-metadata">

**Author:** ![odow](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/odow/32/28685_2.png) [@odow](https://discourse.julialang.org/u/odow)\
**Post date:** [November 26, 2022, 6:06am UTC](https://discourse.julialang.org/t/the-final-position-of-nonlinear-optimization-of-trajectory-is-not-on-the-desired-position/90782/5 "2022-11-26T06:06:39Z")

</div>

> [@elliot000319](#):
>
> I’m not sure if I understood your comment correctly.

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:

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

```

---

<div class="post-metadata">

**Author:** ![elliot000319](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/elliot000319/32/36789_2.png) [@elliot000319](https://discourse.julialang.org/u/elliot000319)\
**Post date:** [December 7, 2022, 6:50am UTC](https://discourse.julialang.org/t/the-final-position-of-nonlinear-optimization-of-trajectory-is-not-on-the-desired-position/90782/6 "2022-12-07T06:50:07Z")

</div>

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.

 ![image](https://global.discourse-cdn.com/julialang/original/3X/4/d/4d137b5fd69f28c019a504849570429d3e4510f4.jpeg)  
I tried swapping the order, but unfortunately, it didn’t do much.

---

<div class="post-metadata">

**Author:** ![odow](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/odow/32/28685_2.png) [@odow](https://discourse.julialang.org/u/odow)\
**Post date:** [December 7, 2022, 7:40pm UTC](https://discourse.julialang.org/t/the-final-position-of-nonlinear-optimization-of-trajectory-is-not-on-the-desired-position/90782/7 "2022-12-07T19:40:22Z")

</div>

Try this:

```julia
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](https://global.discourse-cdn.com/julialang/original/3X/7/2/7228ed5aa8779375237e9ffeb6dd5e0bb72896c6.gif)