import numpy as np
from scipy.linalg import qr
from typing import Tuple, Dict, Callable
from dataclasses import dataclass, field
from qpsolvers import solve_qp
from mpt4py.base import Matrix, Vector
from mpt4py.functions import AffineFunction
from mpt4py.geometry import Polyhedron, PolyUnion
from mpt4py.solvers.plcp_solver import plcp_solve
from mpt4py.tolerances import tolerances
from mpt4py.solvers.lcp import LCP
[docs]
@dataclass
class PqpSolution:
status: str
regions: PolyUnion
# active_sets: List
# # TODO: add the mapping from theta to objective
[docs]
@dataclass
class QuadraticProgram:
r"""
Defines a multi-parametric Quadratic Program (QP) of the form:
.. math::
\begin{aligned}
\min\limits_x &\quad \frac{1}{2} x^\top Hx + (F \theta+f)^\top x + \theta^\top Y \theta + C\theta + c, \\
\text{s.t.} &\quad A x \leq b + B \theta, \\
& \quad A_e x = b_e + E \theta, \\
& \quad x_{lb} \leq x \leq x_{ub},
\end{aligned}
where :math:`H` and :math:`Y` are positive semi-definite.
The upper and lower bounds are not supported in the parametric case yet.
"""
# QP
H: Matrix = field(default_factory=lambda: np.zeros((0, 0)))
f: Vector = field(default_factory=lambda: np.zeros((0,)))
c: float = field(default_factory=lambda: 0.)
pY: Matrix = field(default_factory=lambda: np.zeros((0, 0)))
pF: Vector = field(default_factory=lambda: np.zeros((0, 0)))
pC: Vector = field(default_factory=lambda: np.zeros((0,)))
# inequalities: A*u <= b + pB*th
A: Matrix = field(default_factory=lambda: np.zeros((0, 0)))
b: Vector = field(default_factory=lambda: np.zeros((0,)))
pB: Matrix = field(default_factory=lambda: np.zeros((0, 0)))
# equalities: Ae*u = be + pE*th
Ae: Matrix = field(default_factory=lambda: np.zeros((0, 0)))
be: Vector = field(default_factory=lambda: np.zeros((0,)))
pE: Matrix = field(default_factory=lambda: np.zeros((0, 0)))
# bounds: lb <= u <= ub
lb: Vector = field(default_factory=lambda: np.zeros((0,)))
ub: Vector = field(default_factory=lambda: np.zeros((0,)))
# parameter set: Ath*th <= bth
Ath: Matrix = field(default_factory=lambda: np.zeros((0, 0)))
bth: Vector = field(default_factory=lambda: np.zeros((0,)))
problem_type: str = field(init=False) # allowed values: 'LP', 'QP'
n: int = field(init=False) # problem dimension
m: int = field(init=False) # number of inequalities
me: int = field(init=False) # number of equality constraints
d: int = field(init=False) # number of parameters
def __post_init__(self):
# make sure the inputs are matrices and vectors
for attr in ['A', 'pB', 'Ae', 'pE', 'H', 'pF', 'pY', 'Ath']:
if np.ndim(getattr(self, attr)) != 2:
raise ValueError(attr + " must be a matrix")
for attr in ['b', 'be', 'f', 'lb', 'ub', 'pC', 'bth']:
if np.ndim(getattr(self, attr)) != 1:
raise ValueError(attr + " must be a vector")
for attr in ['c']:
if not np.isscalar(getattr(self, attr)):
raise ValueError(attr + " must be a scalar")
self.c = float(self.c)
self.n = max(self.H.shape[0], self.f.size, self.A.shape[1], self.Ae.shape[1], self.lb.size, self.ub.size)
self.d = max(self.pF.shape[1], self.pB.shape[1], self.pE.shape[1], self.Ath.shape[1])
self.m = self.A.shape[0]
self.me = self.Ae.shape[0]
if self.A.size == 0 and self.Ae.size == 0 and self.ub.size == 0 and self.lb.size == 0 and self.d == 0:
if self.H.size == 0 and self.f.size == 0:
raise ValueError("Opt: empty problem")
elif self.H.size == 0 and self.f.size:
raise ValueError("Opt: unconstrained problem")
# if the size of H is 0, or all elements in H are almost zero, consider the problem as LP
if np.size(self.H) == 0 or np.all(np.abs(self.H) < tolerances['zero']):
self.problem_type = 'LP'
self.H = np.zeros((0, 0))
if self.f.size != self.n:
raise ValueError("Opt: f must be of size ({},)".format(self.n))
else:
self.problem_type = 'QP'
if self.H.shape != (self.n, self.n):
raise ValueError("Opt: H must be of size ({},{})".format(self.n, self.n))
if self.f.size == 0:
self.f = np.zeros((self.n,))
if self.A.size:
if self.A.shape[1] != self.n:
raise ValueError("Opt: A must have {} columns".format(self.n))
if self.b.shape != (self.m,):
raise ValueError("Opt: b must be of size ({},)".format(self.m))
else:
self.A = np.zeros((self.m, self.n)) # TODO: in MPT3 validate.m line 185, A is set to zeros(m,n), why?
if self.Ae.size:
if self.Ae.shape[1] != self.n:
raise ValueError("Opt: Ae must have {} columns".format(self.n))
if self.be.shape != (self.me,):
raise ValueError("Opt: be must be of size ({},)".format(self.me))
else:
self.Ae = np.zeros((self.me, self.n))
if self.lb.size:
if self.lb.shape != (self.n,):
raise ValueError("Opt: lb must be of size ({},)".format(self.n))
else:
self.lb = -np.inf * np.ones(self.n)
if self.ub.size:
if self.ub.shape != (self.n,):
raise ValueError("Opt: ub must be of size ({},)".format(self.n))
else:
self.ub = np.inf * np.ones(self.n)
# check if lb is actually lower than ub
violated = np.where(self.lb > self.ub)[0].tolist()
if len(violated):
raise ValueError("Opt: Lower bound {} is higher than its upper bound".format(violated))
# check the parametric inputs
for attr, shape in zip(['pF', 'pB', 'pE'], [(self.n, self.d), (self.m, self.d), (self.me, self.d)]):
if np.size(getattr(self, attr)):
if np.shape(getattr(self, attr)) != shape:
raise ValueError("Opt: {} must have shape {}".format(attr, shape))
else:
setattr(self, attr, np.zeros(shape))
if self.pY.size == 0:
self.pY = np.zeros((self.d, self.d))
else:
if self.pY.shape != (self.d, self.d):
raise ValueError("Opt: pY must be of size ({},{})".format(self.d, self.d))
@property
def is_parametric(self) -> bool:
"""Determine if the quadratic program is parametric or not."""
return self.d > 0
@property
def num_ineq(self) -> int:
return self.m
@property
def num_eq(self) -> int:
return self.me
def __str__(self):
if self.problem_type == 'QP':
lines = [
"Quadratic Program",
f"Num variables: {self.n}",
f"Num inequality constraints: {self.m}",
f"Num equality constraints: {self.me}",
]
elif self.problem_type == 'LP':
lines = [
"Linear Program",
f"Num variables: {self.n}",
f"Num inequality constraints: {self.m}",
f"Num equality constraints: {self.me}",
]
else:
raise NotImplementedError
if self.is_parametric:
lines[0] = "Parametric " + lines[0]
lines.append(f"Num parameters: {self.d}")
max_line_length = max(len(line) for line in lines)
dashed_line = "-" * max_line_length
return f"\n{dashed_line}\n" + "\n".join(lines) + f"\n{dashed_line}"
[docs]
def solve(self):
"""Solve the QP/pQP."""
if not self.is_parametric:
# solve the QP with state-of-the-art solver
sol = solve_qp(P=self.H, q=self.f, G=self.A, h=self.b, A=self.Ae, b=self.be,
lb=self.lb, ub=self.ub, solver='clarabel')
return sol
else:
USE_PPOPT = True
if USE_PPOPT:
return solve_pqp_with_ppopt(self)
[docs]
def solve_for_param(self, theta: Vector, **kwargs):
"""
Solve the LCP / pLCP for a specific parameter value
"""
if not self.is_parametric:
raise ValueError("Cannot solve for a specific parameter value for non-parametric LCP.")
else:
# assert theta.shape == (self.d,), f"theta must be of size ({self.d},)"
# return lcp_solve(self.M, self.Q @ theta + self.q, **kwargs)
raise NotImplementedError
[docs]
def qp2lcp(self) -> Tuple[LCP, Callable, Dict[str, Tuple[Matrix, Matrix, Vector]]]:
r"""
Transform LP/QP into LCP formulation, or transform pLP/pQP into pLCP formulation.
The original QP is in the following form:
.. math::
:label: original-qp
\begin{aligned}
\min\limits_x & \quad \frac{1}{2}x^\top Hx + (F \theta + f)^\top x \\
\text{s.t.} & \quad Ax \leq b + B\theta \\
& \quad A_e x = b_e + E\theta
\end{aligned}
There are 3 steps:
1. Eliminate equality constraints in :eq:`original-qp`.
To reduce the optimization problem to a simpler form, the system of linear equations
:math:`A_e x = b_e + E\theta` must be consistent, meaning no linearly dependent rows exist,
and the number of equalities :math:`m_e` is strictly less than the number of variables :math:`n`, i.e., :math:`m_e < n`.
The principle is based on factorizing the equality constraints :math:`A_e x = b_e + E\theta` into
basic variables :math:`x_B` and non-basic variables :math:`x_N`:
.. math::
A_{e,B} x_B + A_{e,N} x_N = b_e + E\theta,
where the index sets :math:`B` and :math:`N` denote the columns for the factored system.
The factored submatrix :math:`A_{e,B}` must be invertible to express basic variables
as a function of non-basic variables:
.. math::
x_{B} = - A_{e,B}^{-1} A_{e,N} x_N + A_{e,B}^{-1} b_e + A_{e,B}^{-1} E\theta.
Using the substitutions:
.. math::
C = - A_{e,B}^{-1} A_{e,N},
D_1 = A_{e,B}^{-1} b_e,
D_2 = A_{e,B}^{-1} E,
the relation between basic and non-basic variables simplifies to:
.. math::
x_B = C x_N + D_1 + D_2 \theta.
The original QP can be expressed with only non-basic variables :math:`x_N` as follows:
.. math::
:label: qp_no_equality
\begin{aligned}
\min\limits_{x_N} &\quad \frac{1}{2}x_N \tilde{H} x_N + (\tilde{F} \theta + \tilde{f})^\top x_N \\
\text{s.t.} & \quad \tilde{A} x_N \leq \tilde{b} + \tilde{B}\theta
\end{aligned}
with the following definitions (:math:`A_B` and :math:`A_N` are the columns of :math:`A` corresponding to :math:`x_B` and :math:`x_N` respectively):
.. math::
\begin{aligned}
& \tilde{H} = C^\top H_{BB} C + C^\top H_{BN} + H_{NB}C + H_{NN}, \\
& \tilde{f} = \frac{1}{2} C^\top \left(H_{BB}+H_{BB}^\top \right) D_1 + \frac{1}{2} \left(H_{BN}^\top + H_{NB}\right) D_1 + C^\top f_{B} + f_{N}, \\
& \tilde{F} = \frac{1}{2} C^\top \left(H_{BB}+H_{BB}^\top \right) D_2 + \frac{1}{2} \left(H_{BN}^\top + H_{NB}\right) D_2 + C^\top F_{B} + F_{N}, \\
& \tilde{A} = A_B C + A_N, \\
& \tilde{b} = b - A_B D_1, \\
& \tilde{B} = B - A_B D_2. \\
\end{aligned}
2. Transform the QP with only inequalities :eq:`qp_no_equality` into affine and non-negative form:
.. math::
:label: qp_non_negative
\begin{aligned}
\min\limits_y & \quad \frac{1}{2}y^\top \hat{H} y + (\hat{F} \theta + \hat{f})^\top y \\
\text{s.t.} & \quad \hat{A}y \geq \hat{b} + \hat{B}\theta \\
&\quad y \geq 0
\end{aligned}
**Case 1**: if :math:`\tilde{A}` is not full row rank, factorize it row-wise into :math:`\tilde{A}_B` and :math:`\tilde{A}_N` where :math:`\tilde{A}_B` is a square invertible matrix:
.. math::
\begin{aligned}
-\tilde{A}_B x_N &= -\tilde{b}_B - \tilde{B}_B \theta + y \\
-\tilde{A}_N x_N &\geq -\tilde{b}_N - \tilde{B}_N \theta \\
y &\geq 0
\end{aligned}
Apply substitution :math:`x_N = \tilde{A}_B^{-1} \left( -y + \tilde{b}_B + \tilde{B}_B \theta \right)` into :eq:`qp_no_equality`,
we can express the problem into the form of :eq:`qp_non_negative` with:
.. math::
\begin{aligned}
& \hat{H} = \tilde{A}_B^{-\top} \tilde{H} \tilde{A}_B^{-1}, &
& \hat{F} = - \hat{H} \tilde{B}_B - \tilde{A}_B^{-\top} \tilde{F}, &
& \hat{f} = - \hat{H} \tilde{b}_B - \tilde{A}_B^{-\top} \tilde{f}, \\
& \hat{A} = \tilde{A}_N \tilde{A}_B^{-1}, &
& \hat{b} = \tilde{A}_N \tilde{A}_B^{-1} \tilde{b}_B - \tilde{b}_N, &
& \hat{B} = \tilde{A}_N \tilde{A}_B^{-1} \tilde{B}_B - \tilde{B}_N.
\end{aligned}
**Case 2**: the :math:`x_N` can be expressed as the difference of two positive vectors,
i.e., :math:`x_N = x_N^+ - x_N^-` with :math:`x_N^+\geq 0, ~x_N^-\geq 0`.
By denoting :math:`y` as their concatenation, we get the corresponding parts in :eq:`qp_non_negative`:
.. math::
\begin{aligned}
& \hat{H} = \begin{bmatrix} \tilde{H} & -\tilde{H} \\ \tilde{H} & \tilde{H} \end{bmatrix}, &
& \hat{F} = \begin{bmatrix} \tilde{F} & -\tilde{F} \end{bmatrix}, &
& \hat{f} = \begin{bmatrix} \tilde{f} & -\tilde{f} \end{bmatrix}, \\
& \hat{A} = \begin{bmatrix} -\tilde{A} & \tilde{A} \end{bmatrix}, &
& \hat{b} = -\tilde{b}, &
& \hat{B} = -\tilde{B}.
\end{aligned}
3. Transform to LCP.
By introducing slack variables :math:`s = \hat{A}y-\hat{b}-\hat{B}\theta`, the KKT condition of :eq:`qp_non_negative` is given by:
.. math::
\begin{aligned}
& \hat{H}y + \hat{F}\theta + \hat{f} -\hat{A}^\top\lambda - \nu = 0 & \text{stationary point} \\
& \lambda^\top s = \nu^\top y = 0 & \text{complementarity} \\
& s, y\geq 0 & \text{primal feasibility} \\
& \lambda, \nu \geq 0 & \text{dual feasibility}
\end{aligned}
The corresponding LCP is:
.. math::
\begin{bmatrix} v \\ s \end{bmatrix} - \begin{bmatrix} \hat{H} & -\hat{A}^\top \\ \hat{A} & 0 \end{bmatrix} \begin{bmatrix} y \\ \lambda \end{bmatrix}
= \begin{bmatrix} \hat{F} \\ -\hat{B} \end{bmatrix} \theta + \begin{bmatrix} \hat{f} \\ -\hat{b} \end{bmatrix}
The relationship between the solution of LCP and original QP is:
.. math::
\begin{aligned}
& x = P \begin{bmatrix} x_B \\ x_N \end{bmatrix} = P \left( \begin{bmatrix} C \\ I \end{bmatrix} x_N + \begin{bmatrix} D_1 \\ 0 \end{bmatrix} + \begin{bmatrix} D_2 \\ 0 \end{bmatrix}\theta \right), \\
& x_N = \begin{bmatrix} I & -I \end{bmatrix} y, \\
& y = \begin{bmatrix} I & 0 \end{bmatrix} z,
\end{aligned}
where :math:`P` is a proper permutation matrix.
"""
qp = self
# TODO: deal with bounds
if np.any(np.isfinite(qp.ub)) or np.any(np.isfinite(qp.lb)):
raise NotImplementedError
# ============================= Step 1: eliminate equalities =============================
eliminated_eq = (qp.me > 0)
if eliminated_eq:
# Ae*x = be = pE*th is required to be consistent, i.e., no linearly dependent rows are found
# and the number of equalities me is strictly less than number of variables n, i.e. me < n.
# Remove linearly dependent rows in the matrix.
equality_matrix = np.column_stack([qp.Ae, qp.be, qp.pE])
_, S, _ = np.linalg.svd(equality_matrix, full_matrices=False)
independent_indices = np.where(S > tolerances['zero'])[0]
equality_matrix = equality_matrix[independent_indices, :]
if equality_matrix.shape[0] >= qp.n:
raise ValueError("Cannot eliminate equality constraints because number of equalities is not strictly less than number of variables.")
Ae = equality_matrix[:, :qp.n]
be = equality_matrix[:, qp.n]
pE = equality_matrix[:, qp.n+1:]
# split the variable x into basic and non-basic parts, x consists of x_B and x_N,
# where B is the set of basic variables indices and N is the set of non-basic variable indices
# the consistent Ae matrix is also split as Ae = [Ae_B, Ae_N], where Ae_B is a square invertible matrix
_, _, P = qr(Ae, pivoting=True)
permutation_matrix = np.eye(P.size)[:, P]
idx_B = P[:Ae.shape[0]] # indices of basic variables (columns)
idx_N = P[Ae.shape[0]:] # indices of non-basic variables (columns)
Ae_B = Ae[:, idx_B]
Ae_N = Ae[:, idx_N]
Ae_B_inv = np.linalg.inv(Ae_B)
C = - Ae_B_inv @ Ae_N
D1 = Ae_B_inv @ be
D2 = Ae_B_inv @ pE
# inequalities with x split
A_tilde = qp.A[:, idx_B] @ C + qp.A[:, idx_N]
b_tilde = qp.b - qp.A[:, idx_B] @ D1
pB_tilde = qp.pB - qp.A[:, idx_B] @ D2
if qp.problem_type == 'QP':
H_BB = qp.H[np.ix_(idx_B, idx_B)]
H_NN = qp.H[np.ix_(idx_N, idx_N)]
H_NB = qp.H[np.ix_(idx_N, idx_B)]
H_BN = qp.H[np.ix_(idx_B, idx_N)]
H_tilde = C.T @ H_BB @ C + C.T @ H_BN + H_NB @ C + H_NN
pF_tilde = 0.5 * (C.T @ H_BB @ D2 + C.T @ H_BB.T @ D2 + H_BN.T @ D2 + H_NB @ D2) + C.T @ qp.pF[idx_B] + qp.pF[idx_N]
f_tilde = 0.5 * (C.T @ H_BB @ D1 + C.T @ H_BB.T @ D1 + H_BN.T @ D1 + H_NB @ D1) + C.T @ qp.f[idx_B] + qp.f[idx_N]
elif qp.problem_type == 'LP':
H_BB = None # for recovery later
H_tilde = np.zeros((idx_N.size, idx_N.size))
pF_tilde = C.T @ qp.pF[idx_B] + qp.pF[idx_N]
f_tilde = C.T @ qp.f[idx_B] + qp.f[idx_N]
else:
raise NotImplementedError
else:
H_tilde = qp.H if qp.problem_type == 'QP' else np.zeros((qp.n, qp.n))
pF_tilde = qp.pF
f_tilde = qp.f
A_tilde = qp.A
b_tilde = qp.b
pB_tilde = qp.pB
permutation_matrix = np.eye(qp.n)
# fabricate split symbols to simplify code later
idx_N = np.arange(qp.n)
C = None
D1 = None
D2 = None
H_BB = None
Ae_B = None
Ae_B_inv = None
idx_B = None
# ============================= Step 2: transform inequalities to Ay>=b and y>=0 =============================
# Case 1: can directly decompose A_tilde into a square invertible matrix A_tilde_B and a non-square matrix A_tilde_N
if (np.linalg.matrix_rank(A_tilde) >= A_tilde.shape[1]):
# choose |B| = #cols(A_tilde); we need a square invertible A_tilde_B
# pick row indices for B and N via QR with pivoting on rows; emulate by pivoting on A_tilde.T and map back
_, _, P_rows = qr(A_tilde.T, pivoting=True)
# first mB rows become B (mB = ncols), rest N
mB = A_tilde.shape[1]
idx_B_rows = P_rows[:mB]
idx_N_rows = P_rows[mB:]
# consistent with your previous notation:
A_tilde_B, b_tilde_B, pB_tilde_B = A_tilde[idx_B_rows, :], b_tilde[idx_B_rows], pB_tilde[idx_B_rows, :]
A_tilde_N, b_tilde_N, pB_tilde_N = A_tilde[idx_N_rows, :], b_tilde[idx_N_rows], pB_tilde[idx_N_rows, :]
A_tilde_B_inv = np.linalg.inv(A_tilde_B)
H_hat = A_tilde_B_inv.T @ H_tilde @ A_tilde_B_inv
pF_hat = -H_hat @ pB_tilde_B - A_tilde_B_inv.T @ pF_tilde
f_hat = -H_hat @ b_tilde_B - A_tilde_B_inv.T @ f_tilde
A_hat = A_tilde_N @ A_tilde_B_inv
b_hat = A_hat @ b_tilde_B - b_tilde_N
pB_hat = A_hat @ pB_tilde_B - pB_tilde_N
# linear map xN = KxN_y z + KxN_th theta + kxN
# z = [y; lambda], so select y
ny = H_hat.shape[0]
mhat = A_hat.shape[0]
Sz_y = np.hstack([np.eye(ny), np.zeros((ny, mhat))])
Sz_lam = np.hstack([np.zeros((mhat, ny)), np.eye(mhat)])
Kz_xN = -A_tilde_B_inv @ Sz_y # (nN x (ny+mhat))
Kth_xN = A_tilde_B_inv @ pB_tilde_B
k_xN = A_tilde_B_inv @ b_tilde_B
# Recover tilde-π: rows in B -> ν (multipliers for y>=0), rows in N -> λ (multipliers for A_hat y >= b_hat+...)
# ν = v = H_hat y + pF_hat θ + f_hat - A_hat^T λ
Kz_nu = H_hat @ Sz_y - A_hat.T @ Sz_lam # (ny x (ny+mhat))
Kth_nu = pF_hat
k_nu = f_hat
# assemble pi_tilde in original row order (size = rows of A_tilde)
m_tilde = A_tilde.shape[0]
Kz_pi_tilde = np.zeros((m_tilde, ny + mhat))
Kth_pi_tilde = np.zeros((m_tilde, qp.d))
k_pi_tilde = np.zeros((m_tilde,))
Kz_pi_tilde[idx_B_rows, :] = Kz_nu
Kth_pi_tilde[idx_B_rows, :] = Kth_nu
k_pi_tilde[idx_B_rows] = k_nu
Kz_pi_tilde[idx_N_rows, :] = Sz_lam
# (Kth for λ part remains zeros)
else:
# Case 2: xN = [I, -I] y, and inequalities become A_hat = [-A_tilde, A_tilde], single λ per row
H_hat = np.vstack([np.hstack([H_tilde, -H_tilde]),
np.hstack([-H_tilde, H_tilde])])
pF_hat = np.hstack([pF_tilde, -pF_tilde])
f_hat = np.concatenate([f_tilde, -f_tilde])
A_hat = np.hstack([-A_tilde, A_tilde])
b_hat = -b_tilde
pB_hat = -pB_tilde
ny = H_hat.shape[0] # equals 2 * nN
mhat = A_hat.shape[0]
Sz_y = np.hstack([np.eye(ny), np.zeros((ny, mhat))])
Sz_lam = np.hstack([np.zeros((mhat, ny)), np.eye(mhat)])
# xN = [I, -I] y
S_pm = np.hstack([np.eye(A_tilde.shape[1]), -np.eye(A_tilde.shape[1])]) # (nN x 2*nN)
Kz_xN = S_pm @ Sz_y
Kth_xN = np.zeros((A_tilde.shape[1], qp.d))
k_xN = np.zeros((A_tilde.shape[1],))
# tilde-π is exactly λ (one per original inequality row)
Kz_pi_tilde = Sz_lam
Kth_pi_tilde = np.zeros((mhat, qp.d))
k_pi_tilde = np.zeros((mhat,))
# ============================= Step 3: turn into LCP =============================
lcp_M = np.vstack([
np.hstack([H_hat, -A_hat.T]),
np.hstack([A_hat, np.zeros((A_hat.shape[0], A_hat.shape[0]))])
])
lcp_q = np.concatenate([f_hat, -b_hat])
lcp_Q = np.vstack([pF_hat, -pB_hat]) if qp.d > 0 else np.zeros((lcp_M.shape[0], 0))
# selectors again (defined above)
ny = H_hat.shape[0]
mhat = A_hat.shape[0]
Sz_y = np.hstack([np.eye(ny), np.zeros((ny, mhat))])
Sz_lam = np.hstack([np.zeros((mhat, ny)), np.eye(mhat)])
# Build affine recovery maps in Kx + k form
# 1) xN(z, θ) = Kz_xN z + Kth_xN θ + k_xN
# 2) x from xN:
# if equalities eliminated: x = P [ [C],[I] ] xN + P[[D1],[0]] + P[[D2],[0]] θ
# else: x = xN (since permutation_matrix = I and C/D* unused)
if eliminated_eq:
nN = Kz_xN.shape[0]
nB = C.shape[0]
# stack matrices to map x = P * ([C; I] xN + [D1;0] + [D2;0] θ)
S_stack = np.vstack([C, np.eye(nN)]) # ((nB+nN) x nN)
D1_stack = np.concatenate([D1, np.zeros((nN,))])
D2_stack = np.vstack([D2, np.zeros((nN, qp.d))])
Kz_x = permutation_matrix @ (S_stack @ Kz_xN)
Kth_x = permutation_matrix @ (S_stack @ Kth_xN + D2_stack)
k_x = permutation_matrix @ (S_stack @ k_xN + D1_stack)
else:
Kz_x = Kz_xN
Kth_x = Kth_xN
k_x = k_xN
# π = tilde-π (no further change)
Kz_pi = Kz_pi_tilde
Kth_pi = Kth_pi_tilde
k_pi = k_pi_tilde
# μ from the formula in the doc (zero if no equalities)
if eliminated_eq:
# Build blocks needed for μ
# A_B is the columns of original A corresponding to x_B
A_B = qp.A[:, idx_B]
if qp.problem_type == 'QP':
# prepare (H_BB C + H_BN) * xN term
HBBC_plus_HBN = (H_BB @ C + qp.H[np.ix_(idx_B, idx_N)])
# θ term: (H_BB D2 + F_B) θ
F_B = qp.pF[idx_B]
HBB_D2_plus_FB = H_BB @ D2 + F_B
# constant: (H_BB D1 + f_B)
f_B = qp.f[idx_B]
HBB_D1_plus_fB = H_BB @ D1 + f_B
else:
# LP: H = 0
HBBC_plus_HBN = np.zeros((len(idx_B), len(idx_N)))
HBB_D2_plus_FB = qp.pF[idx_B]
HBB_D1_plus_fB = qp.f[idx_B]
# μ = -Ae_B^{-T} [ (H_BB C + H_BN) xN + A_B^T π + (H_BB D1 + f_B) + (H_BB D2 + F_B) θ ]
Kz_mu = - Ae_B_inv.T @ (HBBC_plus_HBN @ Kz_xN + A_B.T @ Kz_pi)
Kth_mu = - Ae_B_inv.T @ (HBBC_plus_HBN @ Kth_xN + A_B.T @ Kth_pi + HBB_D2_plus_FB)
k_mu = - Ae_B_inv.T @ (HBBC_plus_HBN @ k_xN + A_B.T @ k_pi + HBB_D1_plus_fB)
else:
Kz_mu = np.zeros((0, ny + mhat))
Kth_mu = np.zeros((0, qp.d))
k_mu = np.zeros((0,))
# Package the recovery callable (convenience)
def recover(lcp_z: np.ndarray, theta: np.ndarray):
x = Kz_x @ lcp_z + Kth_x @ theta + k_x
pi = Kz_pi @ lcp_z + Kth_pi @ theta + k_pi
mu = Kz_mu @ lcp_z + Kth_mu @ theta + k_mu
return x, pi, mu
# Also return explicit K,k maps
recover_coeffs = {
"x": (Kz_x, Kth_x, k_x), # map z to the primal variable x
"pi": (Kz_pi, Kth_pi, k_pi), # map z to the dual variable π (for inequalities)
"mu": (Kz_mu, Kth_mu, k_mu), # map z to the dual variable μ (for equalities)
# Optional helpers if you also want y, λ, ν, s for debugging:
# "y": (Sz_y, np.zeros((ny, qp.d)), np.zeros(ny)),
# "lam":(Sz_lam,np.zeros((mhat, qp.d)),np.zeros(mhat)),
# "nu": (H_hat @ Sz_y - A_hat.T @ Sz_lam, pF_hat, f_hat),
# "s": (A_hat @ Sz_y, -pB_hat, -b_hat),
}
return LCP(M=lcp_M, Q=lcp_Q, q=lcp_q, Ath=self.Ath, bth=self.bth), recover, recover_coeffs
[docs]
def solve_pqp_with_ppopt(pqp: QuadraticProgram) -> PqpSolution:
r"""
Solve the parametric QP using PPOPT package.
In PPOPT, the standard form of quadratic multiparametric programming is defined as follows (from mpqp_program.py, https://ppopt.readthedocs.io/en/main/ppopt.html#module-ppopt.mpqp_program):
.. math::
\begin{align}
\min_x\quad \frac{1}{2}x^TQx& + \theta^TH^Tx + c^Tx\\
\text{s.t.}\quad Ax &\leq b + F\theta\\
A_{eq}x &= b_{eq}\\
A_\theta \theta &\leq b_\theta\\
x &\in R^n\\
\end{align}
"""
assert pqp.is_parametric, "The provided QP is not parametric."
from ppopt.mpqp_program import MPQP_Program
from ppopt.mp_solvers.solve_mpqp import solve_mpqp, mpqp_algorithm
num_eq = pqp.me
mpqp_prog = MPQP_Program(
A = np.vstack([pqp.Ae, pqp.A]),
b = np.concatenate([pqp.be, pqp.b]).reshape((-1, 1)),
F = np.vstack([pqp.pE, pqp.pB]),
Q = pqp.H,
c = pqp.f.reshape((-1,1)),
H = pqp.pF,
Q_t = pqp.pY,
A_t = pqp.Ath,
b_t = pqp.bth.reshape((-1, 1)),
equality_indices=list(range(num_eq))
)
# mpqp_prog.process_constraints()
mpqp_solution = solve_mpqp(mpqp_prog, mpqp_algorithm.geometric)
# Extract the solution map
critical_regions_list = []
for region in mpqp_solution.critical_regions:
polyhedron = Polyhedron.from_Hrep(A=region.E, b=region.f.flatten())
polyhedron.add_function(AffineFunction(region.A, region.b), 'primal')
polyhedron.add_function(AffineFunction(region.C[num_eq:], region.d[num_eq:]), 'dual_ineq')
polyhedron.add_function(AffineFunction(region.C[:num_eq], region.d[:num_eq]), 'dual_eq')
critical_regions_list.append(polyhedron)
critical_regions = PolyUnion(*critical_regions_list)
return PqpSolution('ok', critical_regions)
QP = QuadraticProgram