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 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 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 translate(t, P):
"""
Shifts the polytope by t vector
"""
assert t.shape[0] == P.n # Dimension match
if P.type == 'AH_polytope':
return pp.AH_polytope(t=t + P.t, T=P.T, P=P.P)
elif P.type == 'zonotope':
return pp.zonotope(x=t + P.x, G=P.G)
elif P.type == "H_polytope":
return pp.H_polytope(H=P.H, h=P.h + np.dot(P.H, t))
else:
return ValueError('Polytope type: ', P.type, " Not recognized")
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 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 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 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 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 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 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")
