diff --git a/pypolycontain/containment.py b/pypolycontain/containment.py index 7e739fe..d7fb894 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 gurobipy 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,98 @@ 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': + + # 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_abs,Lambda_abs.reshape(circumbody.G.shape[1],1)),axis=1) + A=np.ones(Gamma_Lambda.shape[1]) + if alpha=='scalar' or alpha== 'vector': + A=np.append(A,-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= 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 )) + ) + + if alpha==None: + return Lambda, Gamma + else: + return Lambda, Gamma , alfa + + elif solver=='gurobi': + + 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) + 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] * 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]) ]) + Lambda_abs[i] <= 1 for i in range(circumbody.G.shape[1]) ) + program.update() + 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]) ]) + Lambda_abs[i] <= Alpha 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 + 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)