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 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 extend(mysystem,
start,
T,
list_of_nodes,
H_rep=True,
N_H=500,
tol_H=1e-5,
color=None):
if type(color) == type(None):
color = (np.random.random(), np.random.random(), np.random.random())
else:
color = color
x, u, mu, G, theta = polytopic_trajectory(mysystem, start, T,
list_of_nodes)
Z = {t: pp.zonotope(x=x[t], G=G[t], color=color) for t in range(T + 1)}
funnel = [None] * T
H_funnel = [None] * T
for t in range(T):
funnel[t] = pp.convex_hull(Z[t], Z[t + 1])
funnel[t].color = color
if H_rep:
H_funnel[t] = pp.ray_shooting_hyperplanes(funnel[t],
N=N_H,
tol=tol_H)
# fig,ax=plt.subplots()
# mu[T,'free'],mu[T,'contact']=1,1
# ax.plot( [x[t][0] for t in range(T+1)] , [x[t][2] for t in range(T+1)] )
# ax.plot( [x[t][0] for t in range(T+1) if mu[t,'free']==0] , \
# [x[t][2] for t in range(T) if mu[t,'free']==0],'o',\
# color='red')
# ax.plot( [x[t][0] for t in range(T+1) if mu[t,'free']==1] , \
# [x[t][2] for t in range(T+1) if mu[t,'free']==1],'o',\
# color='blue')
# pp.visualize([Omega]+[funnel[t] for t in range(T)],ax=ax,fig=fig,a=0.01,alpha=0.99,tuple_of_projection_dimensions=(0,2))
return funnel, H_funnel, x, mu, G
def pca_order_reduction(zonotope, desired_order=1):
"""
PCA method for zonotope order reduction
inputs: input zonotope , order of the output zonotope
output: zonotope
Based on Kopetzki, Anna-Kathrin, Bastian Schürmann, and Matthias Althoff.
"Methods for order reduction of zonotopes."
2017 IEEE 56th Annual Conference on Decision and Control (CDC). IEEE, 2017.
"""
assert (
type(zonotope) == pp.zonotope
), "TypeError: The first argument need to be from \"zonotope\" class in pypolycontain package "
assert (
type(desired_order) == int or type(desired_order) == float
), "TypeError: The second argument need to be a number greater than 1. \n \
It is the order of the reduced zonotpe which equal to the number of \
columns over the space dimension."
assert (
desired_order >= 1
), "desired order of the outcome zonotope needs to be greater or equal to 1"
x = np.array(zonotope.x)
G = np.array(zonotope.G)
dimension, numberofcolumns = G.shape
desired_numberofcolumns = round(desired_order * dimension)
if numberofcolumns <= desired_numberofcolumns:
return zonotope
G_reduced, G_untouched = sorting_generator(G, desired_numberofcolumns)
X = np.concatenate((G_reduced, -G_reduced), axis=1).T
covariance = np.dot(X.T, X)
U, _, _ = np.linalg.svd(covariance)
interval_hull = boxing_order_reduction(
pp.zonotope(np.dot(U.T, G_reduced), x)).G
G_pca = np.dot(U, interval_hull)
if type(G_untouched) == np.ndarray:
return pp.zonotope(np.concatenate((G_pca, G_untouched), axis=1), x)
elif G_untouched == None:
return pp.zonotope(G_pca, x)
def boxing_order_reduction(zonotope, desired_order=1):
"""
boxing method for zonotope order reduction
inputs: input zonotope , order of the output zonotope
output: zonotope
Based on Kopetzki, Anna-Kathrin, Bastian Schurmann, and Matthias Althoff.
"Methods for order reduction of zonotopes."
2017 IEEE 56th Annual Conference on Decision and Control (CDC). IEEE, 2017.
"""
assert (
type(zonotope) == pp.zonotope
), "TypeError: The first argument need to be from \"zonotope\" class in pypolycontain package "
assert (
type(desired_order) == int or type(desired_order) == float
), "TypeError: The second argument need to be a number greater than 1. \n \
It is the order of the reduced zonotpe which equal to the number of \
columns over the space dimension."
assert (
desired_order >= 1
), "desired order of the outcome zonotope needs to be greater or equal to 1"
x = np.array(zonotope.x)
G = np.array(zonotope.G)
dimension, numberofcolumns = G.shape
desired_numberofcolumns = round(desired_order * dimension)
if numberofcolumns <= desired_numberofcolumns:
return zonotope
elif dimension == desired_numberofcolumns:
G_box = np.diag(np.sum(abs(G), axis=1))
return pp.zonotope(G=G_box, x=x)
else:
G_reduced, G_untouched = sorting_generator(G, desired_numberofcolumns)
G_box = np.concatenate(
(np.diag(np.sum(abs(G_reduced), axis=1)), G_untouched), axis=1)
return pp.zonotope(G=G_box, x=x)
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 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")
import numpy as np
import pypolycontain as pp
import matplotlib.pyplot as plt
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
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Tue Jun 23 23:35:22 2020
@author: sadra
"""
import numpy as np
from itertools import combinations
import pypolycontain as pp
G = np.random.random((2, 8)) - 0.5
G = np.array([[1, 0, 0], [0, 1, 0]])
Z = pp.zonotope(G, color='red')
S = combinations(range(G.shape[1]), G.shape[0])
V = 0
for s in S:
# print(s)
Gs = np.hstack([G[:, i:i + 1] for i in s])
print(Gs)
V += abs(np.linalg.det(Gs))
V *= 2**G.shape[0]
print(V)
pp.visualize([Z, pp.zonotope(np.eye(2) * 0.5)], title=r'volume=%0.002f' % V)
S = combinations(range(G.shape[1]), G.shape[0])
V_dot = np.zeros(G.shape)
for s in S:
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
