From ff90a464908517631f447852cc5f830b590bfa96 Mon Sep 17 00:00:00 2001 From: Kasra Ghasemi Date: Wed, 9 Sep 2020 14:37:20 -0400 Subject: [PATCH 1/7] adding zonotope_subset() and gurobi --- pypolycontain/containment.py | 157 +++++++++++++++++++++-------------- 1 file changed, 94 insertions(+), 63 deletions(-) diff --git a/pypolycontain/containment.py b/pypolycontain/containment.py index 7e739fe..27053dc 100644 --- a/pypolycontain/containment.py +++ b/pypolycontain/containment.py @@ -26,6 +26,10 @@ # pass # warnings.warn("You don't have pypolycontain not properly installed.") +try: + from gurobi import * +except: + warnings.warn("You don't have gurobi not properly installed.") try: import pydrake.solvers.mathematicalprogram as MP import pydrake.solvers.gurobi as Gurobi_drake @@ -117,7 +121,7 @@ def member_in_set(program,x,P): -def subset(program,inbody,circumbody,k=-1,Theta=None,i=0,alpha=None,verbose=False): +def subset(program,inbody,circumbody,k=-1,Theta=None,i=0,verbose=False): """ Adds containment property Q1 subset Q2 @@ -134,67 +138,6 @@ def subset(program,inbody,circumbody,k=-1,Theta=None,i=0,alpha=None,verbose=Fals 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 @@ -295,4 +238,92 @@ def alpha(X,Y,Theta): # print(result.GetSolution(alpha),result.GetSolution(t)) return 1/result.GetSolution(alpha)[0] else: - print("not a subset") \ No newline at end of file + print("not a subset") + + +def zonotope_subset(program,inbody,circumbody,alpha=None,solver='drake'): + """ + For the case when both inbody and circumbody sets are zonotope, it introduces some constraints so that inbody \subset circumbody + Inputs are inbody and circumbody as zonotopes. + The especial case is when alpha is 'scalar' or 'vector', in those case it defines alpha as a variable or a vecotor of variables and + force the followinf relation: inbody \subset zontope(x=circumbody.x , G= (circumbody.G * diag(alpha) ) ) + """ + + assert(type(inbody).__name__=="zonotope" and type(circumbody).__name__=="zonotope"),"Both inbody and circumbody need to be in the form of zonotopes" + assert(inbody.x.shape==circumbody.x.shape),"Both input zonotopes need to be in the same dimension" + + if solver =='drake': + + 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 )) + ) + + if alpha==None: + return Lambda, Gamma + else: + return Lambda, Gamma , alfa + + + + elif solver=='gurobi': + + # from gurobipy import * + + Gamma = program.addVars(circumbody.G.shape[1] , inbody.G.shape[1] ,lb= -GRB.INFINITY, ub= GRB.INFINITY) + beta = [program.addVar( lb = -GRB.INFINITY , ub = GRB.INFINITY) for i in range( circumbody.G.shape[1] ) ] + Gamma_abs = program.addVars( circumbody.G.shape[1] , inbody.G.shape[1] ,lb= 0, ub= GRB.INFINITY) + beta_abs = [program.addVar( lb = 0 , ub = GRB.INFINITY) for i in range(circumbody.G.shape[1]) ] + program.update() + + program.addConstrs( inbody.G[i][j] == sum([ circumbody.G[i][k]*Gamma[k,j] for k in range(circumbody.G.shape[1]) ]) for i in range(inbody.G.shape[0]) for j in range(inbody.G.shape[1]) ) + program.addConstrs( sum( [ circumbody.G[i][j] * beta[j] for j in range(circumbody.G.shape[1]) ] ) == circumbody.x[i][0] - inbody.x[i][0] for i in range(inbody.G.shape[0]) ) + [program.addGenConstrAbs(Gamma_abs[i,j], Gamma[i,j] ) for i in range(circumbody.G.shape[1]) for j in range(inbody.G.shape[1]) ] + [program.addGenConstrAbs(beta_abs[i] , beta[i] ) for i in range(circumbody.G.shape[1])] + + if alpha == None: + program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= 1 for i in range(circumbody.G.shape[1]) ) + program.update() + return beta , Gamma + + elif alpha == 'scalar': + Alpha = program.addVar(lb=0,ub=1) + program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= Alpha for i in range(circumbody.G.shape[1]) ) + elif alpha == 'vector': + Alpha = [program.addVar(lb=0,ub=1) for i in range(circumbody.G.shape[1])] + program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= Alpha[i] for i in range(circumbody.G.shape[1]) ) + + program.update() + + return beta , Gamma , Alpha + From 41df30282cc5562a134bba8677c19431360a2a2b Mon Sep 17 00:00:00 2001 From: Kasra Ghasemi Date: Wed, 9 Sep 2020 18:20:29 -0400 Subject: [PATCH 2/7] removing the bounds over alpha in zonotope_subset() --- pypolycontain/containment.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pypolycontain/containment.py b/pypolycontain/containment.py index 27053dc..a30a6d8 100644 --- a/pypolycontain/containment.py +++ b/pypolycontain/containment.py @@ -317,10 +317,10 @@ def zonotope_subset(program,inbody,circumbody,alpha=None,solver='drake'): return beta , Gamma elif alpha == 'scalar': - Alpha = program.addVar(lb=0,ub=1) + Alpha = program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY) program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= Alpha for i in range(circumbody.G.shape[1]) ) elif alpha == 'vector': - Alpha = [program.addVar(lb=0,ub=1) for i in range(circumbody.G.shape[1])] + Alpha = [program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY) for i in range(circumbody.G.shape[1])] program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= Alpha[i] for i in range(circumbody.G.shape[1]) ) program.update() From da69668806c339457e171a7ca66b39ca7817f7dd Mon Sep 17 00:00:00 2001 From: Kasra Ghasemi Date: Thu, 10 Sep 2020 14:32:18 -0400 Subject: [PATCH 3/7] fixing zonotope_subset() with gurobi --- pypolycontain/containment.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/pypolycontain/containment.py b/pypolycontain/containment.py index a30a6d8..082fd6d 100644 --- a/pypolycontain/containment.py +++ b/pypolycontain/containment.py @@ -27,7 +27,7 @@ # warnings.warn("You don't have pypolycontain not properly installed.") try: - from gurobi import * + from gurobipy import * except: warnings.warn("You don't have gurobi not properly installed.") try: @@ -297,8 +297,6 @@ def zonotope_subset(program,inbody,circumbody,alpha=None,solver='drake'): elif solver=='gurobi': - - # from gurobipy import * Gamma = program.addVars(circumbody.G.shape[1] , inbody.G.shape[1] ,lb= -GRB.INFINITY, ub= GRB.INFINITY) beta = [program.addVar( lb = -GRB.INFINITY , ub = GRB.INFINITY) for i in range( circumbody.G.shape[1] ) ] @@ -307,7 +305,7 @@ def zonotope_subset(program,inbody,circumbody,alpha=None,solver='drake'): program.update() program.addConstrs( inbody.G[i][j] == sum([ circumbody.G[i][k]*Gamma[k,j] for k in range(circumbody.G.shape[1]) ]) for i in range(inbody.G.shape[0]) for j in range(inbody.G.shape[1]) ) - program.addConstrs( sum( [ circumbody.G[i][j] * beta[j] for j in range(circumbody.G.shape[1]) ] ) == circumbody.x[i][0] - inbody.x[i][0] for i in range(inbody.G.shape[0]) ) + program.addConstrs( sum( [ circumbody.G[i][j] * beta[j] for j in range(circumbody.G.shape[1]) ] ) == circumbody.x[i] - inbody.x[i] for i in range(inbody.G.shape[0]) ) [program.addGenConstrAbs(Gamma_abs[i,j], Gamma[i,j] ) for i in range(circumbody.G.shape[1]) for j in range(inbody.G.shape[1]) ] [program.addGenConstrAbs(beta_abs[i] , beta[i] ) for i in range(circumbody.G.shape[1])] From b6c345bd5028f8ad3ac91745f458786d041c808b Mon Sep 17 00:00:00 2001 From: Kasra Ghasemi Date: Thu, 24 Sep 2020 12:15:20 -0400 Subject: [PATCH 4/7] In decompose() putting the symmetric assumption on G (beacause of volume) --- pypolycontain/operations.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pypolycontain/operations.py b/pypolycontain/operations.py index 52bce61..53f490a 100644 --- a/pypolycontain/operations.py +++ b/pypolycontain/operations.py @@ -721,8 +721,8 @@ def decompose(zonotope,dimensions): #defining varibales x_i = [prog.NewContinuousVariables(i) for i in dimensions] - G_i = [prog.NewContinuousVariables(i,i) for i in dimensions] #non-symmetric G_i - #G_i = [prog.NewSymmetricContinuousVariables(i) for i in dimensions ] #symmentric G_i + #G_i = [prog.NewContinuousVariables(i,i) for i in dimensions] #non-symmetric G_i + G_i = [prog.NewSymmetricContinuousVariables(i) for i in dimensions ] #symmentric G_i #inbody_zonotope inbody_x = np.concatenate(x_i) From badc9a086dcbe7a1c8710fa65fb3f68ec295d9a8 Mon Sep 17 00:00:00 2001 From: Kasra Ghasemi Date: Thu, 24 Sep 2020 13:47:34 -0400 Subject: [PATCH 5/7] Fixing zon_subset() both for drake and just gurobi --- pypolycontain/containment.py | 175 +++++++++++++++++++++++++++++++---- 1 file changed, 156 insertions(+), 19 deletions(-) diff --git a/pypolycontain/containment.py b/pypolycontain/containment.py index 082fd6d..11dcced 100644 --- a/pypolycontain/containment.py +++ b/pypolycontain/containment.py @@ -254,20 +254,69 @@ def zonotope_subset(program,inbody,circumbody,alpha=None,solver='drake'): if solver =='drake': - from itertools import product + # from itertools import product - #Defining Variables + # #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 )) + # ) + + # if alpha==None: + # return Lambda, Gamma + # else: + # return Lambda, Gamma , alfa + + + + + # Defining Variables Gamma=program.NewContinuousVariables( circumbody.G.shape[1], inbody.G.shape[1], 'Gamma') Lambda=program.NewContinuousVariables( circumbody.G.shape[1],'Lambda') + Gamma_abs=program.NewContinuousVariables( circumbody.G.shape[1], inbody.G.shape[1], 'Gamma') + Lambda_abs=program.NewContinuousVariables( circumbody.G.shape[1],'Lambda') + + # Constraints for Gamma_abs>= +Gamma and Gamma_abs>= -Gamma , Lambda_abs>= +Lambda and Lambda_abs>= -Lambda + [program.AddLinearConstraint(Lambda_abs[i]-Lambda[i]>=0) for i in range(circumbody.G.shape[1])] + [program.AddLinearConstraint(Lambda_abs[i]+Lambda[i]>=0) for i in range(circumbody.G.shape[1])] + [program.AddLinearConstraint(Gamma_abs[i,j]-Gamma[i,j]>=0) for i in range(circumbody.G.shape[1]) for j in range(inbody.G.shape[1])] + [program.AddLinearConstraint(Gamma_abs[i,j]+Gamma[i,j]>=0) for i in range(circumbody.G.shape[1]) for j in range(inbody.G.shape[1])] #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]) + Gamma_Lambda = np.concatenate((Gamma_abs,Lambda_abs.reshape(circumbody.G.shape[1],1)),axis=1) + A=np.ones(Gamma_Lambda.shape[1]) if alpha=='scalar' or alpha== 'vector': - comb= np.concatenate( (comb,-1*np.ones((comb.shape[0],1))) , axis=1) + A=np.append(A,-1) # Managing alpha if alpha==None: @@ -283,9 +332,9 @@ def zonotope_subset(program,inbody,circumbody,alpha=None,solver='drake'): # 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]), + A= A.reshape(1,-1), + lb= [-np.inf], + ub= [1.0] if alpha==None else [0.0], vars= variable[j,:] if alpha!='scalar' else np.concatenate((Gamma_Lambda[j,:], alfa )) ) @@ -294,34 +343,122 @@ def zonotope_subset(program,inbody,circumbody,alpha=None,solver='drake'): else: return Lambda, Gamma , alfa + + + elif solver=='gurobi': + ######################################################## + ####################### LP ####################### + ######################################################## + # from itertools import product + # # Gamma = np.array( program.addVars(circumbody.G.shape[1] , inbody.G.shape[1] ,lb= -GRB.INFINITY, ub= GRB.INFINITY,vtype=GRB.CONTINUOUS) ) + # # Lambda = np.array( [program.addVar( lb = -GRB.INFINITY , ub = GRB.INFINITY , vtype=GRB.CONTINUOUS) for i in range( circumbody.G.shape[1] ) ] ) + + # Gamma = np.array( [[program.addVar(lb= -GRB.INFINITY, ub= GRB.INFINITY,vtype=GRB.CONTINUOUS) for i in range(inbody.G.shape[1])] for j in range(circumbody.G.shape[1])] ) + # Lambda = np.array( [program.addVar( lb = -GRB.INFINITY , ub = GRB.INFINITY , vtype=GRB.CONTINUOUS) for i in range( circumbody.G.shape[1] ) ] ) + + # # Gamma = program.addMVar(shape=(circumbody.G.shape[1] , inbody.G.shape[1]), lb= -GRB.INFINITY, ub= GRB.INFINITY,vtype=GRB.CONTINUOUS) + # # Lambda= program.addMVar(shape=circumbody.G.shape[1], lb = -GRB.INFINITY , ub = GRB.INFINITY,vtype=GRB.CONTINUOUS) + # program.update() + + # program.addConstrs( inbody.G[i][j] == sum([ circumbody.G[i][k]*Gamma[k,j] for k in range(circumbody.G.shape[1]) ]) for i in range(inbody.G.shape[0]) for j in range(inbody.G.shape[1]) ) + # program.addConstrs( sum( [ circumbody.G[i][j] * Lambda[j] for j in range(circumbody.G.shape[1]) ] ) == circumbody.x[i] - inbody.x[i] for i in range(inbody.G.shape[0]) ) + # program.update() + + # 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) + + + # if alpha==None: + # for j in range(Gamma_Lambda.shape[0]): + # #program.addMConstrs(comb,Gamma_Lambda[j,:].reshape(-1) , '<=', np.ones(comb.shape[0])) + # [program.addConstr(np.dot(comb[i,:],Gamma_Lambda[j,:])<=1)for i in range(comb.shape[0]) ] + # program.update() + # return Lambda, Gamma + + # elif alpha=='scalar': + # Alpha = program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY,vtype=GRB.CONTINUOUS) + # program.update() + # for j in range(Gamma_Lambda.shape[0]): + # variable=np.concatenate((Gamma_Lambda[j,:], Alpha )).reshape(-1) + # #program.addMConstrs(comb,variable , '<=', np.zeros(comb.shape[0])) + # [program.addConstr(np.dot(comb[i,:],variable)<=0) for i in range(comb.shape[0])] + + # elif alpha=='vector': + # #Alpha = program.addMVar(shape=circumbody.G.shape[1], lb = -GRB.INFINITY , ub = GRB.INFINITY,vtype=GRB.CONTINUOUS) + # Alpha = np.array( [program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY,vtype=GRB.CONTINUOUS) for i in range(circumbody.G.shape[1])] ) + # program.update() + # variable = np.concatenate((Gamma_Lambda, Alpha.reshape(-1,1)),axis=1) + # for j in range(Gamma_Lambda.shape[0]): + # #program.addMConstrs(comb,variable[j,:].reshape(-1) , '<=', np.zeros(comb.shape[0])) + # [program.addConstr(np.dot(comb[i,:],variable[j,:])<=0) for i in range(comb.shape[0])] + # program.update() + # return Lambda, Gamma , Alpha + + + ########################################################### + ####################### MILP ####################### + ########################################################### + # Gamma = program.addVars(circumbody.G.shape[1] , inbody.G.shape[1] ,lb= -GRB.INFINITY, ub= GRB.INFINITY) + # beta = [program.addVar( lb = -GRB.INFINITY , ub = GRB.INFINITY) for i in range( circumbody.G.shape[1] ) ] + # Gamma_abs = program.addVars( circumbody.G.shape[1] , inbody.G.shape[1] ,lb= 0, ub= GRB.INFINITY) + # beta_abs = [program.addVar( lb = 0 , ub = GRB.INFINITY) for i in range(circumbody.G.shape[1]) ] + # program.update() + + # program.addConstrs( inbody.G[i][j] == sum([ circumbody.G[i][k]*Gamma[k,j] for k in range(circumbody.G.shape[1]) ]) for i in range(inbody.G.shape[0]) for j in range(inbody.G.shape[1]) ) + # program.addConstrs( sum( [ circumbody.G[i][j] * beta[j] for j in range(circumbody.G.shape[1]) ] ) == circumbody.x[i] - inbody.x[i] for i in range(inbody.G.shape[0]) ) + # [program.addGenConstrAbs(Gamma_abs[i,j], Gamma[i,j] ) for i in range(circumbody.G.shape[1]) for j in range(inbody.G.shape[1]) ] + # [program.addGenConstrAbs(beta_abs[i] , beta[i] ) for i in range(circumbody.G.shape[1])] + + # if alpha == None: + # program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= 1 for i in range(circumbody.G.shape[1]) ) + # program.update() + # return beta , Gamma + + # elif alpha == 'scalar': + # Alpha = program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY) + # program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= Alpha for i in range(circumbody.G.shape[1]) ) + # elif alpha == 'vector': + # Alpha = [program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY) for i in range(circumbody.G.shape[1])] + # program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= Alpha[i] for i in range(circumbody.G.shape[1]) ) + + # program.update() + + # return beta , Gamma , Alpha + + Gamma = program.addVars(circumbody.G.shape[1] , inbody.G.shape[1] ,lb= -GRB.INFINITY, ub= GRB.INFINITY) - beta = [program.addVar( lb = -GRB.INFINITY , ub = GRB.INFINITY) for i in range( circumbody.G.shape[1] ) ] + Lambda = [program.addVar( lb = -GRB.INFINITY , ub = GRB.INFINITY) for i in range( circumbody.G.shape[1] ) ] Gamma_abs = program.addVars( circumbody.G.shape[1] , inbody.G.shape[1] ,lb= 0, ub= GRB.INFINITY) - beta_abs = [program.addVar( lb = 0 , ub = GRB.INFINITY) for i in range(circumbody.G.shape[1]) ] + Lambda_abs = [program.addVar( lb = 0 , ub = GRB.INFINITY) for i in range(circumbody.G.shape[1]) ] program.update() program.addConstrs( inbody.G[i][j] == sum([ circumbody.G[i][k]*Gamma[k,j] for k in range(circumbody.G.shape[1]) ]) for i in range(inbody.G.shape[0]) for j in range(inbody.G.shape[1]) ) - program.addConstrs( sum( [ circumbody.G[i][j] * beta[j] for j in range(circumbody.G.shape[1]) ] ) == circumbody.x[i] - inbody.x[i] for i in range(inbody.G.shape[0]) ) - [program.addGenConstrAbs(Gamma_abs[i,j], Gamma[i,j] ) for i in range(circumbody.G.shape[1]) for j in range(inbody.G.shape[1]) ] - [program.addGenConstrAbs(beta_abs[i] , beta[i] ) for i in range(circumbody.G.shape[1])] + program.addConstrs( sum( [ circumbody.G[i][j] * Lambda[j] for j in range(circumbody.G.shape[1]) ] ) == circumbody.x[i] - inbody.x[i] for i in range(inbody.G.shape[0]) ) + [program.addConstr(Gamma_abs[i,j] >= Gamma[i,j] ) for i in range(circumbody.G.shape[1]) for j in range(inbody.G.shape[1]) ] + [program.addConstr(Gamma_abs[i,j] >= -Gamma[i,j] ) for i in range(circumbody.G.shape[1]) for j in range(inbody.G.shape[1]) ] + [program.addConstr(Lambda_abs[i] >= Lambda[i] ) for i in range(circumbody.G.shape[1])] + [program.addConstr(Lambda_abs[i] >= -Lambda[i] ) for i in range(circumbody.G.shape[1])] + program.update() + if alpha == None: - program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= 1 for i in range(circumbody.G.shape[1]) ) + program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + Lambda_abs[i] <= 1 for i in range(circumbody.G.shape[1]) ) program.update() - return beta , Gamma + return Lambda , Gamma elif alpha == 'scalar': Alpha = program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY) - program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= Alpha for i in range(circumbody.G.shape[1]) ) + program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + Lambda_abs[i] <= Alpha for i in range(circumbody.G.shape[1]) ) elif alpha == 'vector': Alpha = [program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY) for i in range(circumbody.G.shape[1])] - program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= Alpha[i] for i in range(circumbody.G.shape[1]) ) + program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + Lambda_abs[i] <= Alpha[i] for i in range(circumbody.G.shape[1]) ) program.update() - return beta , Gamma , Alpha - + return Lambda , Gamma , Alpha From f746b0a733908fa725a2bd226246b9d6a5193878 Mon Sep 17 00:00:00 2001 From: Kasra Ghasemi Date: Thu, 24 Sep 2020 13:51:15 -0400 Subject: [PATCH 6/7] cleaning the zon_subset() --- pypolycontain/containment.py | 130 ----------------------------------- 1 file changed, 130 deletions(-) diff --git a/pypolycontain/containment.py b/pypolycontain/containment.py index 11dcced..3708448 100644 --- a/pypolycontain/containment.py +++ b/pypolycontain/containment.py @@ -253,49 +253,6 @@ def zonotope_subset(program,inbody,circumbody,alpha=None,solver='drake'): assert(inbody.x.shape==circumbody.x.shape),"Both input zonotopes need to be in the same dimension" if solver =='drake': - - # 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 )) - # ) - - # if alpha==None: - # return Lambda, Gamma - # else: - # return Lambda, Gamma , alfa - - - # Defining Variables Gamma=program.NewContinuousVariables( circumbody.G.shape[1], inbody.G.shape[1], 'Gamma') @@ -343,95 +300,8 @@ def zonotope_subset(program,inbody,circumbody,alpha=None,solver='drake'): else: return Lambda, Gamma , alfa - - - - - elif solver=='gurobi': - ######################################################## - ####################### LP ####################### - ######################################################## - # from itertools import product - # # Gamma = np.array( program.addVars(circumbody.G.shape[1] , inbody.G.shape[1] ,lb= -GRB.INFINITY, ub= GRB.INFINITY,vtype=GRB.CONTINUOUS) ) - # # Lambda = np.array( [program.addVar( lb = -GRB.INFINITY , ub = GRB.INFINITY , vtype=GRB.CONTINUOUS) for i in range( circumbody.G.shape[1] ) ] ) - - # Gamma = np.array( [[program.addVar(lb= -GRB.INFINITY, ub= GRB.INFINITY,vtype=GRB.CONTINUOUS) for i in range(inbody.G.shape[1])] for j in range(circumbody.G.shape[1])] ) - # Lambda = np.array( [program.addVar( lb = -GRB.INFINITY , ub = GRB.INFINITY , vtype=GRB.CONTINUOUS) for i in range( circumbody.G.shape[1] ) ] ) - - # # Gamma = program.addMVar(shape=(circumbody.G.shape[1] , inbody.G.shape[1]), lb= -GRB.INFINITY, ub= GRB.INFINITY,vtype=GRB.CONTINUOUS) - # # Lambda= program.addMVar(shape=circumbody.G.shape[1], lb = -GRB.INFINITY , ub = GRB.INFINITY,vtype=GRB.CONTINUOUS) - # program.update() - - # program.addConstrs( inbody.G[i][j] == sum([ circumbody.G[i][k]*Gamma[k,j] for k in range(circumbody.G.shape[1]) ]) for i in range(inbody.G.shape[0]) for j in range(inbody.G.shape[1]) ) - # program.addConstrs( sum( [ circumbody.G[i][j] * Lambda[j] for j in range(circumbody.G.shape[1]) ] ) == circumbody.x[i] - inbody.x[i] for i in range(inbody.G.shape[0]) ) - # program.update() - - # 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) - - - # if alpha==None: - # for j in range(Gamma_Lambda.shape[0]): - # #program.addMConstrs(comb,Gamma_Lambda[j,:].reshape(-1) , '<=', np.ones(comb.shape[0])) - # [program.addConstr(np.dot(comb[i,:],Gamma_Lambda[j,:])<=1)for i in range(comb.shape[0]) ] - # program.update() - # return Lambda, Gamma - - # elif alpha=='scalar': - # Alpha = program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY,vtype=GRB.CONTINUOUS) - # program.update() - # for j in range(Gamma_Lambda.shape[0]): - # variable=np.concatenate((Gamma_Lambda[j,:], Alpha )).reshape(-1) - # #program.addMConstrs(comb,variable , '<=', np.zeros(comb.shape[0])) - # [program.addConstr(np.dot(comb[i,:],variable)<=0) for i in range(comb.shape[0])] - - # elif alpha=='vector': - # #Alpha = program.addMVar(shape=circumbody.G.shape[1], lb = -GRB.INFINITY , ub = GRB.INFINITY,vtype=GRB.CONTINUOUS) - # Alpha = np.array( [program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY,vtype=GRB.CONTINUOUS) for i in range(circumbody.G.shape[1])] ) - # program.update() - # variable = np.concatenate((Gamma_Lambda, Alpha.reshape(-1,1)),axis=1) - # for j in range(Gamma_Lambda.shape[0]): - # #program.addMConstrs(comb,variable[j,:].reshape(-1) , '<=', np.zeros(comb.shape[0])) - # [program.addConstr(np.dot(comb[i,:],variable[j,:])<=0) for i in range(comb.shape[0])] - # program.update() - # return Lambda, Gamma , Alpha - - - ########################################################### - ####################### MILP ####################### - ########################################################### - # Gamma = program.addVars(circumbody.G.shape[1] , inbody.G.shape[1] ,lb= -GRB.INFINITY, ub= GRB.INFINITY) - # beta = [program.addVar( lb = -GRB.INFINITY , ub = GRB.INFINITY) for i in range( circumbody.G.shape[1] ) ] - # Gamma_abs = program.addVars( circumbody.G.shape[1] , inbody.G.shape[1] ,lb= 0, ub= GRB.INFINITY) - # beta_abs = [program.addVar( lb = 0 , ub = GRB.INFINITY) for i in range(circumbody.G.shape[1]) ] - # program.update() - - # program.addConstrs( inbody.G[i][j] == sum([ circumbody.G[i][k]*Gamma[k,j] for k in range(circumbody.G.shape[1]) ]) for i in range(inbody.G.shape[0]) for j in range(inbody.G.shape[1]) ) - # program.addConstrs( sum( [ circumbody.G[i][j] * beta[j] for j in range(circumbody.G.shape[1]) ] ) == circumbody.x[i] - inbody.x[i] for i in range(inbody.G.shape[0]) ) - # [program.addGenConstrAbs(Gamma_abs[i,j], Gamma[i,j] ) for i in range(circumbody.G.shape[1]) for j in range(inbody.G.shape[1]) ] - # [program.addGenConstrAbs(beta_abs[i] , beta[i] ) for i in range(circumbody.G.shape[1])] - - # if alpha == None: - # program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= 1 for i in range(circumbody.G.shape[1]) ) - # program.update() - # return beta , Gamma - - # elif alpha == 'scalar': - # Alpha = program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY) - # program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= Alpha for i in range(circumbody.G.shape[1]) ) - # elif alpha == 'vector': - # Alpha = [program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY) for i in range(circumbody.G.shape[1])] - # program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + beta_abs[i] <= Alpha[i] for i in range(circumbody.G.shape[1]) ) - - # program.update() - - # return beta , Gamma , Alpha - - Gamma = program.addVars(circumbody.G.shape[1] , inbody.G.shape[1] ,lb= -GRB.INFINITY, ub= GRB.INFINITY) Lambda = [program.addVar( lb = -GRB.INFINITY , ub = GRB.INFINITY) for i in range( circumbody.G.shape[1] ) ] Gamma_abs = program.addVars( circumbody.G.shape[1] , inbody.G.shape[1] ,lb= 0, ub= GRB.INFINITY) From 501009b6b8379260e00e29d78194d40d3a6af925 Mon Sep 17 00:00:00 2001 From: Kasra Ghasemi Date: Fri, 14 May 2021 16:47:27 -0400 Subject: [PATCH 7/7] Fixing zonotope_containement for alpha = vector --- pypolycontain/containment.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/pypolycontain/containment.py b/pypolycontain/containment.py index 3708448..d7fb894 100644 --- a/pypolycontain/containment.py +++ b/pypolycontain/containment.py @@ -325,10 +325,11 @@ def zonotope_subset(program,inbody,circumbody,alpha=None,solver='drake'): elif alpha == 'scalar': Alpha = program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY) program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + Lambda_abs[i] <= Alpha for i in range(circumbody.G.shape[1]) ) - elif alpha == 'vector': - Alpha = [program.addVar(lb=-GRB.INFINITY,ub=GRB.INFINITY) for i in range(circumbody.G.shape[1])] - program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + Lambda_abs[i] <= Alpha[i] for i in range(circumbody.G.shape[1]) ) + return Lambda , Gamma , Alpha + # alpha == 'vector': + Alpha = [program.addVar(lb= 0 ,ub=GRB.INFINITY) for i in range(circumbody.G.shape[1])] + program.addConstrs( sum( [ Gamma_abs[i,j] for j in range(inbody.G.shape[1]) ]) + Lambda_abs[i] <= Alpha[i] for i in range(circumbody.G.shape[1]) ) program.update() - return Lambda , Gamma , Alpha +