How to add constant value to a simple MPC Problem

Specific Domains Optimization (Mathematical)
, ,
1

The following is a code that takes the current position & velocity of a box and returns the optimized trajectory of position, velocity, and acceleration. The box is placed in a 2D frictionless map. I want to add a gravity constant to the code: a constant -9.81 value on the y-axis. So I added two constraints. It seems to not work.

@constraint(model, [i=1:num_time_steps, j=[1]], -max_acceleration_Thr <= acceleration[j, i] <= max_acceleration_Thr)
	
@constraint(model, [i=1:num_time_steps, j=[2]], -(max_acceleration_Thr+accel_g) <= acceleration[j, i] <= (max_acceleration_Thr-accel_g))

The following is the original code.

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

    Δt = 0.1
    num_time_steps = 10 # Change this -> Affects Optimization
    max_acceleration_Thr = 10
    accel_g = 9.81

    @variables model begin
        position[1:2, 1:num_time_steps]
        velocity[1:2, 1:num_time_steps]
        -max_acceleration <= acceleration[1:2, 1:num_time_steps] <= max_acceleration
    end

    # Dynamics constraints
    # Here is where I added the new constraints
    @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
    @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)

    optimize!(model)
    return value.(position), value.(velocity), value.(acceleration)
end;

begin
	# The robot's starting position and velocity
	q = [1.0, 0.0]
	v = [-1.0, 1.0]
	Δt = 0.1
	
	anim = @animate for i in 1:80
	    # Plot the current position
	    plot([q[1]], [q[2]], marker=(:hex, 10), xlim=(-1.1, 1.1), ylim=(-1.1, 1.1))
	    
	    # Run the MPC control optimization
	    q_plan, v_plan, u_plan = run_mpc(q, v)
	    
	    # Draw the planned future states from the MPC optimization
	    plot!(q_plan[1, :], q_plan[2, :], linewidth=5)
	    
	    # Apply the planned acceleration and simulate one step in time
	    u = u_plan[:, 1]
	    global v += u * Δt
	    global q += v * Δt
	end
	gif(anim, "~/Downloads/mpc2.gif", fps=15)
end
2

You need to change the variable bounds, not add new constraints. Do something like this:

    accel_offset = [0.0, -accel_g]
    @variables(model, begin
        position[1:2, 1:T]
        velocity[1:2, 1:T]
        -max_acceleration + accel_offset[i] <= acceleration[i=1:2, 1:T] <= max_acceleration + accel_offset[i]
    end)
3

This sets bounds for both x and y components of acceleration right?
I want to have different variable bounds for them.
I tried the following, and it says It already has acceleration as variable.

-max_acceleration <= acceleration[1, 1:T] <= max_acceleration

-max_acceleration + accel_offset[i] <= acceleration[2, 1:T] <= max_acceleration + accel_offset[i]
4

My approach does what you want. You can see it by calling lower_bound.(acceleration) and upper_bound.(acceleration).

The key is to note the i in:

-max_acceleration + accel_offset[i] <= acceleration[i=1:2, 1:T] <= max_acceleration + accel_offset[I]

It results in

-max_acceleration + 0.0 <= acceleration[1, 1:T] <= max_acceleration + 0.0

for i=1 and

-max_acceleration - accel_g <= acceleration[2, 1:T] <= max_acceleration - accel_g

for i=2.