import numpy as np
from scipy.linalg import null_space
from abc import ABC, abstractmethod
import cvxpy as cp
from typing import Optional, Tuple, List, Union, Callable
from mpt4py.base import Vector, Matrix
from mpt4py import exceptions as mpterr
from mpt4py.util import range_space, is_positive_semidefinite
from mpt4py.tolerances import tolerances
from mpt4py.functions.function_base import FunctionBase
from .visualization import PlotterProtocol
[docs]
class ConvexSet(ABC):
def __init__(self):
self._functions = {}
@abstractmethod
def _cvxrep(self) -> Tuple[cp.Variable, List[cp.Constraint]]:
"""
Returns a CVXPY representation of the set.
Returns a variable and a set of constraints such that
x in Set <=> x satisfies constraints
"""
pass
@property
@abstractmethod
def dim(self) -> int:
"""
Returns the dimension of the convex set.
"""
pass
def __contains__(self, x: Vector) -> bool:
"""
Check if the point x is in the set.
"""
return bool(self.distance(x) < tolerances['zero'])
[docs]
def extreme(self, x: Vector) -> Vector:
r"""
Computes an extreme point :math:`y` of the set :math:`\mathcal{C}` in the direction :math:`x`:
.. math::
\begin{align*}
\mathop{\arg\min}\limits_y &\quad x^\top y \\
\text{s.t.} &\quad y \in \mathcal{C} \\
\end{align*}
"""
var, con = self._cvxrep()
obj = cp.Maximize(var.T @ x)
prob = cp.Problem(obj, con)
result = prob.solve()
if prob.status == cp.INFEASIBLE:
raise mpterr.InfeasibleError("The set is empty")
if prob.status == cp.UNBOUNDED:
raise mpterr.UnboundedError("The set is unbounded")
# TODO: consider the case when the extreme point is not unique
return var.value
[docs]
def shoot(self, r: Vector, x0: Vector) -> Tuple[float, Vector]:
r"""
Computes the solution to the optimization problem:
.. math::
\begin{align*}
\max\limits_\alpha &\quad \alpha \\
\text{s.t.} &\quad x_0 + \alpha r \in \mathcal{C}
\end{align*}
The problem can be explained as to find the point that lies along the line given by the ray :math:`r`
which starts from the point :math:`x_0` such that it still lies inside the set :math:`\mathcal{C}`.
Typically, the solution of this set is the point that lies on the boundary of the set,
or can be Inf if the set is unbounded.
"""
# check dimension
r = np.reshape(r, (-1,))
x0 = np.reshape(x0, (-1,))
if r.size != self.dim:
raise ValueError("Incompatible dimensions: r must be {0} dimension".format(self.dim))
if x0.size != self.dim:
raise ValueError("Incompatible dimensions: x0 must be {0} dimension".format(self.dim))
alpha = cp.Variable() # length
x, con = self._cvxrep()
con += [x == x0 + alpha * r]
obj = cp.Maximize(alpha)
prob = cp.Problem(obj, con)
result = prob.solve()
if prob.status == cp.INFEASIBLE:
raise mpterr.InfeasibleError("The set is empty")
if prob.status == cp.UNBOUNDED:
raise mpterr.UnboundedError("The set is unbounded")
return float(alpha.value), x.value
[docs]
def project(self, x: Vector) -> Vector:
r"""
Projects the given point :math:`y` onto the set :math:`\mathcal{C}`:
.. math::
\begin{aligned}
\min_x &\quad \|x - y\|_2^2 \\
\text{s.t.} &\quad x \in \mathcal{C}
\end{aligned}
"""
var, con = self._cvxrep()
obj = cp.Minimize(cp.sum_squares(var - x))
prob = cp.Problem(obj, con)
result = prob.solve()
if prob.status == cp.INFEASIBLE:
raise mpterr.InfeasibleError("The set is empty")
if prob.status == cp.UNBOUNDED:
raise mpterr.UnboundedError("The set is unbounded")
return var.value
[docs]
def support(self, x: Vector) -> float:
r"""
Compute the support of the set :math:`\mathcal{C}` in the direction :math:`x`:
.. math::
\begin{align*}
\max\limits_y &\quad x^\top y \\
\text{s.t.} &\quad y \in \mathcal{C} \\
\end{align*}
"""
var, con = self._cvxrep()
obj = cp.Maximize(var.T @ x)
prob = cp.Problem(obj, con)
result = prob.solve()
if prob.status == cp.INFEASIBLE:
raise mpterr.InfeasibleError("The set is empty")
if prob.status == cp.UNBOUNDED:
return np.inf
return float(prob.value)
[docs]
def seperate(self, x: Vector) -> "Hyperplane":
"""
Compute the maximal seperating hyperplane between the point x and the Set.
"""
# Project the point onto the set
x_proj = self.project(x)
# The maximal seperating hyperplane is the hyperplane orthogonal to the line connecting x and x_proj
normal = x - x_proj
normal = normal / np.linalg.norm(normal)
# The hyperplane passes through the point halway between x and x_proj
x0 = (x - x_proj) / 2
return Hyperplane(a = normal, x0 = x0)
[docs]
def distance(self, other: Union[Vector, "ConvexSet"]) -> np.floating:
r"""
Compute the distance between this convex set :math:`\mathcal{C}` and the given point :math:`z`
or convex set :math:`\mathcal{S}`.
By providing real vector :math:`z`, the distance between :math:`z` and :math:`\mathcal{C}`
is computed by solving the optimization problem
.. math::
\begin{align*}
\min\limits_y &\quad \|z-y\|_2^2 \\
\text{s.t.} &\quad y \in \mathcal{C}.
\end{align*}
By providing a convex set :math:`\mathcal{S}`, the distance between :math:`\mathcal{C}` and :math:`\mathcal{S}`
is computed by solving the optimization problem
.. math::
\begin{align*}
\min\limits_{x,y} &\quad \|x-y\|_2^2 \\
\text{s.t.} &\quad x \in \mathcal{C}, \\
&\quad y \in \mathcal{S}.
\end{align*}
"""
if isinstance(other, np.ndarray):
other = np.reshape(other, (-1,))
return np.linalg.norm(other - self.project(x=other))
elif isinstance(other, ConvexSet):
if self.dim != other.dim:
raise ValueError("Incompatible dimensions: the other convex set must be {0} dimension".format(self.dim))
var_x, con_x = self._cvxrep()
var_y, con_y = other._cvxrep()
con = []
con += con_x
con += con_y
obj = cp.Minimize(cp.sum_squares(var_x - var_y))
prob = cp.Problem(obj, con)
result = prob.solve()
if prob.status == cp.INFEASIBLE:
raise mpterr.InfeasibleError("Either self or the other set is empty.")
return np.linalg.norm(var_x.value - var_y.value)
[docs]
def add_function(self, func: Union[FunctionBase, Callable[[Vector], float]], func_name: str) -> None:
"""
Adds a function associated to the convex set.
"""
if isinstance(func, FunctionBase):
self._functions[func_name] = func
elif callable(func):
self._functions[func_name] = FunctionBase(lambda_expr=func)
else:
raise ValueError("function must be either a FunctionBase or a callable.")
[docs]
def has_function(self, func_name: Optional[str] = None) -> bool:
"""
Returns whether the convex set has a function specified by the name.
"""
return func_name in self._functions.keys() if func_name is not None else len(self._functions)
[docs]
def remove_function(self, func_name: str) -> None:
"""
Removes a function associated to the convex set.
"""
if func_name in self._functions.keys():
del self._functions[func_name]
else:
raise KeyError(f"Function with name {func_name} does not exist.")
[docs]
def remove_all_functions(self) -> None:
"""
Removes all functions associated to the convex set.
"""
self._functions = {}
[docs]
def get_function(self, func_name: str) -> FunctionBase:
"""
Returns a function associated to the convex set.
"""
if func_name in self._functions.keys():
return self._functions[func_name]
else:
raise KeyError(f"Function with name {func_name} does not exist.")
[docs]
def list_functions(self):
raise NotImplementedError
[docs]
class AffineSet(ConvexSet):
r"""
Represents an affine set:
.. math::
\{ x \in \mathbb{R}^n \mid Ax=b \}
"""
def __init__(self, A: Matrix, b: Vector):
"""
Initialize an affine set.
Parameters:
- A (array-like, optional): Matrix for the affine constraint Ax == b.
- b (array-like, optional): Vector for the affine constraint Ax == b.
"""
super().__init__()
self._A = np.array(A)
self._b = np.array(b)
if self.A.ndim != 2:
raise ValueError("A must be a matrix")
if self.b.ndim != 1:
raise ValueError("b must be a vector")
if self.A.shape[0] != self.b.shape[0]:
raise ValueError("Incompatible dimensions: number of rows of A must match the number of elements in b")
[docs]
@staticmethod
def from_basis(basis: Matrix, x0: Vector) -> "AffineSet":
"""
Construct an affine set from a basis and a point in the set.
Parameters:
- basis (array-like): Basis for the affine set.
- x0 (array-like): Point in the affine set.
Returns:
- AffineSet: Affine set defined by Ax == b.
"""
# Compute the left null-space of basis
A = null_space(basis.T).T
return AffineSet(A=A, b=A@x0)
@property
def A(self) -> Matrix:
return self._A
@property
def b(self) -> Vector:
return self._b
@property
def basis(self) -> Tuple[Matrix, Vector]:
r"""
Returns a basis and solution for the affine set such that
.. math::
Ax = b \iff x = B y + x_0
"""
U, s, Vt = np.linalg.svd(self.A, full_matrices=True)
rank = np.sum(s > tolerances['zero'])
null_space = Vt.T[:, rank:]
return null_space, self.x0
@property
def x0(self) -> Vector:
"""
Returns a point in the affine set
"""
return np.linalg.lstsq(self.A, self.b, rcond=None)[0]
def _cvxrep(self) -> Tuple[cp.Variable, List[cp.Constraint]]:
r"""
Returns a `CVXPY` representation of the set, consisting of variables and a set of constraints.
"""
x = cp.Variable(self.dim)
constraints: List[cp.Constraint] = [self.A @ x == self.b]
return x, constraints
@property
def dim(self) -> int:
"""
Returns the dimension of the convex set.
"""
return self.A.shape[1]
[docs]
def minimal_representation(self) -> "AffineSet":
"""
Returns a minimal representation of the affine set
"""
from scipy.linalg import svd, diagsvd
if self.A.size == 0:
return self
U, s, Vt = np.linalg.svd(self.A, full_matrices=True)
rank = np.sum(s > tolerances['zero'])
A_min = ((U[:rank, :rank] * s[:rank]) @ Vt[:rank, :]).astype(np.float64)
return AffineSet(A=A_min, b=A_min @ self.x0)
[docs]
def homogenize(self):
"""
Homogenize the affine set
"""
A = np.hstack([self.A, -self.b.reshape(-1, 1)])
b = np.zeros(self.A.shape[0])
return AffineSet(A=A, b=b)
@property
def coefficients(self) -> Tuple[Matrix, Vector]:
return self.A, self.b
[docs]
class Hyperplane(AffineSet):
r"""
Represents the hyperplane: :math:`\langle a, x \rangle = b` or :math:`\langle a, x-x_0 \rangle = 0`.
"""
def __init__(self, a: Vector, b: Optional[np.floating] = None, x0: Optional[Vector] = None):
"""
Initialize a hyperplane.
Parameters:
- a (array-like, optional): Normal vector of the hyperplane.
- b (array-like, optional): Scalar offset of the hyperplane.
- x0 (array-like, optional): Point on the hyperplane.
Specify either b or x0, but not both.
"""
if a.ndim != 1:
raise ValueError("a must be a vector")
if x0 is not None:
b = a.dot(x0)
super().__init__(A = a.reshape(1, -1), b = np.array([b]))
@property
def normal(self):
return self.A[0]
@property
def offset(self):
return self.b[0]
[docs]
class Ellipsoid(ConvexSet):
r"""
Represents an ellipsoid:
.. math::
\{ x \in \mathbb{R}^n \mid (x - c)^\top P (x - c) \leq r^2 \}
where :math:`P\in\mathbb{R}^{n\times n}` is positive semi-definite.
"""
def __init__(self,
P: Matrix,
center: Optional[Vector] = None,
radius: float = 1.):
super().__init__()
if not is_positive_semidefinite(P):
raise ValueError("P must be a positive semidefinite matrix.")
self.P = P
self.center = np.zeros(self.P.shape[0]) if center is None else center
if self.center.ndim != 1 or self.center.shape[0] != self.P.shape[0]:
raise ValueError(f"Invalid shape for center: Must be a vector of length {self.P.shape[0]}.")
self.radius = radius
if self.radius <= 0:
raise ValueError("Invalid radius: must be positive.")
@property
def dim(self) -> int:
return len(self.center)
def _cvxrep(self) -> Tuple[cp.Variable, List[cp.Constraint]]:
x = cp.Variable(self.dim)
err = x - self.center
constraints : List[cp.Constraint] = [err.T @ self.P @ err <= self.radius**2]
return x, constraints
[docs]
def plot(self, fig: PlotterProtocol, **kwargs):
"""Plot the ellipsoid.
Args:
fig (PlotterProtocol): _description_
"""
if self.dim > 3:
raise ValueError("Can only plot 2D or 3D ellipsoids.")
if self.dim <= 1:
raise ValueError("Cannot plot 1D or 0D ellipsoids.")
fig.plot_ellipsoid(self.P, self.center, self.radius, **kwargs)
class Sphere(Ellipsoid):
r"""
Represents a sphere centered at :math:`c` with radius :math:`r`:
.. math::
\{ x \in \mathbb{R}^n \mid \|x - c\|_2^2 \leq r^2 \}.
"""
def __init__(self, center: Vector, radius: float):
super().__init__(P=np.eye(center.size), center=center, radius=radius)
self._center = center
self._radius = radius