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