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 decompose(zonotope, dimensions):
"""
@author: kasra
Decompising a given set into bunch of fewer dimensional sets such that
the Cartesian product of those sets is a subset of the given set.
"""
assert (sum(dimensions) == len(
zonotope.G)), "ValueError: sum of the given dimensions has \
to be equal to the dimension of the input set"
#number_of_sets = len(dimensions)
prog = MP.MathematicalProgram()
#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
#inbody_zonotope
inbody_x = np.concatenate(x_i)
inbody_G = block_diag(*G_i)
inbody_zonotope = pp.zonotope(inbody_G, inbody_x)
#Defining Constraints
prog.AddPositiveSemidefiniteConstraint(inbody_G)
pp.subset(prog, inbody_zonotope, zonotope)
# ASSUMPTION for PSD of inbody_G
# ASSUMPTION for SYMMENTRICITY of G_i
#Defining the cost function
prog.AddMaximizeLogDeterminantSymmetricMatrixCost(inbody_G)
#Solving
result = MP.Solve(prog)
print(f"Is optimization successful? {result.is_success()}")
print(f"Solution to x_i: {result.GetSolution(x_i[0])}")
print(f"Solution to G_i: {result.GetSolution(G_i[0])}")
print(f"optimal cost: {result.get_optimal_cost()}")
print('solver is: ', result.get_solver_id().name())
x_i_result = [result.GetSolution(x_i[i]) for i in range(len(dimensions))]
G_i_result = [result.GetSolution(G_i[i]) for i in range(len(dimensions))]
list_zon = [
pp.zonotope(G=G_i_result[i], x=x_i_result[i])
for i in range(len(dimensions))
]
return list_zon
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 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 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 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