Attempt at Adding Obstacle Avoidance Constraints with Alpaqa (Errors)

[gabriellewitt]

Hello, I have a question regarding Alpaqa. I am currently utilizing this package for my master thesis, wherein I am using Alpaqa as the base for my path planner for robot navigation. Currently, I have been building my planner through your Inverted Pendulum example, for python.

I am currently trying to include obstacle avoidance constraints into my double integrator script, and have received many errors where the problem statement does not include the constraints at all, or includes the constraints but does not avoid the obstacle adequately enough.

I am looking for help with fixing these errors. My script is below:

# Define obstacle parameters and distance buffer
x_obs, y_obs = 3, 3
r_obs = 0.5
dist_buffer = 0.2 

# Define the obstacle avoidance constraint for each step in the horizon
mpc_obstacle_constraints = []
for k in range(N_horiz):
    x_k = mpc_x[0, k]
    y_k = mpc_x[1, k]
    # Updated to include distance buffer in the avoidance constraint
    safe_distance = r_obs + dist_buffer
    distance_sq = (x_k - x_obs) ** 2 + (y_k - y_obs) ** 2
    mpc_obstacle_constraints.append(distance_sq - safe_distance**2)  # Avoiding (radius + buffer)^2

# Stack constraints as a single vector
mpc_obstacle_constraints_expr = cs.vertcat(*mpc_obstacle_constraints)

# Define the lower and upper bounds as 1D arrays
D_lower = np.zeros(mpc_obstacle_constraints_expr.shape[0])  # Ensure shape (N_horiz,)
D_upper = np.inf * np.ones(mpc_obstacle_constraints_expr.shape[0])  # Ensure shape (N_horiz,)
D = (D_lower, D_upper)

import alpaqa
from alpaqa import minimize

print("Line 99")
# Generate C code for the cost function, compile it, and load it as an
# alpaqa problem description:
problem = (
    alpaqa.minimize(mpc_cost, cs.vec(mpc_u))  # objective and variables
    .subject_to_box(C)  #                       box constraints on the variables
    .subject_to(mpc_obstacle_constraints_expr, D)
    .with_param(mpc_param, param_0)  #          parameters to be changed later
    .with_name(f"double_integrator_Nov4b_{N_horiz}")  # name used in generated files
).compile(sym=cs.MX.sym)

If additional script is needed, please let me know.

[tttapa]

