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 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 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 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 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 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")
x = np.array([4, 0]).reshape(2, 1) # offset G = np.array([[1, 0, 0.5], [0, 0.5, -1]]).reshape(2, 3) C = pp.zonotope(x=x, G=G) pp.visualize([C], title=r'$C$') plt.show() x = np.array([-1, -2]).reshape(2, 1) # offset G = np.array([[0, 1, 0.707, 0.293, 0.293, 0.707], [-1, 0, -0.293, 0.707, -0.707, 0.293]]).reshape(2, 6) C = pp.zonotope(x=x, G=G) pp.visualize([C], title=r'$C$') plt.show() H = np.array([[1, 1], [-1, 1], [0, -1], [2, 3]]) h = np.array([1, 1, 0, 1]) A = pp.H_polytope(H, h) # pp.visualize([A],title=r'$A$') # D = pp.operations.intersection(A, A) # pp.visualize([D]) # D=pp.operations.convex_hull(A,C) # D = pp.operations.check_subset(C, C) # D.color=(0.9, 0.9, 0.1) # pp.visualize([D,A, C],title=r'$A$ (red),$C$ (blue), $D=A\oplus C$ (yellow)') # t=np.array([5,0]).reshape(2,1) # offset # theta=np.pi/6 # 30 degrees # T=np.array([[np.cos(theta),np.sin(theta)],[-np.sin(theta),np.cos(theta)]]) # Linear transformation # B=pp.AH_polytope(t,T,A) # pp.visualize([B],title=r'$B$')
def __rmul__(self, scalar): """ Scaling polytopes by a scalar. The scalar needs to be an integer or a float. """ return pp.H_polytope(self.H, self.h * scalar)
def polytopic_trajectory(system, start, T, list_of_goals, q=None, Q=None, R=None): n, m = system.n, system.m if type(Q) == type(None): Q = np.eye(n) if type(R) == type(None): R = np.eye(m) if type(q) == type(None): q = n S = list(system.modes) S_all = S + ['all'] prog = MP.MathematicalProgram() x={(i,t): prog.NewContinuousVariables(n,1,"x%s%d"%(i,t)) \ for i in S_all for t in range(T+1)} u={(i,t): prog.NewContinuousVariables(m,1,"x%s%d"%(i,t)) \ for i in S_all for t in range(T)} mu={(i,t): prog.NewBinaryVariables(1,1,"x%s%d"%(i,t)) \ for i in S for t in range(T)} G={(i,t): prog.NewContinuousVariables(n,q,"x%s%d"%(i,t)) \ for i in S_all for t in range(T+1)} theta={(i,t): prog.NewContinuousVariables(m,q,"x%s%d"%(i,t)) \ for i in S_all for t in range(T)} # Containment for i in S: for t in range(T): XU = system.modes[i].XU xu = np.vstack((x[i, t], u[i, t])) Gtheta = np.vstack((G[i, t], theta[i, t])) inbody = pp.zonotope(x=xu, G=Gtheta) circumbody = pp.H_polytope(XU.H, XU.h * mu[i, t]) pp.subset(prog, inbody, circumbody) # Dynamics of point for t in range(T): _M=np.hstack([np.hstack((-system.modes[i].A,-system.modes[i].B,\ -system.modes[i].c)) for i in S]\ + [np.eye(n)]) _v = np.vstack([np.vstack((x[i, t], u[i, t], mu[i, t])) for i in S] + [x['all', t + 1]]) prog.AddLinearEqualityConstraint(_M, np.zeros((n, 1)), _v) # Dynamics of polytopes _M=np.hstack([np.hstack((-system.modes[i].A,-system.modes[i].B)) for i in S]\ + [np.eye(n)]) _v = np.vstack([np.vstack((G[i, t], theta[i, t])) for i in S] + [G['all', t + 1]]) for j in range(q): prog.AddLinearEqualityConstraint(_M, np.zeros((n, 1)), _v[:, j]) # Summation Equation for t in range(T): _u = np.vstack([u[i, t] for i in S_all]) _uI = np.hstack([np.eye(system.m) for i in S] + [-np.eye(m)]) # Very non-efficient prog.AddLinearEqualityConstraint(_uI, np.zeros((m, 1)), _u) _theta = np.vstack([theta[i, t] for i in S_all]) for j in range(q): prog.AddLinearEqualityConstraint(_uI, np.zeros((m, 1)), _theta[:, j]) _mu = np.vstack([mu[i, t] for i in S]) prog.AddLinearEqualityConstraint(np.ones((1, len(S))), np.ones((1, 1)), _mu) for t in range(T + 1): _x = np.vstack([x[i, t] for i in S_all]) _xI = np.hstack([np.eye(n) for i in S] + [-np.eye(n)]) prog.AddLinearEqualityConstraint(_xI, np.zeros((n, 1)), _x) _G = np.vstack([G[i, t] for i in S_all]) for j in range(q): prog.AddLinearEqualityConstraint(_xI, np.zeros((n, 1)), _G[:, j]) # start prog.AddLinearConstraint( np.equal(x['all', 0], start, dtype='object').flatten()) # end mu_d, t_d, T_d = pp.add_disjunctive_subsets( prog, pp.zonotope(x=x['all', T], G=G['all', T]), list_of_goals) # pp.subset(prog, pp.zonotope(x=x['all',T],G=G['all',T]), goal) # Cost function for t in range(T + 1): prog.AddQuadraticCost(Q, np.zeros(n), x['all', t]) for t in range(T): prog.AddQuadraticCost(R, np.zeros(n), u['all', t]) # Volume Optimization prog.AddLinearCost(G['all', 0][0, 0] * 10 + G['all', 0][1, 1] * 1) print("*" * 10, " Set up a mixed-integer optimization problem", "*" * 10) # solve and result result = gurobi_solver.Solve(prog, None, None) if result.is_success(): print('polytopic trajectory optimization succesfull') x_n = {t: result.GetSolution(x["all", t]) for t in range(T + 1)} u_n = {t: result.GetSolution(u["all", t]) for t in range(T)} mu_n = {(t, i): result.GetSolution(mu[i, t]).item() for i in S for t in range(T)} G_n = {t: result.GetSolution(G["all", t]) for t in range(T + 1)} theta_n = {t: result.GetSolution(theta["all", t]) for t in range(T)} # Disjunctive Sets # print(mu_d,type(mu_d)) # print({result.GetSolution(i) for i in mu_d}) # for i in t_d: # print(result.GetSolution(t_d[i])) # print(result.GetSolution(T_d[i])) return x_n, u_n, mu_n, G_n, theta_n else: print('polytopic trajectory optimization failed') return