def _cut_half(c, P): """ Given polytopen and direction c, cut it into half """ n = c.shape[0] assert n == P.H.shape[1] model = Model("n") x = tupledict_to_array( model.addVars(range(n), [0], lb=-GRB.INFINITY, ub=GRB.INFINITY, name="x")) model.update() constraints_list_of_tuples(model, [(P.H, x), (-np.eye(P.h.shape[0]), P.h)], sign="<") J = LinExpr([(c[i, 0], x[i, 0]) for i in range(n)]) model.setParam('OutputFlag', False) model.setObjective(J, GRB.MINIMIZE) model.optimize() g_min = model.ObjVal model.setObjective(J, GRB.MAXIMIZE) # Max model.reset() model.optimize() g_max = model.ObjVal g = np.array([(g_max + g_min) / 2.0]).reshape(1, 1) P1 = polytope(np.vstack((P.H, c.T)), np.vstack((P.h, g))) P2 = polytope(np.vstack((P.H, -c.T)), np.vstack((P.h, -g))) return P1, P2, (c, g)
def ground_polyhedrons(symbolic_carrot, X, U): """ Returns polyhedron for ground force being zero """ t = time.time() x, y, theta, x_dot, y_dot, theta_dot = X[0:6] c, d, D, psi = X[6:10] v_c, v_d, v_D, v_psi = U[0:4] assert theta < np.pi / 2 # Curve side is below! phi, v_slide = symbolic_carrot.lambda_ground_curve() # Numbers first phi_num = SUB(symbolic_carrot, np.array([phi]).reshape(1, 1), X, U) v_slide_num = SUB(symbolic_carrot, np.array([v_slide]).reshape(1, 1), X, U) """ Returns polyhedron for ground force being zero. No contact """ H_x_symbolic = GRADIENT([phi], symbolic_carrot.X) H_u_symbolic = GRADIENT([phi], symbolic_carrot.U) H_x = SUB(symbolic_carrot, H_x_symbolic, X, U) H_u = SUB(symbolic_carrot, H_u_symbolic, X, U) # print H_x,H_x.shape # print H_u,H_u.shape H = np.hstack((H_x, H_u)) h = np.dot(H_x, X) + np.dot(H_u, U) - phi_num poly_no_contact = polytope(-H, -h) """ Returns polyhedron for ground force with sliding toward positive v_slide=positive H(x,u)<=h """ G_x_symbolic = GRADIENT([v_slide], symbolic_carrot.X) G_u_symbolic = GRADIENT([v_slide], symbolic_carrot.U) G_x = SUB(symbolic_carrot, G_x_symbolic, X, U) G_u = SUB(symbolic_carrot, G_u_symbolic, X, U) G = np.hstack((G_x, G_u)) g = np.dot(G_x, X) + np.dot(G_u, U) - v_slide_num poly_positive_slide = polytope(np.vstack((H, -G)), np.vstack((h, -g))) poly_negative_slide = polytope(np.vstack((H, G)), np.vstack((h, g))) print "time taken", time.time() - t, "seconds" return (poly_no_contact, poly_positive_slide, poly_negative_slide)
def PWA_cells_from_state(symbolic_sys, x, h, epsilon_min, epsilon_max): sys = system_hard_contact_PWA_numeric(symbolic_sys) Eta_now = environment_from_state(symbolic_sys, x, h) sys.add_environment(Eta_now, epsilon_max, epsilon_min) E, e = enumerate_modes_E(sys, Eta_now) list_of_linear_cells = [] for mode in sys.Sigma: p = polytope(E[mode], e[mode]) if is_nonempty(p): new_cell = LC(sys.A[Eta_now], np.hstack((sys.B_u[Eta_now], sys.B_lambda[Eta_now])), sys.c[Eta_now], p) new_cell.name = mode list_of_linear_cells.append(new_cell) return list_of_linear_cells
def build_cells(self): from pypolycontain.utils.redundancy_reduction import canonical_polytope from pypolycontain.lib.polytope import polytope from itertools import product all_modes = list( product(*[self.list_of_modes[n] for n in self.list_of_sum_indices])) self.cell = {} print all_modes for mode in all_modes: delta_PWA = {} for n in self.list_of_sum_indices: for j in range(len(self.list_of_sum_indices)): if self.list_of_sum_indices[j] == n: break for i in self.list_of_modes[n]: # delta_PWA[n,i]=int(mode[n]==i) delta_PWA[n, i] = int(mode[j] == i) print delta_PWA A = sum([ self.A[n, i] * delta_PWA[n, i] for n in self.list_of_sum_indices for i in self.list_of_modes[n] ]) B = sum([ self.B[n, i] * delta_PWA[n, i] for n in self.list_of_sum_indices for i in self.list_of_modes[n] ]) c = sum([ self.c[n, i] * delta_PWA[n, i] for n in self.list_of_sum_indices for i in self.list_of_modes[n] ]) H = np.vstack([ self.C[n, i].H for n in self.list_of_sum_indices for i in self.list_of_modes[n] if delta_PWA[n, i] == 1 ]) h = np.vstack([ self.C[n, i].h for n in self.list_of_sum_indices for i in self.list_of_modes[n] if delta_PWA[n, i] == 1 ]) # H,h=canonical_polytope(H,h) cell = linear_cell(A, B, c, polytope(H, h)) self.cell[mode] = (cell)
def hybrid_reachable_sets_from_state(symbolic_sys, x, h, epsilon_min, epsilon_max): sys = system_hard_contact_PWA_numeric(symbolic_sys) Eta_now = environment_from_state(symbolic_sys, x, h) sys.add_environment(Eta_now, epsilon_max, epsilon_min) E, e = enumerate_modes_E(sys, Eta_now) list_of_sets = [] for mode in sys.Sigma: H = E[mode][:, symbolic_sys.n:] q = H.shape[0] h = e[mode].reshape(q, 1) - np.dot(E[mode][:, :symbolic_sys.n], x.reshape(symbolic_sys.n, 1)) p = polytope(H, h) if is_nonempty(p): B = np.hstack((sys.B_u[Eta_now], sys.B_lambda[Eta_now])) t = np.dot(sys.A[Eta_now], x) + sys.c[Eta_now] new_set = AH_polytope(T=B, t=t, P=p) new_set.name = mode list_of_sets.append(new_set) return list_of_sets
def find_the_dynamics(pwa_cell, x, u, A0, B0, c0, H0, h0, x_plus, eps=1, delta_t=0.0025): A = A0 B = B0 c = c0 H = H0 h = h0 xu = np.vstack((x, u)) for contact in ["ground", "left", "right"]: for i in range(3): p = pwa_cell[i, contact].p # print p.H.shape,p.h.shape,xu.shape flag = point_in_polytope(p, xu) # print p.H,p.h,p.h-np.dot(p.H,xu) print p.h - np.dot(p.H, xu) print contact, i, flag A = A + pwa_cell[i, contact].A * flag * delta_t B = B + pwa_cell[i, contact].B * flag * delta_t c = c + pwa_cell[i, contact].c * flag * delta_t if flag == True: H = np.vstack((H, p.H)) h = np.vstack((h, p.h)) # print c0.T (H, h) = canonical_polytope(H, h) e = x_plus - np.dot(A, x) - np.dot(B, u) - c c = c + e print e.T, (x_plus - np.dot(A, x) - np.dot(B, u) - c).T return linear_cell(A, B, c, polytope(H, h))
def trajectory_to_list_of_linear_cells(sys, Eta, x_traj, u_traj, lambda_traj, mode_traj): """ Assumption: single environment """ E, e = enumerate_modes_E(sys, Eta) T = max(mode_traj.keys()) + 1 list_of_cells = [] if Eta not in sys.Eta: raise ValueError("Enviornement not defined / added to the system yet") for t in range(T): p = polytope(E[mode_traj[t]], e[mode_traj[t]]) new_cell = LC(sys.A[Eta], np.hstack((sys.B_u[Eta], sys.B_lambda[Eta])), sys.c[Eta], p) list_of_cells.append(new_cell) # Verify correctness: v = np.hstack((x_traj[t], u_traj[t], lambda_traj[t])).reshape(p.n, 1) if p.if_inside(v, tol=10**-5) == False: raise ValueError( "Nominal Trajectory not inside polytope! Maybe an issue of numerical tolerance" ) return list_of_cells
def trajectory_to_list_of_linear_cells_full_linearization( symbolic_sys, x_traj, u_traj, lambda_traj, mode_traj, h, epsilon_min, epsilon_max): """ Linearize all points """ list_of_cells = [] T = max(mode_traj.keys()) + 1 for t in range(T): sys = system_hard_contact_PWA_numeric(symbolic_sys) Eta_now = environment_from_state(symbolic_sys, x_traj[t], h) sys.add_environment(Eta_now, epsilon_max, epsilon_min) E, e = enumerate_modes_E(sys, Eta_now) p = polytope(E[mode_traj[t]], e[mode_traj[t]]) new_cell = LC(sys.A[Eta_now], np.hstack((sys.B_u[Eta_now], sys.B_lambda[Eta_now])), sys.c[Eta_now], p) list_of_cells.append(new_cell) # Verify correctness: v = np.hstack((x_traj[t], u_traj[t], lambda_traj[t])).reshape(p.n, 1) # print p.h-np.dot(p.H,v) # if p.if_inside(v,tol=10**-1)==False: # raise ValueError("Nominal Trajectory not inside polytope! Maybe an issue of numerical tolerance") return list_of_cells
sys.c[1, 0] = np.zeros((2, 1)) sys.A[1, 1] = np.array([[0, 0], [-10, 0]]) sys.B[1, 1] = np.array([[0, 0]]).T sys.c[1, 1] = np.array([[0, 1]]).T sys.A[2, 0] = np.zeros((2, 2)) sys.B[2, 0] = np.zeros((2, 1)) sys.c[2, 0] = np.zeros((2, 1)) sys.A[2, 1] = np.array([[0, 0], [-10, 0]]) sys.B[2, 1] = np.array([[0, 0]]).T sys.c[2, 1] = np.array([[0, -1]]).T 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.12, 1, 4, 4]]).T sys.C[0, 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.1, 1, 0.12, 1, 4, 4]]).T sys.C[1, 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[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)
from pypolycontain.visualization.visualize_2D import visualize_2D_zonotopes_ax as visZ from pypolycontain.visualization.visualize_2D import visualize_2D as vis from PWA_lib.polytree.bsp.bsp import BSP_tree_cells import matplotlib.pyplot as plt from matplotlib.patches import Rectangle n = 2 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
sys.A[1, 0] = np.array([[1, 0.01], [0.1, 1]]) sys.A[1, 1] = np.array([[1, 0.01], [-9.9, 1]]) sys.B[1, 0] = np.array([[0, 0.01]]).T sys.B[1, 1] = np.array([[0, 0.01]]).T sys.c[1, 0] = np.array([[0, 0]]).T sys.c[1, 1] = np.array([[0, 1]]).T sys.A[1, 1] = sys.A[1, 0] sys.B[1, 1] = sys.B[1, 0] sys.c[1, 1] = sys.c[1, 0] 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.10, 1, 0.12, 1, 4, 4]]).T sys.C[1, 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) sys.goal = zonotope( np.array([0, 0]).reshape(2, 1), np.array([[0.0, 0], [0, 0]])) sys.n = 2 sys.m = 1 sys.list_of_sum_indices = [1] sys.list_of_modes = {} sys.list_of_modes[1] = [0, 1]
def Ball(n): H=np.vstack((np.eye(n),-np.eye(n))) h=np.ones((2*n,1)) return polytope(H,h)