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