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 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 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
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)