Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Kasra #7

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
Open

Kasra #7

Changes from 1 commit
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
ff90a46
adding zonotope_subset() and gurobi
Kasraghasemi Sep 9, 2020
41df302
removing the bounds over alpha in zonotope_subset()
Kasraghasemi Sep 9, 2020
da69668
fixing zonotope_subset() with gurobi
Kasraghasemi Sep 10, 2020
b6c345b
In decompose() putting the symmetric assumption on G (beacause of vol…
Kasraghasemi Sep 24, 2020
badc9a0
Fixing zon_subset() both for drake and just gurobi
Kasraghasemi Sep 24, 2020
f746b0a
cleaning the zon_subset()
Kasraghasemi Sep 24, 2020
501009b
Fixing zonotope_containement for alpha = vector
Kasraghasemi May 14, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Fixing zon_subset() both for drake and just gurobi
  • Loading branch information
@Kasraghasemi
Kasraghasemi committed Sep 24, 2020
commit badc9a086dcbe7a1c8710fa65fb3f68ec295d9a8
175 changes: 156 additions & 19 deletions pypolycontain/containment.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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 ))
)

Expand All @@ -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