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