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=], -max_acceleration_Thr <= acceleration[j, i] <= max_acceleration_Thr) @constraint(model, [i=1:num_time_steps, j=], -(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], [q], 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