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