Exercises: Constrained Optimization

Chapter 04: Constrained Optimization

Self-assessment guide: Focus on understanding KKT conditions and formulating real robotics problems as constrained optimization. The math is less important than the modelling skill.

Section A — Conceptual Questions

A1. State the KKT conditions for the problem:

$$\min_x f(x) \quad \text{s.t.} \quad g_i(x) \leq 0, \quad h_j(x) = 0$$

Then apply them to: $\min_{x,y} x^2 + y^2$ subject to $x + y = 1$.

A2. Explain the difference between a penalty method and a barrier method. When would you choose each?

A3. A robot arm has joint limits: $-\pi \leq q_i \leq \pi$ for each joint $q_i$. You want to minimize the distance to a target. Formulate this as a constrained optimization problem and list which solver type (LP, QP, NLP) applies.

Section B — Computation Problems

B1. Implement the penalty method for:

$$\min_{x,y} (x - 3)^2 + (y - 2)^2 \quad \text{s.t.} \quad x + y \leq 3, \quad x \geq 0, \quad y \geq 0$$

import numpy as np
from scipy.optimize import minimize

def objective(xy):
    x, y = xy
    return (x - 3)**2 + (y - 2)**2

def penalty_objective(xy, mu):
    """
    Objective + penalty for constraint violations.
    Constraints: x+y <= 3, x >= 0, y >= 0
    """
    x, y = xy
    cost = objective(xy)
    # TODO: add penalty terms for each constraint
    # Penalty for g(x) <= 0: (mu/2) * max(0, g(x))^2
    pass

# Solve with increasing penalty parameter
# TODO: loop mu from 1 to 1e6, solve each, report convergence

import numpy as np
from scipy.optimize import minimize

def objective(xy):
    x, y = xy
    return (x - 3)**2 + (y - 2)**2

def penalty_objective(xy, mu):
    x, y = xy
    cost = objective(xy)
    # g1: x + y <= 3  →  x + y - 3 <= 0
    cost += (mu / 2) * max(0, x + y - 3)**2
    # g2: x >= 0  →  -x <= 0
    cost += (mu / 2) * max(0, -x)**2
    # g3: y >= 0  →  -y <= 0
    cost += (mu / 2) * max(0, -y)**2
    return cost

x0 = np.array([0.0, 0.0])
for mu in [1, 10, 100, 1000, 10000]:
    result = minimize(lambda xy: penalty_objective(xy, mu), x0, method='Nelder-Mead')
    x, y = result.x
    print(f"mu={mu:6d}: x={x:.4f}, y={y:.4f}, "
          f"cost={objective(result.x):.4f}, x+y={x+y:.4f}")
    x0 = result.x  # warm start

# Expected: converges to (2, 1) or close — the unconstrained minimum (3,2)
# violates x+y <= 3, so the solution is on the constraint boundary.

B2. Use scipy.optimize.linprog to solve a simple resource allocation:

A warehouse robot can carry boxes of type A (weight 2kg, value 5) or type B (weight 3kg, value 7). Capacity: 12kg. Max 3 of type A, max 4 of type B. Maximize total value.

from scipy.optimize import linprog

# linprog minimizes c^T x, so negate for maximization
c = [-5, -7]  # negate values

# Inequality constraints: A_ub @ x <= b_ub
A_ub = [
    [2, 3],     # weight constraint: 2a + 3b <= 12
]
b_ub = [12]

# Bounds for each variable
x_bounds = [(0, 3), (0, 4)]  # 0 <= a <= 3, 0 <= b <= 4

result = linprog(c, A_ub=A_ub, b_ub=b_ub, bounds=x_bounds, method='highs')
print(f"Type A: {result.x[0]:.1f}, Type B: {result.x[1]:.1f}")
print(f"Total value: {-result.fun:.1f}")

# Note: LP gives fractional solutions. For integer solutions, 
# need integer programming (scipy doesn't support this directly).

Section C — OKS Connection

C1. The OKS MPC controller needs to compute velocity commands $(v, \omega)$ that: - Track a reference path (minimize tracking error) - Respect velocity limits: $|v| \leq v_{\max}$, $|\omega| \leq \omega_{\max}$ - Respect acceleration limits: $|\dot{v}| \leq a_{\max}$, $|\dot{\omega}| \leq \alpha_{\max}$ - Avoid obstacles (distance $\geq d_{\min}$)

1. Classify each constraint as linear or nonlinear
2. What optimization problem type is this? (LP/QP/SOCP/NLP)
3. Why is the obstacle constraint the hardest?