Hello, what's the exact behavior you're seeing? Does the solver converge? The constraints are nonconvex, so local minima and saddle points are possible. Could you share the full script so I can reproduce the issue myself? (You can add it as an attachment if it's too large.)

[gabriellewitt]

Hi there, thank you so much for your quick reply! I am attaching an image of the graphical representation of what the path constructs.

I'll also attach the full script below. The solver does converge, but as you can (hopefully if the photo uploads) see, the path does not correctly avoid the obstacle. It begins the avoidance manuever too late, while it is already inside the obstacle.

from typing import cast
import casadi as cs
import numpy as np

# State vector -------- POS AND VEL
x: cs.SX = cast(cs.SX, cs.SX.sym("x"))  # Cart position x                   [m]
y = cs.SX.sym("y")  # Cart position y                   [m]
v = cs.SX.sym("v")  # Cart velocity x                     [m/s]
w = cs.SX.sym("w")  # Cart velocity x                     [m/s]
state = cs.vertcat(x, y, v, w)  # Full state vector
nx = state.shape[0]  # Number of states

# Input ------- ACC_X AND ACC_Y
ux = cs.SX.sym("ux")  # External force applied to the model [N]
uy = cs.SX.sym("uy")  # External force applied to the model [N]
control = cs.vertcat(ux, uy)  # Full control vector
nu = control.shape[0]  # Number of inputs

# Parameters
Ts = 0.1  #       Simulation sampling time             [s]
N_horiz = 50  #       MPC horizon                          [time steps]
N_sim = 50  #       Simulation length                    [time steps]
u_max_x = 2.0  #       Maximum control input (acceleration)
u_max_y = 2.0  #       Maximum control input (acceleration)
# u_min_x, u_min_y = -2.0  # Minimum control input (acceleration)
control_max = cs.vertcat(u_max_x, u_max_y)

# Continous-time dynamics
x_dot_x = v
x_dot_y = w
v_dot_x = ux
v_dot_y = uy
f_c = cs.Function("f_c", [state, control], [cs.vertcat(v, w, ux, uy)])  # ---------vertcat() is the derivatives of the state vector
print(f_c)
# 4th order Runge-Kutta integrator - stays the same
k1 = f_c(state, control)
k2 = f_c(state + Ts * k1 / 2, control)
k3 = f_c(state + Ts * k2 / 2, control)
k4 = f_c(state + Ts * k3, control)

# Discrete-time dynamics - stays the same
f_d_expr = state + (Ts / 6) * (k1 + 2 * k2 + 2 * k3 + k4)
f_d = cs.Function("f", [state, control], [f_d_expr])


# %% Model predictive control

# MPC inputs and states
mpc_x0 = cs.MX.sym("x0", nx)  # Initial state
print(mpc_x0.shape)
mpc_u = cs.MX.sym("u", (2, N_horiz))  # Inputs
mpc_x = f_d.map(N_horiz)(mpc_x0, mpc_u)  # Simulated states

# MPC cost
Q = cs.MX.sym("Q", nx)  # Stage state cost
Qf = cs.MX.sym("Qf", nx)  # Terminal state cost
R = cs.MX.sym("R", nu)  # Stage input cost
s, u = cs.MX.sym("s", nx), cs.MX.sym("u", nu)
xref = cs.MX.sym("xref", nx)
xref = (5.5, 5, 0, 0)
# sin_s = cs.vertcat(cs.sin(s[0] / 2), s[1:])  # Penalize sin(θ/2), not θ

stage_cost_x = cs.Function("lx", [s, Q], [cs.dot(s, cs.diag(Q) @ s)])
print(stage_cost_x)
terminal_cost_x = cs.Function("lf", [s, Qf], [cs.dot(s, cs.diag(Qf) @ s)])
print(terminal_cost_x)
stage_cost_u = cs.Function("lu", [u, R], [cs.dot(u, cs.diag(R) @ u)])
print(stage_cost_u)
# print(cs.diag(Q)) #HAVE THESE VALUES PRINT DURING THE CALLING OF THE SOLVER; MAKE SURE TO MAKE N_SIM SHORTER SO IT DOESNT PRINT A BILLION LINES
# print(cs.diag(Qf))
# print(cs.diag(R))

# subtract xref and the value
temp = mpc_x[:, :-1]
mpc_error = xref - temp
mpc_param = cs.vertcat(mpc_x0, Q, Qf, R)
mpc_y_cost = cs.sum2(
    stage_cost_x.map(N_horiz - 1)(mpc_error, Q)
)  # put here(mpc_x[:, :-1], Q)        #The sum of all individual stage costs associated at each time step, minus the final step
mpc_u_cost = cs.sum2(stage_cost_u.map(N_horiz)(mpc_u, R))  # The sum of all individual input costs associated at each time step
mpc_terminal_cost = terminal_cost_x(mpc_x[:, -1], Qf)  # Cost of the state at the final step
mpc_cost = mpc_y_cost + mpc_u_cost + mpc_terminal_cost  # Sum total of mpc costs across entire N_horiz

# CONSTRAINTS

# Define obstacle parameters and distance buffer
x_obs, y_obs = 3, 3
r_obs = 0.5
dist_buffer = 0.2

# Define the obstacle avoidance constraint for each step in the horizon
mpc_obstacle_constraints = []
for k in range(N_horiz):
    x_k = mpc_x[0, k]
    y_k = mpc_x[1, k]
    # Updated to include distance buffer in the avoidance constraint
    safe_distance = r_obs + dist_buffer
    distance_sq = (x_k - x_obs) ** 2 + (y_k - y_obs) ** 2
    mpc_obstacle_constraints.append(distance_sq - safe_distance**2)  # Avoiding (radius + buffer)^2

# Stack constraints as a single vector
mpc_obstacle_constraints_expr = cs.vertcat(*mpc_obstacle_constraints)

# Define the lower and upper bounds as 1D arrays
D_lower = np.zeros(mpc_obstacle_constraints_expr.shape[0])  # Ensure shape (N_horiz,)
D_upper = np.inf * np.ones(mpc_obstacle_constraints_expr.shape[0])  # Ensure shape (N_horiz,)
D = (D_lower, D_upper)

# Box constraints on the actuator force:
temp = (2, N_horiz)
C = -control_max * np.ones(temp), +control_max * np.ones(temp)

# Initial state of the system
state_0 = np.array([0, 0, 0, 0])

# Initial parameter value
Q = np.array([10, 10, 1, 1])
Qf = np.array([10, 10, 0, 0])
R = np.array([0.01, 0.01])
param_0 = np.concatenate((state_0, Q, Qf, R))

# Compile into an alpaqa problem
import alpaqa

# from alpaqa import minimize

print("Line 99")
# Generate C code for the cost function, compile it, and load it as an
# alpaqa problem description:
problem = (
    alpaqa.minimize(mpc_cost, cs.vec(mpc_u))  # objective and variables
    .subject_to_box(C)  #                       box constraints on the variables
    .subject_to(mpc_obstacle_constraints_expr, D)
    .with_param(mpc_param, param_0)  #          parameters to be changed later
    .with_name(f"double_integrator_Nov4b_{N_horiz}")  # name used in generated files
).compile(sym=cs.MX.sym)
print("Line 108")
from datetime import timedelta

# Configure an alpaqa solver:
Solver = alpaqa.PANOCSolver
inner_solver = Solver(
    panoc_params={
        "max_time": timedelta(seconds=Ts),
        "max_iter": 200,
        "stop_crit": alpaqa.PANOCStopCrit.ProjGradUnitNorm2,
    },
    direction=alpaqa.StructuredLBFGSDirection(
        lbfgs_params={"memory": 15},
        direction_params={"hessian_vec_factor": 0},
    ),
)
alm_params = {
    "tolerance": 1e-4,
    "initial_tolerance": 1e-4,
}
solver = alpaqa.ALMSolver(
    alm_params=alm_params,
    inner_solver=inner_solver,
)

print("Line 133")
# %% Controller class


# Wrap the solver in a class that solves the optimal control problem at each
# time step, implementing warm starting:
class MPCController:
    def __init__(self, problem: alpaqa.CasADiProblem):
        self.problem = problem
        self.tot_it = 0
        self.tot_time = timedelta()
        self.max_time = timedelta()
        self.failures = 0
        self.u = np.ones(problem.n)

    def __call__(self, state_n):
        state_n = np.array(state_n).ravel()
        # Set the current state as the initial state
        self.problem.param[:nx] = state_n
        # Shift over the previous solution by one time step
        self.u = np.concatenate((self.u[nu:], self.u[-nu:]))
        # Solve the optimal control problem
        # (warm start using the shifted previous solution)
        self.u, _, stats = solver(self.problem, self.u)  # ------------- CALLS ON THE SOLVER HERE -> THIS IS WHERE WE INCLUDE CONSTRAINTS
        # Print some solver statistics
        print(
            f"{stats['status']!s:24} iter={stats['inner']['iterations']:4} "
            f"time={stats['elapsed_time']} "
            f"failures={stats['inner_convergence_failures']} "
            f"tol={stats['ε']:1.3e} stepsize={stats['inner']['final_γ']:1.3e}"
        )
        print(self.u)
        self.tot_it += stats["inner"]["iterations"]
        self.failures += stats["status"] != alpaqa.SolverStatus.Converged
        self.tot_time += stats["elapsed_time"]
        self.max_time = max(self.max_time, stats["elapsed_time"])
        # Return the optimal control signal for the first time step
        return self.u[:nu]


# %% Simulate the system using the MPC controller

# Simulation
mpc_states = np.empty((nx, N_sim + 1))
mpc_inputs = np.empty((nu, N_sim))
mpc_states[:, 0] = state_0
controller = MPCController(problem)
for n in range(N_sim):
    # Solve the optimal control problem:
    mpc_inputs[:, n] = controller(mpc_states[:, n])
    # Apply the first optimal control input to the system and simulate for
    # one time step, then update the state:
    mpc_states[:, n + 1] = f_d(mpc_states[:, n], mpc_inputs[:, n]).T

print(f"{controller.tot_it} iterations, {controller.failures} failures")
print(f"time: {controller.tot_time} (total), {controller.max_time} (max), " f"{controller.tot_time / N_sim} (avg)")

import matplotlib.pyplot as plt
import matplotlib.patches as patches

# Time array
ts = np.arange(N_sim + 1) * Ts

import matplotlib.pyplot as plt
import matplotlib.patches as patches
import numpy as np

# Example obstacle definitions (each obstacle defined as [x, y, radius])
obstacles = np.array(
    [
        [3, 3, 1],  # obstacle 1: center at (3,3) with radius 1 # obstacle 2: center at (6,6) with radius 0.5
    ]
)

# Buffer distance for safety around each obstacle
dist_buffer = 0.2  # increase radius by 0.2 meters for buffer

# Time array for plotting
ts = np.arange(N_sim + 1) * Ts

# Plot x vs y position as a 2D trajectory
plt.figure(figsize=(8, 6))
plt.plot(mpc_states[0, :], mpc_states[1, :], label="Trajectory", color="blue")

# Plot each obstacle as a circle with radius including buffer
for obs in obstacles:
    obs_x, obs_y, obs_radius = obs
    total_radius = obs_radius + dist_buffer  # Total radius including buffer
    obstacle_circle = patches.Circle((obs_x, obs_y), total_radius, color="red", alpha=0.3, edgecolor="black", linestyle="--")
    plt.gca().add_patch(obstacle_circle)

# Set plot details
plt.xlabel("Position $x$ [m]")
plt.ylabel("Position $y$ [m]")
plt.title("2D Trajectory of the Cart with Obstacles")
plt.legend(["Trajectory"])
plt.grid(True)
plt.axis("equal")
plt.show()

[gabriellewitt]

double_integrator_Nov4b.txt
Here is the file attachment form of the script!

[tttapa]

Thanks, I believe there's a problem with the simulation of the states:

mpc_x = f_d.map(N_horiz)(mpc_x0, mpc_u)  # Simulated states

This computes the vector $\big(f_d(x_0, u_0), f_d(x_0, u_1) ... f_d(x_0, u_{N-1})\big)$, whereas you want mapaccum to compute the composition of the dynamics.
By using map instead of mapaccum, the problem is essentially an OCP with horizon one, so once the obstacle comes into view, it's too late to brake, and the problem becomes infeasible. You can see in the output that the solver failed for some of the time steps (you may want to print stats["δ"] to check the constraint violation). Changing map to mapaccum, the solves succeed, and I get the following result:

When adding constraints, it's best to keep the ALM initial tolerance large enough. You may also want to increase the initial_penalty parameter.