def add_branch(self, x, u, G, theta, goal):
"""
add x,u,G,theta
"""
T = len(u)
Z = {}
import random as rand
c = (rand.random(), rand.random(), rand.random()
) #over-ride previous one
for t in range(T):
# c=(t/(T+0.01), 1-t/(T+0.01), 0.5)
if self.fill == "discrete":
p = zonotope(x[t], G[t], color=c)
Z[t] = state(p, len(self.branches), t)
elif self.fill == "continous":
G_c = np.hstack((G[t], (x[t + 1] - x[t]) / 1.0))
p_c = zonotope((x[t] + x[t + 1]) / 2.0, G_c, color=c)
Z[t] = state(p_c, len(self.branches), t)
else:
raise self.fill, ": not understood, please enter 'discrete' or 'continous' for tree.fill"
Z[T + 1] = state(goal)
for t in range(T - 1):
Z[t].child = Z[t + 1]
Z[t].control = (u[t], theta[t])
Z[t + 1].parents.append(Z[t])
self.states.extend([Z[t] for t in range(T)])
self.branches.append(Z)
def test_zonotope_to_AABB(self):
G_l = np.array([[1, 0, 0, 3], [0, 1, 2, -1]]) * 0.8
G_r = np.array([[1, 0, 1, 1, 2, -2], [0, 1, 1, -1, 5, 2]]) * 1
x_l = np.array([0, 1]).reshape(2, 1)
x_r = np.array([1, 0]).reshape(2, 1)
zono_l = zonotope(x_l, G_l)
zono_r = zonotope(x_r, G_r)
AABB_r = zonotope_to_box(zono_r)
AABB_l = zonotope_to_box(zono_l)
fix, ax = visZ([zono_r,zono_l], title="")
visualize_boxes([AABB_r,AABB_l], ax= ax)
plt.show()
def RCI(sys, q=0, alpha=0, K=5):
"""
Computes a Robust Control Invariant (RCI) set for LTI system sys
"""
q = K * sys.n if q == 0 else q
model = Model("RCI")
phi = tupledict_to_array(
model.addVars(range(sys.n),
range(q),
lb=-GRB.INFINITY,
ub=GRB.INFINITY,
name="phi"))
theta = tupledict_to_array(
model.addVars(range(sys.m),
range(q),
lb=-GRB.INFINITY,
ub=GRB.INFINITY,
name="theta"))
psi = tupledict_to_array(
model.addVars(range(sys.n),
range(sys.w),
lb=-GRB.INFINITY,
ub=GRB.INFINITY,
name="psi"))
model.update()
_fat_matrix = np.hstack((psi, phi))
constraints_list_of_tuples(model, [(sys.A, phi), (sys.B, theta),
(-np.eye(sys.n), _fat_matrix[:, 0:q])],
"=")
constraints_list_of_tuples(model,
[(np.eye(sys.n), sys.W),
(-np.eye(sys.n), _fat_matrix[:, q:q + sys.w])],
"=")
_outer = zonotope(np.zeros((sys.n, 1)), sys.W * alpha)
_inner = zonotope(np.zeros((sys.n, 1)), psi)
subset_generic(model, _inner, _outer)
if sys.X != None:
sys.X.G *= (1 - alpha)
subset_generic(model, zonotope(np.zeros((sys.n, 1)), phi), sys.X)
if sys.U != None:
sys.U.G *= (1 - alpha)
subset_generic(model, zonotope(np.zeros((sys.m, 1)), theta), sys.U)
model.optimize()
phi_n = np.array([[phi[i, j].X for i in range(phi.shape[0])]
for j in range(phi.shape[1])]).T
theta_n = np.array([[theta[i, j].X for i in range(theta.shape[0])]
for j in range(theta.shape[1])]).T
# psi_n=np.array([[psi[i,j].X for i in range(psi.shape[0])] for j in range(psi.shape[1])]).T
sys.phi = phi_n * 1.0 / (1 - alpha)
sys.theta = theta_n * 1.0 / (1 - alpha)
return phi_n, theta_n
def polytopic_trajectory_given_modes(x0,list_of_cells,goal,eps=0,order=1):
"""
Description:
Polytopic Trajectory Optimization with the ordered list of polytopes given
This is a convex program as mode sequence is already given
list_of_cells: each cell has the following attributes: A,B,c, and polytope(H,h)
"""
model=Model("Fixed Mode Polytopic Trajectory")
T=len(list_of_cells)
n,m=list_of_cells[0].B.shape
q=int(order*n)
x=model.addVars(range(T+1),range(n),lb=-GRB.INFINITY,ub=GRB.INFINITY,name="x")
u=model.addVars(range(T),range(m),lb=-GRB.INFINITY,ub=GRB.INFINITY,name="u")
G=model.addVars(range(T+1),range(n),range(q),lb=-GRB.INFINITY,ub=GRB.INFINITY,name="G")
theta=model.addVars(range(T),range(m),range(q),lb=-GRB.INFINITY,ub=GRB.INFINITY,name="theta")
model.update()
for j in range(n):
model.addConstr(x[0,j]<=x0[j,0]+eps)
model.addConstr(x[0,j]>=x0[j,0]-eps)
for t in range(T):
print "adding constraints of t",t
cell=list_of_cells[t]
A,B,c,p=cell.A,cell.B,cell.c,cell.p
for j in range(n):
expr_x=LinExpr([(A[j,k],x[t,k]) for k in range(n)])
expr_u=LinExpr([(B[j,k],u[t,k]) for k in range(m)])
model.addConstr(x[t+1,j]==expr_x+expr_u+c[j,0])
for i in range(n):
for j in range(q):
expr_x=LinExpr([(A[i,k],G[t,k,j]) for k in range(n)])
expr_u=LinExpr([(B[i,k],theta[t,k,j]) for k in range(m)])
model.addConstr(G[t+1,i,j]==expr_x+expr_u)
x_t=np.array([x[t,j] for j in range(n)]).reshape(n,1)
u_t=np.array([u[t,j] for j in range(m)]).reshape(m,1)
G_t=np.array([G[t,i,j] for i in range(n) for j in range(q)]).reshape(n,q)
theta_t=np.array([theta[t,i,j] for i in range(m) for j in range(q)]).reshape(m,q)
GT=sp.linalg.block_diag(G_t,theta_t)
xu=np.vstack((x_t,u_t))
subset_LP(model,xu,GT,Ball(2*q),p)
x_T=np.array([x[T,j] for j in range(n)]).reshape(n,1)
G_T=np.array([G[T,i,j] for i in range(n) for j in range(q)]).reshape(n,q)
z=zonotope(x_T,G_T)
subset_zonotopes(model,z,goal)
J=LinExpr([(1/(t+1.0),G[t,i,i]) for t in range(T+1) for i in range(n)])
model.setObjective(J)
model.write("sadra.lp")
model.setParam('TimeLimit', 150)
model.optimize()
x_num,G_num={},{}
for t in range(T+1):
x_num[t]=np.array([[x[t,j].X] for j in range(n)]).reshape(n,1)
G_num[t]=np.array([[G[t,i,j].X] for i in range(n) for j in range(q)]).reshape(n,q)
return (x_num,G_num)
def test_zonotope_to_voronoi():
zonotope_count = 10
zonotopes = []
centroid_range = zonotope_count * 4
generator_range = 3
for i in range(zonotope_count):
m = np.random.random_integers(4, 10)
G = (np.random.rand(2, m) - 0.5) * generator_range
x = (np.random.rand(2, 1) - 0.5) * centroid_range
zonotopes.append(zonotope(x, G))
vcp = VoronoiClosestPolytope(zonotopes)
def point_trajectory_given_modes_and_central_traj(x_traj,list_of_cells,goal,eps=0,scale=[]):
"""
Description:
Point Trajectory Optimization with the ordered list of polytopes given
This is a convex program as mode sequence is already given
list_of_cells: each cell has the following attributes: A,B,c, and polytope(H,h)
"""
model=Model("Fixed Mode Polytopic Trajectory")
T=len(list_of_cells)
n,m=list_of_cells[0].B.shape
x=model.addVars(range(T+1),range(n),lb=-GRB.INFINITY,ub=GRB.INFINITY,name="x")
u=model.addVars(range(T),range(m),lb=-GRB.INFINITY,ub=GRB.INFINITY,name="u")
model.update()
if len(scale)==0:
scale=np.ones(x_traj[0].shape[0])
print "inside function epsilon is",eps
for j in range(n):
model.addConstr(x[0,j]<=x_traj[0][j]+eps*scale[j])
model.addConstr(x[0,j]>=x_traj[0][j]-eps*scale[j])
for t in range(T):
cell=list_of_cells[t]
A,B,c,_p=cell.A,cell.B,cell.c,cell.p
for j in range(n):
expr_x=LinExpr([(A[j,k],x[t,k]) for k in range(n)])
expr_u=LinExpr([(B[j,k],u[t,k]) for k in range(m)])
model.addConstr(x[t+1,j]==expr_x+expr_u+c[j,0])
x_t=np.array([x[t,j] for j in range(n)]).reshape(n,1)
u_t=np.array([u[t,j] for j in range(m)]).reshape(m,1)
xu=np.vstack((x_t,u_t))
subset_LP(model,xu,np.zeros((n+m,1)),Ball(1),_p)
x_T=np.array([x[T,j] for j in range(n)]).reshape(n,1)
z=zonotope(x_T,np.zeros((n,1)))
subset_generic(model,z,goal)
# Cost function
J=QuadExpr()
for t in range(T):
for i in range(n):
J.add(x[t,i]*x[t,i]-x[t,i]*x_traj[t][i])
model.setObjective(J)
model.write("polytopic_trajectory.lp")
model.setParam('TimeLimit', 150)
model.optimize()
x_num,u_num={},{}
for t in range(T+1):
x_num[t]=np.array([[x[t,j].X] for j in range(n)])
for t in range(T):
u_num[t]=np.array([[u[t,i].X] for i in range(m) ])
return (x_num,u_num)
def __init__(self, p, branch=0, t=0):
self.name = "state branch=%d t=%d" % (branch, t)
self.p = p
self.x = p.x
self.G = p.G + np.random.random(p.G.shape) * 0.0001
self.p = zonotope(self.x,
self.G,
color=p.color,
name="zonotope" + self.name)
self.t = t
v = vcube(p.G.shape[1])
self.vertices = p.x.T + (np.dot(p.G, v.T)).T
self.cost_to_go = 0
self.time_to_go = 0
self.cost_to_child = 0
self.parents = []
self.child = None
self.control = None
500
]).reshape(20, 1)
sys.add_environment(Eta_1, epsilon_max, epsilon_min)
#sys.add_environment(Eta_1)
#raise 1
up_shift = 0
right_shift = 0
theta_shift = 0.25
x_goal = np.array(
[0, R - p, theta_shift, x_m_0, y_m_0, theta_m_0, s_m_0, 0, 0,
0]).reshape(10, 1)
x0 = np.array([0, R - p, 0, x_m_0, y_m_0, theta_m_0, s_m_0, 0, 0,
0]).reshape(10, 1).reshape(10, 1)
sys.goal = zonotope(x_goal.reshape(10, 1),
100 * np.diag([1, 1, 0, 1, 1, 1, 1, 0, 0, 0]))
T = 10
sys.scale = np.array([0, 0, 0, 1, 1, 1, 1, 0, 0, 0])
x_traj, u_traj, u_lambda, mode = point_trajectory_tishcom(
sys,
x0, [sys.goal],
T,
optimize_controls_indices=[0, 1, 2, 3],
cost=1,
eps=0.0)
list_of_linear_cells = trajectory_to_list_of_linear_cells(
sys, Eta_1, x_traj, u_traj, u_lambda, mode)
list_of_linear_cells_full = trajectory_to_list_of_linear_cells_full_linearization(
mysystem, x_traj, u_traj, u_lambda, mode, h, epsilon_min, epsilon_max)
list_of_linear_cells_PWA = PWA_cells_from_state(mysystem, x_traj[2], h,
H = np.array([[1, 0, 0], [0, 1, 0], [-1, 0, 0], [0, -1, 0], [0, 0, 1],
[0, 0, -1]])
h = np.array([[0.12, 1, 0.1, 1, 4, 4]]).T
sys.C[2, 0] = polytope(H, h)
H = np.array([[1, 0, 0], [0, 1, 0], [-1, 0, 0], [0, -1, 0], [0, 0, 1],
[0, 0, -1]])
h = np.array([[0.12, 1, -0.1, 1, 4, 4]]).T
sys.C[1, 1] = polytope(H, h)
H = np.array([[1, 0, 0], [0, 1, 0], [-1, 0, 0], [0, -1, 0], [0, 0, 1],
[0, 0, -1]])
h = np.array([[-0.1, 1, 0.12, 1, 4, 4]]).T
sys.C[2, 1] = polytope(H, h)
sys.goal = zonotope(
np.array([0, 0.0]).reshape(2, 1), np.array([[0, 0], [0, 0]]))
sys.n = 2
sys.m = 1
sys.list_of_sum_indices = [0, 1]
sys.list_of_modes = {}
sys.list_of_modes[0] = [0]
sys.list_of_modes[1] = [0, 1]
sys.list_of_modes[2] = [0, 1]
sys.build()
sys.scale = np.array([0.12, 1])
sys.build_cells()
from PWA_lib.polytree.tree import tree
A[1] = np.array([[1, dt], [1, 0.6]])
B[1] = np.array([[0, dt]]).T
W[1] = np.array([[1, 0], [0, 1]]) * dt * 0.2
A[2] = np.array([[1, dt], [-1, 1]])
B[2] = np.array([[0, dt]]).T
W[2] = np.array([[1, 0], [0, 1]]) * dt * 0.4
A[3] = np.array([[1, dt], [-0.5, 1.2]])
B[3] = np.array([[0, dt]]).T
W[3] = np.array([[1, 0], [0, 1]]) * dt * 0.2
s = system_T_periodic(A, B, W)
s.X = zonotope(np.zeros((2, 1)), np.eye(2) * 2)
s.U = zonotope(np.zeros((1, 1)), np.eye(1) * 3)
G, theta = RCI_periodic(s, q=12)
T = s.T + 1
for t in range(T + 1):
if t == T:
visZ([
zonotope(np.zeros((2, 1)),
G[t],
color=(t / (s.T + 1.5), 0, 1 - t / (s.T + 1.5)))
],
a=0.02,
title=r"$\mathcal{Z}(0,G_{0})=\mathcal{Z}(0,G_{%d})$" % T)
else:
}
epsilon_max = np.array(
[2, 2, 2, 1, 1, 1, 1, 5, 5, 5, 1, 1, 1, 1, 50, 1000, 50,
1000]).reshape(18, 1)
epsilon_min = -np.array([
2, 2, 2, 1, 1, 1, 1, 5, 5, 5, 1, 1, 1, 1, 50, 1000, 50, 1000
]).reshape(18, 1)
sys = system_numeric(mysystem)
sys.add_environment(Eta, epsilon_max, epsilon_min)
y_goal = 0.0
x_goal = np.array([0, y_goal, 0.0, 1, y_goal, -1, y_goal, 0, 0,
0]).reshape(10, 1)
#x_goal=np.array([0,0,0.0,1,0,-1,0,0,0,0]).reshape(10,1)
sys.goal = zonotope(x_goal.reshape(10, 1),
np.diag([0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
x0 = np.array([0.0, 0, 0.0, 1, 0, -1, 0, 0.8, 0, 0]).reshape(10, 1)
T = 20
sys.scale = np.array([0, 0, 0, 1, 1, 1, 1, 0, 0, 0])
x, u_traj, u_lambda, mode = point_trajectory_tishcom(
sys, x0, [sys.goal], T, optimize_controls_indices=[0, 1, 2, 3], cost=1)
list_of_linear_cells = trajectory_to_list_of_linear_cells(
sys, Eta, x, u_traj, u_lambda, mode)
#list_of_linear_cells_full=trajectory_to_list_of_linear_cells_full_linearization(mysystem,x_traj,u_traj,u_lambda,mode,h,epsilon_min,epsilon_max)
#list_of_linear_cells_PWA=PWA_cells_from_state(mysystem,x_traj[2],h,epsilon_min,epsilon_max)
#list_of_reachable_sets=hybrid_reachable_sets_from_state(mysystem,x_traj[1],h,epsilon_min,epsilon_max)
# The time varying PWA system:
#list_of_env=[environment_from_state(mysystem,x_traj[t],h,name=t) for t in range(T)]
#sys=system_numeric(mysystem)
#for env in list_of_env:
from pypolycontain.visualization.visualize_2D import visualize_2D_zonotopes_ax as visZ
from PWA_lib.polytree.bsp.bsp import create_hyperplane, BSP_tree
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
n = 2
q = 3
B = Box(q)
N = 100
X = translate(Box(2, N / 2.0 + 2), np.array([N / 2, N / 2]).reshape(2, 1))
list_of_polytopes = [
AH_polytope(
np.random.random((n, q)) * 1,
np.random.random((n, 1)) * 10 + 1 * i, B) for i in range(N)
]
zonotopes = {poly: zonotope(poly.t, poly.T) for poly in list_of_polytopes}
fig, ax = plt.subplots() # note we must use plt.subplots, not plt.subplot
visZ(ax, zonotopes.values())
mytree = BSP_tree(list_of_polytopes)
mytree.construct_tree(6, N=3)
mytree._get_polyhedrons(X)
P_list = mytree.build_Plist()
for leaf in mytree.leafs:
print leaf, len(P_list[leaf])
n = 4
q = 5
lc = np.array([[0, 0, 0, 0]]).reshape(4, 1)
uc = np.array([[200, 200, 100, 100]]).reshape(4, 1)
N = 20
X_1 = Box(n, corners=(lc, uc))
#X_2=translate(Box(n,5),np.ones((n,1))*15)
#X_3=translate(Box(n,4),np.array([[15,5]]).reshape(2,1))
#X_3=translate(Box(n,4),np.array([[15,5]]).reshape(2,1))
np.random.seed(50)
s = 1
#list_of_zonotopes=[zonotope(np.random.random((n,1))*10+np.array([0.1*i,0.5*i]).reshape(2,1),np.random.random((n,q))*1) for i in range(N)]
list_of_zonotopes = [
zonotope(
np.random.random((n, 1)) * (uc - lc) + lc,
np.random.random((n, q)) * s - s / 2) for i in range(N)
]
for Z in list_of_zonotopes:
Z.J = np.random.random()
#fig, ax = plt.subplots() # note we must use plt.subplots, not plt.subplot
#visZ(ax,list_of_zonotopes)
mytree = BSP_tree_cells([X_1], list_of_zonotopes)
mytree.construct_tree(D=10, N=5)
#mytree.draw_cells(alpha=0.8)
#
#x=np.array([8,3]).reshape(2,1)
#(Z,D)=mytree.query_closest(x)
#C=mytree._query_find_cell(x)
q = 3
B = Box(q)
N = 200
X_1 = translate(Box(n, 12), np.ones((n, 1)) * 10)
H = np.array([[1, 0], [0, 1], [-1, 0], [0, -1], [1, 1]]).reshape(5, 2)
h = np.array([42, 5, -2, -2, 35]).reshape(5, 1)
X_1 = polytope(H, h)
#X_2=translate(Box(n,5),np.ones((n,1))*15)
#X_3=translate(Box(n,4),np.array([[15,5]]).reshape(2,1))
#X_3=translate(Box(n,4),np.array([[15,5]]).reshape(2,1))
s = 2
#list_of_zonotopes=[zonotope(np.random.random((n,1))*10+np.array([0.1*i,0.5*i]).reshape(2,1),np.random.random((n,q))*1) for i in range(N)]
list_of_zonotopes = [
zonotope(
np.random.random((n, 1)) * np.array([40, 3]).reshape(2, 1),
np.random.random((n, q)) * s - s / 2) for i in range(N)
]
for Z in list_of_zonotopes:
Z.J = np.random.random()
fig, ax = plt.subplots() # note we must use plt.subplots, not plt.subplot
visZ(ax, list_of_zonotopes)
mytree = BSP_tree_cells([X_1], list_of_zonotopes)
mytree.construct_tree(D=10, N=5)
mytree.draw_cells(alpha=0.8)
x = np.array([8, 3]).reshape(2, 1)
(Z, D) = mytree.query_closest(x)
C = mytree._query_find_cell(x)
def disturbed_polytopic_trajectory_given_regions(x0,list_of_cells,goal,eps=0,order=1,scale=[]):
"""
Description:
Polytopic Trajectory Optimization with the ordered list of polytopes given
This is a convex program as mode sequence is already given
list_of_cells: each cell has the following attributes: A,B,c,W and an AH-polytope
"""
if len(scale)==0:
scale=np.ones(x0.shape[0])
model=Model("Fixed Mode Polytopic Trajectory")
T=len(list_of_cells)
n,m=list_of_cells[0].B.shape
q=int(order*n)
n_w=list_of_cells[0].w.G.shape[1]
list_of_q=list(q+np.array(range(T))*n_w)
print list_of_q
x=model.addVars(range(T+1),range(n),lb=-GRB.INFINITY,ub=GRB.INFINITY,name="x")
u=model.addVars(range(T),range(m),lb=-GRB.INFINITY,ub=GRB.INFINITY,name="u")
G,theta={},{}
for t in range(T):
_q=list_of_q[t]
G[t]=model.addVars(range(n),range(_q),lb=-GRB.INFINITY,ub=GRB.INFINITY,name="G_%d"%t)
theta[t]=model.addVars(range(T),range(_q),lb=-GRB.INFINITY,ub=GRB.INFINITY,name="theta_%s"%t)
_q=list_of_q[T-1]+n_w
G[T]=model.addVars(range(n),range(_q),lb=-GRB.INFINITY,ub=GRB.INFINITY,name="G_%d"%T)
model.update()
print "inside function epsilon is",eps
for j in range(n):
model.addConstr(x[0,j]<=x0[j,0]+eps*scale[j])
model.addConstr(x[0,j]>=x0[j,0]-eps*scale[j])
for t in range(T):
print "adding constraints of t",t
cell=list_of_cells[t]
A,B,w,p_x,p_u=cell.A,cell.B,cell.w,cell.p_x,cell.p_u
_q=list_of_q[t]
for j in range(n):
expr_x=LinExpr([(A[j,k],x[t,k]) for k in range(n)])
expr_u=LinExpr([(B[j,k],u[t,k]) for k in range(m)])
model.addConstr(x[t+1,j]==expr_x+expr_u+w.x[j,0])
for i in range(n):
for j in range(_q):
expr_x=LinExpr([(A[i,k],G[t][k,j]) for k in range(n)])
expr_u=LinExpr([(B[i,k],theta[t][k,j]) for k in range(m)])
model.addConstr(G[t+1][i,j]==expr_x+expr_u)
for j in range(_q,_q+n_w):
model.addConstr(G[t+1][i,j]==w.G[i,j-_q])
x_t=np.array([x[t,j] for j in range(n)]).reshape(n,1)
u_t=np.array([u[t,j] for j in range(m)]).reshape(m,1)
G_t=np.array([G[t][i,j] for i in range(n) for j in range(_q)]).reshape(n,_q)
theta_t=np.array([theta[t][i,j] for i in range(m) for j in range(_q)]).reshape(m,_q)
X_t=zonotope(x_t,G_t)
U_t=zonotope(u_t,theta_t)
subset_generic(model,X_t,p_x)
subset_generic(model,U_t,p_u)
_q=list_of_q[T-1]+n_w
x_T=np.array([x[T,j] for j in range(n)]).reshape(n,1)
G_T=np.array([G[T][i,j] for i in range(n) for j in range(_q)]).reshape(n,_q)
z=zonotope(x_T,G_T)
subset_zonotopes(model,z,goal)
# Cost function
J=LinExpr([(1.0/scale[i],G[t][i,i]) for t in range(T+1) for i in range(n)])
model.setObjective(J)
model.write("polytopic_trajectory.lp")
model.setParam('TimeLimit', 150)
model.optimize()
x_num,G_num,theta_num,u_num={},{},{},{}
for t in range(T):
x_num[t]=np.array([[x[t,j].X] for j in range(n)]).reshape(n,1)
_q=list_of_q[t]
G_num[t]=np.array([[G[t][i,j].X] for i in range(n) for j in range(_q)]).reshape(n,_q)
_q=list_of_q[t]+n_w
x_num[T]=np.array([[x[T,j].X] for j in range(n)]).reshape(n,1)
G_num[T]=np.array([[G[T][i,j].X] for i in range(n) for j in range(_q)]).reshape(n,_q)
for t in range(T):
_q=list_of_q[t]
theta_num[t]=np.array([[theta[t][i,j].X] for i in range(m) for j in range(_q)]).reshape(m,_q)
u_num[t]=np.array([[u[t,i].X] for i in range(m) ]).reshape(m,1)
return (x_num,u_num,G_num,theta_num)
def RCI_periodic(sys, q=0, alpha=0, K=5):
"""
Computes a Robust Control Invariant (RCI) set for Linear Periodic Systems
"""
model = Model("RCI_periodic")
q = K * sys.n if q == 0 else q
n_w = sys.n_w
T = sys.T + 1
n = sys.n
m = sys.m
list_of_q = list(q + np.array(range(T)) * n_w)
print list_of_q
G, theta = {}, {}
for t in range(T):
_q = list_of_q[t]
G[t] = model.addVars(range(n),
range(_q),
lb=-GRB.INFINITY,
ub=GRB.INFINITY,
name="G_%d" % t)
theta[t] = model.addVars(range(T),
range(_q),
lb=-GRB.INFINITY,
ub=GRB.INFINITY,
name="theta_%s" % t)
_q = list_of_q[T - 1] + n_w
G[T] = model.addVars(range(n),
range(_q),
lb=-GRB.INFINITY,
ub=GRB.INFINITY,
name="G_%d" % T)
model.update()
for t in range(T):
print "adding constraints of t", t
A, B, W = sys.A[t], sys.B[t], sys.W[t]
_q = list_of_q[t]
for i in range(n):
for j in range(_q):
expr_x = LinExpr([(A[i, k], G[t][k, j]) for k in range(n)])
expr_u = LinExpr([(B[i, k], theta[t][k, j]) for k in range(m)])
model.addConstr(G[t + 1][i, j] == expr_x + expr_u)
for j in range(_q, _q + n_w):
model.addConstr(G[t + 1][i, j] == W[i, j - _q])
G_t = np.array([G[t][i, j] for i in range(n)
for j in range(_q)]).reshape(n, _q)
theta_t = np.array([
theta[t][i, j] for i in range(m) for j in range(_q)
]).reshape(m, _q)
X_t = zonotope(np.zeros((n, 1)), G_t)
U_t = zonotope(np.zeros((m, 1)), theta_t)
subset_generic(model, X_t, sys.X)
subset_generic(model, U_t, sys.U)
_q = list_of_q[T - 1] + n_w
for i in range(n):
for j in range(q):
model.addConstr(G[T][i, j + n_w * (T)] == G[0][i, j])
for j in range(0, n_w * (T)):
model.addConstr(G[T][i, j] == 0)
# Cost function
J = LinExpr([(1.0 / (t + 1.1), G[t][i, i]) for t in range(T + 1)
for i in range(n)])
model.setObjective(J)
model.write("peridoic RCI.lp")
model.setParam('TimeLimit', 150)
model.optimize()
G_num, theta_num = {}, {}
for t in range(T):
_q = list_of_q[t]
G_num[t] = np.array([[G[t][i, j].X] for i in range(n)
for j in range(_q)]).reshape(n, _q)
_q = list_of_q[t] + n_w
G_num[T] = np.array([[G[T][i, j].X] for i in range(n)
for j in range(_q)]).reshape(n, _q)
for t in range(T):
_q = list_of_q[t]
theta_num[t] = np.array([[theta[t][i, j].X] for i in range(m)
for j in range(_q)]).reshape(m, _q)
return (G_num, theta_num)
q = K * sys.n if q == 0 else q
model = Model("RCI_periodic")
phi = tupledict_to_array(
model.addVars(range(sys.n),
range(q),
lb=-GRB.INFINITY,
ub=GRB.INFINITY,
name="phi"))
theta = tupledict_to_array(
model.addVars(range(sys.m),
range(q),
lb=-GRB.INFINITY,
ub=GRB.INFINITY,
name="theta"))
psi = tupledict_to_array(
model.addVars(range(sys.n),
range(sys.w),
lb=-GRB.INFINITY,
ub=GRB.INFINITY,
name="psi"))
model.update()
_fat_matrix = np.hstack((psi, phi))
constraints_list_of_tuples(model, [(sys.A, phi), (sys.B, theta),
(-np.eye(sys.n), _fat_matrix[:, 0:q])],
"=")
constraints_list_of_tuples(model,
[(np.eye(sys.n), sys.W),
(-np.eye(sys.n), _fat_matrix[:, q:q + sys.w])],
"=")
_outer = zonotope(np.zeros((sys.n, 1)), sys.W * alpha)
_inner = zonotope(np.zeros((sys.n, 1)), psi)
subset_generic(model, _inner, _outer)
if sys.X != None:
sys.X.G *= (1 - alpha)
subset_generic(model, zonotope(np.zeros((sys.n, 1)), phi), sys.X)
if sys.U != None:
sys.U.G *= (1 - alpha)
subset_generic(model, zonotope(np.zeros((sys.m, 1)), theta), sys.U)
model.optimize()
phi_n = np.array([[phi[i, j].X for i in range(phi.shape[0])]
for j in range(phi.shape[1])]).T
theta_n = np.array([[theta[i, j].X for i in range(theta.shape[0])]
for j in range(theta.shape[1])]).T
# psi_n=np.array([[psi[i,j].X for i in range(psi.shape[0])] for j in range(psi.shape[1])]).T
sys.phi = phi_n * 1.0 / (1 - alpha)
sys.theta = theta_n * 1.0 / (1 - alpha)
return phi_n, theta_n
from parcis.main.system import system_LTI
from parcis.main.parcis import RCI,RCI_controller
from pypolycontain.visualization.visualize_2D import visualize_2D_zonotopes as visZ
from pypolycontain.visualization.visualize_2D import visualize_2D_zonotopes_ax as visZax
from pypolycontain.lib.zonotope import zonotope
dt=0.05
A=np.array([[1,dt],[0,1]])
B=np.array([[0,dt]]).T
W=np.array([[1,0],[0,1]])*dt*0.2
s=system_LTI(A,B,W)
s.X=zonotope(np.zeros((2,1)), np.eye(2) *2 )
s.U=zonotope(np.zeros((1,1)), np.eye(1) *3 )
alpha=0.5
phi,theta=RCI(s,q=10,alpha=alpha)
visZ([zonotope(np.zeros((2,1)),s.phi,color=(1,0,0))],a=0.02,title=r"RCI set")
#visZ([zonotope(np.zeros((1,1)),theta*1/(1-alpha))])
# Simulate
T=200
x,u={},{}
x[0]=np.array([-0.08,0.3]).reshape(2,1)
for t in range(T):
print t,x[t].T
u[t]=RCI_controller(s,x[t])
zeta_w=np.random.random((2,1))*2-1
import pickle
from pypolycontain.utils.redundancy_reduction import canonical_polytope
from pypolycontain.lib.zonotope import zonotope
from PWA_lib.trajectory.system import linear_cell
from PWA_lib.trajectory.poly_trajectory import polytopic_trajectory_given_modes
from PWA_lib.visualization.visualize import add_tube
import matplotlib.pyplot as plt
q = pickle.load(open("carrot_linear_blocks.pkl", "r"))
X_traj = pickle.load(open("X_traj.pkl", "r"))
U_traj = pickle.load(open("U_traj.pkl", "r"))
T = len(q)
T1 = 420
T2 = 480
x0 = X_traj[:, T1].reshape(10, 1)
goal = zonotope(X_traj[:, T2 + 1].reshape(10, 1), np.eye(10) * 0.001)
for t in range(T1, T2 - 1):
cell = q[t]
A, B, c, p = cell.A, cell.B, cell.c, cell.p
x, u, x_plus = X_traj[:, t].reshape(10, 1), U_traj[:, t].reshape(
4, 1), X_traj[:, t + 1].reshape(10, 1)
e = x_plus - np.dot(A, x) - np.dot(B, u) - c
q[t].c = q[t].c + e
print e.T, e.shape
print all(p.h - np.dot(p.H, np.vstack((x, u))) >= -10**-8)
for t in range(T1, T2 - 1):
print t
cell = q[t]
A, B, c, p = cell.A, cell.B, cell.c, cell.p
