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")