def distance_point_polytope(P, x, ball="infinity", solver="Gurobi"):
"""
Computes the distance of point x from AH-polytope Q
"""
x_vector = np.atleast_2d(x) #in case x is not n*1 vector
P = pp.to_AH_polytope(P)
_setup_program_distance_point(P, ball, solver)
prog = P.distance_program
Q = pp.to_AH_polytope(P)
a = P.distance_constraint.evaluator()
x_vector = x_vector.reshape(max(x_vector.shape), 1)
a.UpdateCoefficients(np.hstack((Q.T, -np.eye(Q.n))), x_vector - Q.t)
if solver == "Gurobi":
result = gurobi_solver.Solve(prog, None, None)
elif solver == "osqp":
result = OSQP_solver.Solve(prog, None, None)
else:
result = MP.Solve(prog)
if result.is_success():
zeta_num = result.GetSolution(P.zeta).reshape(P.zeta.shape[0], 1)
x_nearest = np.dot(Q.T, zeta_num) + Q.t
delta = (x_vector - x_nearest).reshape(Q.n)
if ball == "infinity":
d = np.linalg.norm(delta, ord=np.inf)
elif ball == "l1":
d = np.linalg.norm(delta, ord=1)
elif ball == "l2":
d = np.linalg.norm(delta, ord=2)
return d, x_nearest
def intersection(P1, P2):
"""
Inputs:
P1, P2: polytopic objects
Output:
returns :math:`\mathbb{P}_1 \cap \mathbb{P}_2` as an AH-polytope
If both objects are H-polytopes, return H-polytope
"""
if P1.type == "H_polytope" and P2.type == "H_polytope":
H = np.vstack((P1.H, P2.H))
h = np.vstack((P1.h, P2.h))
return pp.H_polytope(H, h)
else:
X, Y = pp.to_AH_polytope(P1), pp.to_AH_polytope(P2)
T = np.hstack((X.T, np.zeros((X.T.shape[0], Y.T.shape[1]))))
H_1 = np.hstack((X.P.H, np.zeros((X.P.H.shape[0], Y.P.H.shape[1]))))
Ty_inv = np.linalg.pinv(Y.T)
H_2 = np.hstack(( np.linalg.multi_dot([Y.P.H,Ty_inv,X.T]),\
np.dot(Y.P.H,np.eye(Y.T.shape[1])-np.dot(Ty_inv,Y.T)) ))
H = np.vstack((H_1, H_2))
h = np.vstack(
(X.P.h, Y.P.h - np.linalg.multi_dot([Y.P.H, Ty_inv, X.t - Y.t])))
new_P = pp.H_polytope(H, h)
return pp.AH_polytope(T=T, t=X.t, P=new_P)
def distance_polytopes(Q1, Q2, ball="infinity", solver="gurobi"):
"""
Finds the closest two points in two polytopes and their distance.
It is zero if polytopes have non-empty intersection
"""
Q1, Q2 = pp.to_AH_polytope(Q1), pp.to_AH_polytope(Q2)
n = Q1.n
prog = MP.MathematicalProgram()
zeta1 = prog.NewContinuousVariables(Q1.P.H.shape[1], 1, "zeta1")
zeta2 = prog.NewContinuousVariables(Q2.P.H.shape[1], 1, "zeta2")
delta = prog.NewContinuousVariables(n, 1, "delta")
prog.AddLinearConstraint(A=Q1.P.H,
ub=Q1.P.h,
lb=-np.inf * np.ones((Q1.P.h.shape[0], 1)),
vars=zeta1)
prog.AddLinearConstraint(A=Q2.P.H,
ub=Q2.P.h,
lb=-np.inf * np.ones((Q2.P.h.shape[0], 1)),
vars=zeta2)
prog.AddLinearEqualityConstraint(np.hstack((Q1.T, -Q2.T, np.eye(n))),
Q2.t - Q1.t,
np.vstack((zeta1, zeta2, delta)))
if ball == "infinity":
delta_abs = prog.NewContinuousVariables(1, 1, "delta_abs")
prog.AddBoundingBoxConstraint(0, np.inf, delta_abs)
prog.AddLinearConstraint(
np.greater_equal(np.dot(np.ones((n, 1)), delta_abs),
delta,
dtype='object'))
prog.AddLinearConstraint(
np.greater_equal(np.dot(np.ones((n, 1)), delta_abs),
-delta,
dtype='object'))
cost = delta_abs
elif ball == "l1":
delta_abs = prog.NewContinuousVariables(n, 1, "delta_abs")
prog.AddBoundingBoxConstraint(0, np.inf, delta_abs)
prog.AddLinearConstraint(
np.greater_equal(delta_abs, delta, dtype='object'))
prog.AddLinearConstraint(
np.greater_equal(delta_abs, -delta, dtype='object'))
cost = np.dot(np.ones((1, n)), delta_abs)
else:
raise NotImplementedError
if solver == "gurobi":
prog.AddLinearCost(cost[0, 0])
result = gurobi_solver.Solve(prog, None, None)
elif solver == "osqp":
prog.AddQuadraticCost(cost[0, 0] * cost[0, 0])
result = OSQP_solver.Solve(prog, None, None)
else:
prog.AddLinearCost(cost[0, 0])
result = MP.Solve(prog)
if result.is_success():
return np.sum(result.GetSolution(delta_abs)),\
np.dot(Q1.T,result.GetSolution(zeta1).reshape(zeta1.shape[0],1))+Q1.t,\
np.dot(Q2.T,result.GetSolution(zeta2).reshape(zeta2.shape[0],1))+Q2.t
def rci(A, B, X, U, W, eta=0.001, q=1):
n, m = A.shape[0], B.shape[1]
W = pp.zonotope(x=np.zeros((n, 1)), G=W.G * eta)
program = MP.MathematicalProgram()
q += n
program = MP.MathematicalProgram()
phi = program.NewContinuousVariables(n, q, 'phi')
theta = program.NewContinuousVariables(m, q, 'theta')
alpha = program.NewContinuousVariables(1, 'alpha')
beta = program.NewContinuousVariables(2, 'beta')
program.AddBoundingBoxConstraint(0, 1, alpha)
program.AddBoundingBoxConstraint(0, 10, beta)
K = np.hstack(((np.dot(A, phi) + np.dot(B, theta))[:, n:], W.G))
program.AddLinearConstraint(np.equal(K, phi, dtype='object').flatten())
inbody = pp.zonotope(x=np.zeros((n, 1)),
G=(np.dot(A, phi) + np.dot(B, theta))[:, 0:n])
_W = pp.to_AH_polytope(W)
_W.P.h = _W.P.h * alpha
_X = pp.to_AH_polytope(X)
_X.P.h = _X.P.h * beta[0]
_U = pp.to_AH_polytope(U)
_U.P.h = _U.P.h * beta[1]
pp.subset(program, inbody, circumbody=_W)
pp.subset(program, pp.zonotope(x=np.zeros((n, 1)), G=phi), circumbody=_X)
pp.subset(program, pp.zonotope(x=np.zeros((n, 1)), G=theta), circumbody=_U)
program.AddLinearCost(beta[0])
program.AddLinearConstraint(beta[0] <= 1 - alpha[0])
program.AddLinearConstraint(beta[1] <= 1 - alpha[0])
program.AddLinearConstraint(beta[0] >= beta[1])
result = gurobi_solver.Solve(program, None, None)
if result.is_success():
print("sucsess")
print("betas are", result.GetSolution(beta))
beta_1_n = result.GetSolution(beta)[0]
beta_2_n = result.GetSolution(beta)[1]
alpha_n = result.GetSolution(alpha)[0]
phi_n = result.GetSolution(phi)
theta_n = result.GetSolution(theta)
Omega = pp.zonotope(x=np.zeros((2, 1)),
G=phi_n / beta_1_n,
color='red')
pp.visualize(
[X, Omega],
title='Robust control invariant set $\mathcal{X}$ (red) \n \
inside safe set $\mathbb{X}$ (green)',
figsize=(6, 6),
a=0.02)
print("alpha was", alpha_n)
omega_U = theta_n / beta_2_n
print(omega_U)
print('sum_omega=', np.sum(np.abs(omega_U), 1)[0])
return Omega
else:
print("failure")
def add_disjunctive_subsets(program, inbody, list_of_circumbodies):
"""
Parameters
----------
program : add constraints to the program
DESCRIPTION.
inbody : polytopic object
the inbody.
list_of_circumbodies : list
the list of circumbodies.
Returns
-------
mu : dict
dict of binary auxilary variables
dict[hash(polytope)]=BINARYVARIABLE (drake mathematical program)
"""
mu, T, t = {}, {}, {}
print("disjunctive subset with %d circumbodies" %
len(list_of_circumbodies))
assert type(list_of_circumbodies) == list
my_inbody = pp.to_AH_polytope(inbody)
sigma_T = my_inbody.T
sigma_t = my_inbody.t
for circumbody in list_of_circumbodies:
i = hash(circumbody)
T[i] = program.NewContinuousVariables(my_inbody.T.shape[0],
my_inbody.T.shape[1])
t[i] = program.NewContinuousVariables(my_inbody.t.shape[0], 1)
mu[i] = program.NewBinaryVariables(1, 1, 'dmu')
_inbody = pp.AH_polytope(t=t[i], T=T[i], P=my_inbody.P)
_circumbody = pp.to_AH_polytope(circumbody)
_circumbody.P.h = _circumbody.P.h * mu[i]
_circumbody.t = _circumbody.t * mu[i]
pp.subset(program, _inbody, _circumbody)
sigma_T = sigma_T - T[i]
sigma_t = sigma_t - t[i]
# print(sigma_T.shape,sigma_t.shape)
_mu = np.vstack(
[mu[hash(circumbody)] for circumbody in list_of_circumbodies])
program.AddLinearEqualityConstraint(\
np.ones((1,len(list_of_circumbodies))),np.ones((1,1)),_mu)
program.AddLinearConstraint(
np.equal(sigma_T, np.zeros(sigma_T.shape), dtype='object').flatten())
program.AddLinearConstraint(
np.equal(sigma_t, np.zeros(sigma_t.shape), dtype='object').flatten())
return mu, t, T
def Hausdorff_distance(Q1,
Q2,
directed=False,
ball="infinty_norm",
solver="gurobi",
k=-1):
X, Y = pp.to_AH_polytope(Q1), pp.to_AH_polytope(Q2)
prog = MP.MathematicalProgram()
# Variables
n = Q1.n
D1 = prog.NewContinuousVariables(1, "D1")
D2 = prog.NewContinuousVariables(1, "D2")
if ball == "infinty_norm":
P_ball = pp.unitbox(n).H_polytope
elif ball in ["L1", 1, "1", "l1"]:
P_ball = pp.unitball(n, 1)
else:
print("I don't recognize the ball norm")
raise NotImplementedError
if P_ball.type == 'H_polytope':
Dball1 = pp.H_polytope(P_ball.H, P_ball.h * D1)
if not directed:
Dball2 = pp.H_polytope(P_ball.H, P_ball.h * D2)
if P_ball.type == 'AH_polytope':
Dball1=pp.AH_polytope(t=P_ball.t*D1,T=P_ball.T,\
P=pp.H_polytope(P_ball.P.H,P_ball.P.h*D1))
if not directed:
Dball2=pp.AH_polytope(t=P_ball.t*D2,T=P_ball.T,\
P=pp.H_polytope(P_ball.P.H,P_ball.P.h*D2))
X_plus = pp.minkowski_sum(X, Dball1)
pp.subset(prog, Y, X_plus, k=k)
prog.AddLinearCost(np.array([1]), np.array([0]), D1)
if not directed:
Y_plus = pp.minkowski_sum(Y, Dball2)
pp.subset(prog, X, Y_plus, k=k)
prog.AddLinearCost(np.array([1]), np.array([0]), D2)
if solver == "gurobi":
result = gurobi_solver.Solve(prog, None, None)
if result.is_success():
dXY = np.asscalar(result.GetSolution(D1))
if not directed:
dYX = np.asscalar(result.GetSolution(D2))
return max(dXY, dYX), dXY, dYX
else:
return dXY
def bounding_box(Q, solver="Gurobi"):
r"""
Computes the bounding box of a polytope by solving :math:`2n` linear programs.
Each linear program is in the form:
.. math::
\begin{array}{lll}
l_i= & \min & e_i^T x \\
& \text{subject to} & x \in \mathbb{P}
\end{array}
and
.. math::
\begin{array}{lll}
u_i= & \max & e_i^T x \\
& \text{subject to} & x \in \mathbb{P}
\end{array}
where :math:`l,u` define the lower and upper corners of the bounding box.
"""
Q = pp.to_AH_polytope(Q)
prog = MP.MathematicalProgram()
zeta = prog.NewContinuousVariables(Q.P.H.shape[1], 1, "zeta")
x = prog.NewContinuousVariables(Q.n, 1, "x")
prog.AddLinearConstraint(A=Q.P.H,
ub=Q.P.h,
lb=-np.inf * np.ones((Q.P.h.shape[0], 1)),
vars=zeta)
prog.AddLinearEqualityConstraint(np.hstack((-Q.T, np.eye(Q.n))), Q.t,
np.vstack((zeta, x)))
lower_corner = np.zeros((Q.n, 1))
upper_corner = np.zeros((Q.n, 1))
c = prog.AddLinearCost(np.ones(Q.n), 0, x)
if solver == "Gurobi":
solver = gurobi_solver
else:
raise NotImplementedError
a = np.zeros((Q.n, 1))
# Lower Corners
for i in range(Q.n):
# print(i,"lower")
e = c.evaluator()
a[i, 0] = 1
e.UpdateCoefficients(a.reshape(Q.n))
result = solver.Solve(prog, None, None)
assert result.is_success()
lower_corner[i, 0] = result.GetSolution(x)[i]
a[i, 0] = 0
# Upper Corners
for i in range(Q.n):
# print(i,"upper")
e = c.evaluator()
a[i, 0] = -1
e.UpdateCoefficients(a.reshape(Q.n))
result = solver.Solve(prog, None, None)
assert result.is_success()
upper_corner[i, 0] = result.GetSolution(x)[i]
a[i, 0] = 0
return pp.hyperbox(corners=(lower_corner, upper_corner))
def intersection_old(P1, P2):
"""
Inputs:
P1, P2: AH_polytopes :math:`\mathbb{P}_1,\mathbb{P}_2`. Converted to AH-polytopes
Output:
returns :math:`\mathbb{P}_1 \cap \mathbb{P}_2` as an AH-polytope
"""
Q1, Q2 = pp.to_AH_polytope(P1), pp.to_AH_polytope(P2)
T = np.hstack((Q1.T, Q2.T * 0))
t = Q1.t
H_1 = spa.block_diag(*[Q1.P.H, Q2.P.H])
H_2 = np.hstack((Q1.T, -Q2.T))
H = np.vstack((H_1, H_2, -H_2))
h = np.vstack((Q1.P.h, Q2.P.h, Q2.t - Q1.t, Q1.t - Q2.t))
new_P = pp.H_polytope(H, h)
return pp.AH_polytope(T=T, t=t, P=new_P)
def ray_shooting_hyperplanes_old(Q,N=0,H_y=None):
"""
Ray Shooting to find an outer-approximation of the AH-polytope
"""
prog=MP.MathematicalProgram()
Q=pp.to_AH_polytope(Q)
if type(H_y)==type(None):
if N==0:
N=2**(Q.n-1) # This many hyperplanes I want :)
H_rays=np.random.normal(size=(N,Q.n))
H_y=np.vstack(( H_rays , -H_rays ))
else:
N=H_rays.shape[0]
assert H_rays.shape[1]==Q.n
h_n=np.zeros((2*N,1))
zeta=prog.NewContinuousVariables(Q.P.H.shape[1],1,"zeta")
prog.AddLinearConstraint(A=Q.P.H,ub=Q.P.h,lb=-np.inf*np.ones((Q.P.h.shape[0],1)),vars=zeta)
a=np.dot(H_rays[0,:],Q.T)
b=np.dot(H_rays[0,:],Q.t)
cost=prog.AddLinearCost(a,b,zeta)
for i in range(2*N):
new_a=np.dot(H_y[i,:],Q.T)
new_b=np.dot(H_y[i,:],Q.t)
cost.evaluator().UpdateCoefficients( -new_a, new_b)
result=gurobi_solver.Solve(prog,None,None)
if result.is_success():
_s=result.GetSolution(zeta)
h_n[i,0]=np.dot(new_a,_s)+new_b
else:
print("The polytope you gave me seems unbounded or \
there is another error")
return pp.H_polytope(H_y,h_n)
def point_membership_fuzzy(Q, x, tol=10**-5, solver="gurobi"):
"""
Fuzzy membership check. If x contains NaN, the entry is unconstrained
@param Q: Polytope in R^n
@param x: n*1 numpy array, may contain NaNs
@param tol:
@param solver: solver to use
@return: boolean of whether x is in Q
"""
Q = pp.to_AH_polytope(Q)
prog = MP.MathematicalProgram()
zeta = prog.NewContinuousVariables(Q.P.H.shape[1], 1, "zeta")
prog.AddLinearConstraint(A=Q.P.H,
ub=Q.P.h + tol,
lb=-np.inf * np.ones((Q.P.h.shape[0], 1)),
vars=zeta)
assert (x.shape[1] == 1)
for i, xi in enumerate(x):
if not np.isnan(xi):
prog.AddLinearEqualityConstraint(np.atleast_2d(Q.T[i, :]),
(x[i] - Q.t[i]).reshape([-1, 1]),
zeta)
if solver == "gurobi":
result = gurobi_solver.Solve(prog, None, None)
elif solver == "osqp":
prog.AddQuadraticCost(np.eye(zeta.shape[0]), np.zeros(zeta.shape),
zeta)
result = OSQP_solver.Solve(prog, None, None)
else:
result = MP.Solve(prog)
return result.is_success()
def ray_shooting_hyperplanes_older(Q,N=0,H_rays=None):
"""
Ray Shooting to find an outer-approximation of the AH-polytope
"""
prog=MP.MathematicalProgram()
Q=pp.to_AH_polytope(Q)
if type(H_rays)==type(None):
if N==0:
N=2**(Q.n-1) # This many hyperplanes I want :)
H_rays=np.random.normal(size=(N,Q.n))
else:
N=H_rays.shape[0]
assert H_rays.shape[1]==Q.n
h_y=prog.NewContinuousVariables(2*N,1,"hy")
H_y=np.vstack(( H_rays , -H_rays ))
Y=pp.H_polytope(H_y,h_y)
pp.subset(prog,Q,Y)
prog.AddLinearCost(np.ones(2*N),np.array([0]),h_y)
result=gurobi_solver.Solve(prog,None,None)
if result.is_success():
h_y_n=result.GetSolution(h_y).reshape(2*N,1)
return pp.H_polytope(H_y,h_y_n)
else:
print("The polytope you gave me seems unbounded or \
there is another error")
def inner_optimization_new(Q,X=None,N=100,k=-1):
"""
Q= AH_polytope
X= H_polytope Candidate
"""
# Sanity Checks
assert Q.type=='AH_polytope' or Q.type=='V_polytope'
Q=pp.to_AH_polytope(Q)
if type(X)==type(None):
X=ray_shooting_hyperplanes(Q,N=N)
else:
assert X.type=='H_polytope'
# Program
n=Q.n
prog=MP.MathematicalProgram()
# T=prog.NewSymmetricContinuousVariables(Q.n,'T')
T=prog.NewContinuousVariables(n,n,"T")
t=prog.NewContinuousVariables(n,1,"t")
# prog.AddPositiveSemidefiniteConstraint(T)
Y=pp.AH_polytope(T=T,t=t,P=X)
pp.subset(prog,Y,Q,k=k,verbose=True)
result=volume_maximization(prog, T,0.1*np.eye(n)+0.1 )
if result.is_success():
print("success")
T_n= result.GetSolution(T)
t_n= result.GetSolution(t).reshape(n,1)
print("determinent=",np.linalg.det(T_n))
return pp.affine_map( T=T_n, P=X, t=t_n),np.linalg.det(T_n)
else:
print("not succesfull")
def convex_hull(P1, P2):
"""
Inputs:
P1, P2: AH_polytopes
Output:
returns :math:`\text{ConvexHull}(\mathbb{P}_1,\mathbb{P}_2)` as an AH-polytope
"""
Q1, Q2 = pp.to_AH_polytope(P1), pp.to_AH_polytope(P2)
T = np.hstack((Q1.T, Q2.T, Q1.t - Q2.t))
H_1 = np.hstack((Q1.P.H, np.zeros((Q1.P.H.shape[0], Q2.P.n)), -Q1.P.h))
H_2 = np.hstack((np.zeros((Q2.P.H.shape[0], Q1.P.n)), Q2.P.H, Q2.P.h))
H_3 = np.zeros((2, Q1.P.n + Q2.P.n + 1))
H_3[:, -1:] = np.array([1, -1]).reshape(2, 1)
H = np.vstack((H_1, H_2, H_3))
h = np.vstack((Q1.P.h * 0, Q2.P.h, 1, 0))
new_P = pp.H_polytope(H, h)
return pp.AH_polytope(T=T, t=Q2.t, P=new_P)
def alpha(X, Y, Theta):
X2 = pp.to_AH_polytope(X)
Y2 = pp.to_AH_polytope(Y)
prog = MP.MathematicalProgram()
alpha = prog.NewContinuousVariables(1, "alpha")
t = prog.NewContinuousVariables(X2.n, 1, "t")
Y2.P.h = Y2.P.h * alpha
X2.t = X2.t + t
subset(prog, X2, Y2, Theta=Theta)
prog.AddLinearCost(np.eye(1), np.zeros((1)), alpha)
result = gurobi_solver.Solve(prog, None, None)
if result.is_success():
# print("alpha test successful")
# print(result.GetSolution(alpha),result.GetSolution(t))
return 1 / result.GetSolution(alpha)[0]
else:
print("not a subset")
def minkowski_sum(P1, P2):
r"""
Inputs:
P1, P2: AH_polytopes
Returns:
returns the Mkinkowski sum :math:`P_1 \oplus P_2` as an AH-polytope.
**Background**: The Minkowski sum of two sets is defined as:
.. math::
A \oplus B = \{ a + b \big | a \in A, b \in B\}.
"""
Q1, Q2 = pp.to_AH_polytope(P1), pp.to_AH_polytope(P2)
T = np.hstack((Q1.T, Q2.T))
t = Q1.t + Q2.t
H = spa.block_diag(*[Q1.P.H, Q2.P.H])
h = np.vstack((Q1.P.h, Q2.P.h))
new_P = pp.H_polytope(H, h)
return pp.AH_polytope(t=t, T=T, P=new_P)
def _setup_program_distance_point(P, ball="infinity", solver="Gurobi"):
"""
Initilize the mathematial program
Choice of balls:
infinity: L-infinity norm
l1: l1 norm (Manhattan Distance)
l2: l2 norm (Euclidean Distance)
"""
if P.distance_program is None:
prog = MP.MathematicalProgram()
Q = pp.to_AH_polytope(P)
n = Q.n
x = np.zeros((n, 1))
P.zeta = prog.NewContinuousVariables(Q.P.H.shape[1], 1, "zeta")
delta = prog.NewContinuousVariables(n, 1, "delta")
prog.AddLinearConstraint(A=Q.P.H,
ub=Q.P.h,
lb=-np.inf * np.ones((Q.P.h.shape[0], 1)),
vars=P.zeta)
P.distance_constraint = prog.AddLinearEqualityConstraint(
np.hstack((Q.T, -np.eye(n))), x - Q.t, np.vstack((P.zeta, delta)))
if ball == "infinity":
delta_abs = prog.NewContinuousVariables(1, 1, "delta_abs")
prog.AddBoundingBoxConstraint(0, np.inf, delta_abs)
prog.AddLinearConstraint(
np.greater_equal(np.dot(np.ones((n, 1)), delta_abs),
delta,
dtype='object'))
prog.AddLinearConstraint(
np.greater_equal(np.dot(np.ones((n, 1)), delta_abs),
-delta,
dtype='object'))
prog.AddLinearCost(delta_abs[0, 0])
elif ball == "l1":
delta_abs = prog.NewContinuousVariables(n, 1, "delta_abs")
prog.AddBoundingBoxConstraint(0, np.inf, delta_abs)
prog.AddLinearConstraint(
np.greater_equal(delta_abs, delta, dtype='object'))
prog.AddLinearConstraint(
np.greater_equal(delta_abs, -delta, dtype='object'))
cost = np.dot(np.ones((1, n)), delta_abs)
prog.AddLinearCost(cost[0, 0])
elif ball == "l2":
prog.AddQuadraticCost(np.eye(n), np.zeros(n), delta)
else:
print(("Not a valid choice of norm", str(ball)))
raise NotImplementedError
P.distance_program = prog
return
else:
return
def AH_polytope_vertices(P, N=200, epsilon=0.001, solver="Gurobi"):
"""
Returns N*2 matrix of vertices
"""
try:
P.vertices_2D
if type(P.vertices_2D) == type(None):
raise Exception
except:
Q = pp.to_AH_polytope(P)
v = np.empty((N, 2))
prog = MP.MathematicalProgram()
zeta = prog.NewContinuousVariables(Q.P.H.shape[1], 1, "zeta")
prog.AddLinearConstraint(A=Q.P.H,
ub=Q.P.h,
lb=-np.inf * np.ones((Q.P.h.shape[0], 1)),
vars=zeta)
theta = 1
c = np.array([np.cos(theta), np.sin(theta)]).reshape(2, 1)
c_T = np.dot(c.T, Q.T)
get_nonzero_cost_vectors(c_T)
# get_nonzero_cost_vectors(c_T)
a = prog.AddLinearCost(np.dot(c_T, zeta)[0, 0])
if solver == "Gurobi":
solver = gurobi_solver
else:
raise NotImplementedError
for i in range(N):
theta = i * N / 2 / np.pi + 0.01
c = np.array([np.cos(theta), np.sin(theta)]).reshape(2, 1)
c_T = np.dot(c.T, Q.T)
e = a.evaluator()
cost = c_T.reshape(Q.P.H.shape[1])
get_nonzero_cost_vectors(cost)
e.UpdateCoefficients(cost)
result = solver.Solve(prog, None, None)
assert result.is_success()
zeta_n = result.GetSolution(zeta).reshape(zeta.shape)
v[i, :] = (np.dot(Q.T, zeta_n) + Q.t).reshape(2)
w = np.empty((4 * N, 2))
for i in range(N):
w[4 * i, :] = v[i, :] + np.array([epsilon, epsilon])
w[4 * i + 1, :] = v[i, :] + np.array([-epsilon, epsilon])
w[4 * i + 2, :] = v[i, :] + np.array([-epsilon, -epsilon])
w[4 * i + 3, :] = v[i, :] + np.array([epsilon, -epsilon])
P.vertices_2D = v, w
return v, w
else:
return P.vertices_2D
def member_in_set(program, x, P):
"""
Inputs:
@ program: mathematical program
# x: point
# P: my polytope
Adds the constraint ..math::`x in P` to the mathematical program
"""
P = pp.to_AH_polytope(P)
x.reshape(len(x), 1)
zeta = program.NewContinuousVariables(P.P.n, 1, 'zeta')
program.AddLinearConstraint(A=P.P.H,
lb=-np.inf * np.ones((P.P.h.shape[0], 1)),
ub=P.P.h,
vars=zeta)
_f = np.equal(np.dot(P.T, zeta), x - P.t, dtype='object')
program.AddLinearConstraint(_f.flatten())
def check_non_empty(Q, tol=10**-5, solver="gurobi"):
Q = pp.to_AH_polytope(Q)
prog = MP.MathematicalProgram()
zeta = prog.NewContinuousVariables(Q.P.H.shape[1], 1, "zeta")
prog.AddLinearConstraint(A=Q.P.H,
ub=Q.P.h + tol,
lb=-np.inf * np.ones((Q.P.h.shape[0], 1)),
vars=zeta)
if solver == "gurobi":
result = gurobi_solver.Solve(prog, None, None)
elif solver == "osqp":
prog.AddQuadraticCost(np.eye(zeta.shape[0]), np.zeros(zeta.shape),
zeta)
result = OSQP_solver.Solve(prog, None, None)
else:
result = MP.Solve(prog)
return result.is_success()
def theta_k(circumbody, k=0, i=0):
"""
This is from the Section 4 of the paper [Sadraddini and Tedrake, 2020].
Inputs:
* AH-polytope: ``circumbody``
* int ``k``: the number of columns added to the subspace of ``U``.
For more information, look at the paper.
*Default* is 0.
Outputs:
* 2D numpy array ``Theta``, which is defined in the paper
"""
circumbody_AH = pp.to_AH_polytope(circumbody)
H_y = circumbody_AH.P.H
q_y = H_y.shape[0]
if k < 0:
return np.eye(q_y)
Y = circumbody_AH.T
# First identify K=[H_y'^Y'+ ker(H_y')]
S_1 = np.dot(np.linalg.pinv(H_y.T), Y.T)
S_2 = spa.null_space(H_y.T)
if S_2.shape[1] > 0:
S = np.hstack((S_1, S_2))
else:
S = S_1
# phiw>=0. Now with the augmentation
S_complement = spa.null_space(S.T)
circumbody.dim_complement = S_complement.shape[1]
number_of_columns = min(k, S_complement.shape[1])
if k == 0:
psi = S
else:
psi = np.hstack((S, S_complement[:, i:number_of_columns + i]))
p_mat = Matrix(np.hstack((np.zeros((psi.shape[0], 1)), psi)))
p_mat.rep_type = RepType.INEQUALITY
poly = Polyhedron(p_mat)
R = np.array(poly.get_generators())
r = R[:, 1:].T
Theta = np.dot(psi, r)
assert np.all(Theta > -1e-5)
return Theta
def unitball(N, norm="infinity"):
"""
Returns N-dimesional unit ball as the form of a H-polytope
"""
if norm == "infinity":
H = np.vstack((np.eye(N), -np.eye(N)))
h = np.ones((2 * N, 1))
return H_polytope(H, h)
elif norm in ["L1", '1', 'l1', 1]:
if False:
list_V = []
for n in range(N):
v1 = np.zeros((N, 1))
v2 = np.zeros((N, 1))
v1[n] = 1
v2[n] = -1
list_V.append(v1)
list_V.append(v2)
U = pp.V_polytope(list_V)
return pp.to_AH_polytope(U)
else:
H = np.array(list(product([1, -1], repeat=N)))
h = np.ones((2**N, 1))
return H_polytope(H, h)
else:
print('The ', norm, " is not identified. Use Infinity or L1")
raise NotImplementedError
#def simplex(n):
# """
# A N-dimensional standard simplex. Represented in V shape
# """
# v0=np.zeros((n,1))
# V=[0]*(2*n+1)
# V[0]=v0
# for i in range(n):
# V[2*i+1]=np.zeros((n,1))
# V[2*i+1][i,0]=1
# V[2*i+2]=np.zeros((n,1))
# V[2*i+2][i,0]=-1
# return pp.V_polytope(V)
def point_membership(Q, x, tol=10**-5, solver="gurobi"):
if type(Q).__name__ == "H_polytope":
return Q.if_inside(x, tol)
else:
Q = pp.to_AH_polytope(Q)
prog = MP.MathematicalProgram()
zeta = prog.NewContinuousVariables(Q.P.H.shape[1], 1, "zeta")
prog.AddLinearConstraint(A=Q.P.H,
ub=Q.P.h + tol,
lb=-np.inf * np.ones((Q.P.h.shape[0], 1)),
vars=zeta)
prog.AddLinearEqualityConstraint(Q.T, x - Q.t, zeta)
if solver == "gurobi":
result = gurobi_solver.Solve(prog, None, None)
elif solver == "osqp":
prog.AddQuadraticCost(np.eye(zeta.shape[0]), np.zeros(zeta.shape),
zeta)
result = OSQP_solver.Solve(prog, None, None)
else:
result = MP.Solve(prog)
return result.is_success()
def affine_map(T, P, t=None, get_inverse=True):
"""
Returns the affine map of a polytope.
"""
if type(t) == type(None):
t = np.zeros((T.shape[0], 1))
if P.type == 'AH_polytope':
return pp.AH_polytope(t=t + np.dot(T, P.t), T=np.dot(T, P.T), P=P.P)
elif P.type == 'zonotope':
return pp.zonotope(x=t + np.dot(T, P.x), G=np.dot(T, P.G))
elif P.type == "H_polytope":
if T.shape[0] >= T.shape[1] and get_inverse:
Tinv = np.linalg.pinv(T)
H = np.dot(P.H, Tinv)
# print("inverse error=",np.linalg.norm(np.dot(Tinv,T)-np.eye(T.shape[1])))
assert np.linalg.norm(np.dot(Tinv, T) -
np.eye(T.shape[1])) <= 1e-2 * P.n
return pp.H_polytope(H=H, h=P.h + np.dot(H, t))
else:
Q = pp.to_AH_polytope(P)
return affine_map(T, Q, t)
else:
return ValueError('Polytope type: ', P.type, " Not recognized")
def convex_hull_of_point_and_polytope(x, Q):
r"""
Inputs:
x: numpy n*1 array
Q: AH-polytope in R^n
Returns:
AH-polytope representing convexhull(x,Q)
.. math::
\text{conv}(x,Q):=\{y | y= \lambda q + (1-\lambda) x, q \in Q\}.
"""
Q = pp.to_AH_polytope(Q)
q = Q.P.H.shape[1]
new_T = np.hstack((Q.T, Q.t - x))
new_t = x
new_H_1 = np.hstack((Q.P.H, -Q.P.h))
new_H_2 = np.zeros((2, q + 1))
new_H_2[0, q], new_H_2[1, q] = 1, -1
new_H = np.vstack((new_H_1, new_H_2))
new_h = np.zeros((Q.P.h.shape[0] + 2, 1))
new_h[Q.P.h.shape[0], 0], new_h[Q.P.h.shape[0] + 1, 0] = 1, 0
new_P = pp.H_polytope(new_H, new_h)
return pp.AH_polytope(new_T, new_t, new_P)
def outer_optimization(Q,X=None,N=100,k=-1):
"""
Q= AH_polytope
X= H_polytope Candidate
"""
# Sanity Checks
assert Q.type=='AH_polytope' or Q.type=='V_polytope'
Q=pp.to_AH_polytope(Q)
if type(X)==type(None):
X=ray_shooting_hyperplanes(Q,N=N)
else:
assert X.type=='H_polytope'
# Program
n=Q.n
prog=MP.MathematicalProgram()
# T=prog.NewSymmetricContinuousVariables(Q.n,'T')
T=prog.NewContinuousVariables(n,n,"T")
t=prog.NewContinuousVariables(n,1,"t")
# prog.AddPositiveSemidefiniteConstraint(T)
prog.AddMaximizeLogDeterminantSymmetricMatrixCost(T)
Q_new=pp.AH_polytope(T=np.dot(T,Q.T),t=np.dot(T,Q.t)+t,P=Q.P)
pp.subset(prog,Q_new,X,k=k,verbose=True)
result=scs_solver.Solve(prog,None,None)
if result.is_success():
print("success")
T_n= result.GetSolution(T)
t_n= result.GetSolution(t).reshape(n,1)
Tinv=np.linalg.inv(T_n)
t_new=np.dot(-Tinv,t_n)
print("determinent=",np.linalg.det(T_n))
# Q_new_n=pp.AH_polytope(T=np.dot(T_n,Q.T),t=np.dot(T_n,Q.t)+t_n,P=Q.P)
# return Q_new_n
return pp.affine_map( T=Tinv, P=X, t=t_new),np.linalg.det(Tinv)
# return pp.AH_polytope(T=Tinv,t=t_new,P=X)
else:
print("not succesfull")
def subset(program,
inbody,
circumbody,
k=-1,
Theta=None,
i=0,
alpha=None,
verbose=False):
"""
Adds containment property Q1 subset Q2
Inputs:
* program: a `pydrake` mathematical program
* inbody: a polytopic object
* circumbody: a polytopic object
* N:
* **Default**: :math:`-1``. Sufficient as in [Sadraddini and Tedrake, 2019]
* pick `0` for necessary and sufficient encoding (may be too slow)
as in [Sadraddini and Tedrake, 2019]
* pick any positive number. As the number is smaller,
the condition becomes closer to necessity. However, this may be too slow.
Output:
* No direct output, adds :\math:`inbody \subseteq circumbody` to the model
"""
Q1 = pp.to_AH_polytope(inbody)
Q2 = pp.to_AH_polytope(circumbody)
Hx, Hy, hx, hy, X, Y, xbar, ybar = Q1.P.H, Q2.P.H, Q1.P.h, Q2.P.h, Q1.T, Q2.T, Q1.t, Q2.t
qx, qy, nx, ny = Hx.shape[0], Hy.shape[0], X.shape[1], Y.shape[1]
if type(Theta) != type(None):
if verbose:
print("theta already computed")
pass
elif k < 0:
Theta = np.eye(qy)
if verbose:
print("Using Positive Orthant")
else:
if verbose:
print("*" * 20)
print("\n")
print("Computing theta with k=%d" % k)
Theta = theta_k(circumbody, k, i)
if verbose:
print("The maximum for k can be ", circumbody.dim_complement)
print("Theta Size is=", Theta.shape)
print("*" * 20)
print("\n")
Lambda = program.NewContinuousVariables(Theta.shape[1], qx, 'Lambda')
Gamma = program.NewContinuousVariables(ny, nx, 'Gamma')
gamma = program.NewContinuousVariables(ny, 1, 'gamma')
# Constraints
program.AddBoundingBoxConstraint(0, np.inf, Lambda) # Lambda Non-Negative
program.AddLinearConstraint(
np.equal(X, np.dot(Y, Gamma), dtype='object').flatten()) #X=YGamma
program.AddLinearConstraint(
np.equal(ybar - xbar, np.dot(Y, gamma), dtype='object').flatten())
program.AddLinearConstraint(
np.equal(np.dot(Lambda, Hx),
np.dot(Theta.T, np.dot(Hy, Gamma)),
dtype='object').flatten())
program.AddLinearConstraint(np.less_equal(np.dot(Lambda,hx),\
np.dot(Theta.T,hy)+np.dot(Theta.T,np.dot(Hy,gamma)),dtype='object').flatten())
return Theta, Lambda, Gamma, gamma
def necessity_gap_k(X, Y, plot=True, only_final=False):
if only_final == False:
print("\n\n", "=" * 75, "\n", "=" * 75,
"\n\t\tComputing Necessity Gaps ")
print("=" * 75, "\n", "=" * 75)
print("k\tTheta.shape \t delta(X,Y,C) \t alpha \t Computation Time")
print("-" * 75)
Theta_star = theta_k(Y, k=0)
alpha_0 = alpha(X, Y, Theta_star)
Table = {}
Z = pp.to_AH_polytope(Y)
kappa = int(Z.T.shape[1] - Z.T.shape[0])
if only_final:
t_0 = time.time()
Z = pp.to_AH_polytope(Y)
Theta = np.eye(Z.P.H.shape[0])
a = alpha(X, Y, Theta)
delta = 1 - a / alpha_0
cpu_time = time.time() - t_0
result = np.array([Theta_star.shape[1], delta, a, cpu_time])
return result
else:
for k in range(kappa + 1):
t_0 = time.time()
Theta = theta_k(Y, k)
# print(k,"theta shape",Theta.shape,Theta)
a = alpha(X, Y, Theta)
delta = 1 - a / alpha_0
cpu_time = time.time() - t_0
print(k, "\t", Theta.shape, "\t %0.03f" % abs(delta),
"\t\t %0.03f" % a, "\t\t %0.003f" % cpu_time)
Table[k] = np.array([Theta.shape[1], delta, a, cpu_time])
if plot:
import matplotlib.pyplot as plt
plt.figure()
fig, ax1 = plt.subplots()
color = (0.7, 0, 0)
ax1.set_xlabel(r'$k$', FontSize=20)
ax1.set_ylabel(r'#Rows in $\Theta_k$', color=color, FontSize=20)
ax1.plot(range(kappa + 1), [Table[k][0] for k in range(kappa + 1)],
'-',
color=color)
ax1.plot(range(kappa + 1), [Table[k][0] for k in range(kappa + 1)],
'o',
color=color)
ax1.tick_params(axis='y', labelcolor=color, labelsize=10)
ax1.tick_params(axis='x', labelsize=10)
ax2 = ax1.twinx(
) # instantiate a second axes that shares the same x-axis
color = (0, 0, 0.7)
ax2.set_ylabel(r'$\delta(\mathbb{X},\mathbb{Y},\mathbb{C}_k)$',
color=color,
FontSize=20)
ax2.plot(range(kappa + 1), [Table[k][1] for k in range(kappa + 1)],
'-',
color=color)
ax2.plot(range(kappa + 1), [Table[k][1] for k in range(kappa + 1)],
'o',
color=color)
ax2.tick_params(axis='y', labelcolor=color, labelsize=10)
ax2.set_title(r'$ n=%d$' % (Z.n), FontSize=20)
ax1.grid()
fig.tight_layout(
) # otherwise the right y-label is slightly clipped
return Table
def subset_kasra(program,
inbody,
circumbody,
k=-1,
Theta=None,
i=0,
alpha=None,
verbose=False):
"""
Adds containment property Q1 subset Q2
Inputs:
* program: a `pydrake` mathematical program
* inbody: a polytopic object
* circumbody: a polytopic object
* N:
* **Default**: :math:`-1``. Sufficient as in [Sadraddini and Tedrake, 2020]
* pick `0` for necessary and sufficient encoding (may be too slow) (2019b)
* pick any positive number. As the number is smaller,
the condition becomes closer to necessity. However, this may be too slow.
Output:
* No direct output, adds :\math:`inbody \subseteq circumbody` to the model
"""
if type(inbody).__name__ == "zonotope" and type(
circumbody).__name__ == "zonotope":
"""
For the case when both inbody and circumbody sets are zonotope:
"""
from itertools import product
#Defining Variables
Gamma = program.NewContinuousVariables(circumbody.G.shape[1],
inbody.G.shape[1], 'Gamma')
Lambda = program.NewContinuousVariables(circumbody.G.shape[1],
'Lambda')
#Defining Constraints
program.AddLinearConstraint(
np.equal(
inbody.G, np.dot(circumbody.G, Gamma),
dtype='object').flatten()) #inbody_G = circumbody_G * Gamma
program.AddLinearConstraint(
np.equal(circumbody.x - inbody.x,
np.dot(circumbody.G, Lambda),
dtype='object').flatten()
) #circumbody_x - inbody_x = circumbody_G * Lambda
Gamma_Lambda = np.concatenate(
(Gamma, Lambda.reshape(circumbody.G.shape[1], 1)), axis=1)
comb = np.array(list(product([-1, 1],
repeat=Gamma_Lambda.shape[1]))).reshape(
-1, Gamma_Lambda.shape[1])
if alpha == 'scalar' or alpha == 'vector':
comb = np.concatenate((comb, -1 * np.ones((comb.shape[0], 1))),
axis=1)
# Managing alpha
if alpha == None:
variable = Gamma_Lambda
elif alpha == 'scalar':
alfa = program.NewContinuousVariables(1, 'alpha')
elif alpha == 'vector':
alfa = program.NewContinuousVariables(circumbody.G.shape[1],
'alpha')
variable = np.concatenate((Gamma_Lambda, alfa.reshape(-1, 1)),
axis=1)
else:
raise ValueError(
'alpha needs to be \'None\', \'scalaer\', or \'vector\'')
# infinity norm of matrxi [Gamma,Lambda] <= alfa
for j in range(Gamma_Lambda.shape[0]):
program.AddLinearConstraint(
A=comb,
lb=-np.inf * np.ones(comb.shape[0]),
ub=np.ones(comb.shape[0])
if alpha == None else np.zeros(comb.shape[0]),
vars=variable[j, :] if alpha != 'scalar' else np.concatenate(
(Gamma_Lambda[j, :], alfa)))
#from pydrake.symbolic import abs as exp_abs
# Gamma_abs = np.array([exp_abs(Gamma[i,j]) for i in range(circumbody.G.shape[1]) for j in range(inbody.G.shape[1])]).reshape(*Gamma.shape)
# Lambda_abs = np.array([exp_abs(Lambda[i]) for i in range(circumbody.G.shape[1])]).reshape(circumbody.G.shape[1],1)
# Gamma_lambda_abs = np.concatenate((Gamma_abs,Lambda_abs),axis=1)
# infnorm_Gamma_lambda_abs = np.sum(Gamma_lambda_abs,axis=1)
#[program.AddConstraint( infnorm_Gamma_lambda_abs[i] <= 1) for i in range(circumbody.G.shape[1])]
#program.AddBoundingBoxConstraint(-np.inf * np.ones(circumbody.G.shape[1]) , np.ones(circumbody.G.shape[1]), infnorm_Gamma_lambda_abs)
# program.AddLinearConstraint(
# A=np.eye(circumbody.G.shape[1]),
# lb= -np.inf * np.ones(circumbody.G.shape[1]),
# ub=np.ones(circumbody.G.shape[1]),
# vars=infnorm_Gamma_lambda_abs
# )
if alpha == None:
return Lambda, Gamma
else:
return Lambda, Gamma, alfa
Q1 = pp.to_AH_polytope(inbody)
Q2 = pp.to_AH_polytope(circumbody)
Hx, Hy, hx, hy, X, Y, xbar, ybar = Q1.P.H, Q2.P.H, Q1.P.h, Q2.P.h, Q1.T, Q2.T, Q1.t, Q2.t
qx, qy, nx, ny = Hx.shape[0], Hy.shape[0], X.shape[1], Y.shape[1]
if type(Theta) != type(None):
if verbose:
print("theta already computed")
pass
elif k < 0:
Theta = np.eye(qy)
if verbose:
print("Using Positive Orthant")
else:
if verbose:
print("Computing theta with k=%d" % k)
Theta = theta_k(circumbody, k, i)
Lambda = program.NewContinuousVariables(Theta.shape[1], qx, 'Lambda')
Gamma = program.NewContinuousVariables(ny, nx, 'Gamma')
gamma = program.NewContinuousVariables(ny, 1, 'gamma')
# Constraints
program.AddBoundingBoxConstraint(0, np.inf, Lambda) # Lambda Non-Negative
program.AddLinearConstraint(
np.equal(X, np.dot(Y, Gamma), dtype='object').flatten()) #X=YGamma
program.AddLinearConstraint(
np.equal(ybar - xbar, np.dot(Y, gamma), dtype='object').flatten())
program.AddLinearConstraint(
np.equal(np.dot(Lambda, Hx),
np.dot(Theta.T, np.dot(Hy, Gamma)),
dtype='object').flatten())
program.AddLinearConstraint(np.less_equal(np.dot(Lambda,hx),\
np.dot(Theta.T,hy)+np.dot(Theta.T,np.dot(Hy,gamma)),dtype='object').flatten())
return Theta, Lambda, Gamma, gamma
