def comparison_test(self):
p = pc.Polytope(self.A, self.b)
p2 = pc.Polytope(self.A, 2*self.b)
assert(p <= p2)
assert(not p2 <= p)
assert(not p2 == p)
r = pc.Region([p])
r2 = pc.Region([p2])
assert(r <= r2)
assert(not r2 <= r)
assert(not r2 == r)
# test H-rep -> V-rep -> H-rep
v = pc.extreme(p)
p3 = pc.qhull(v)
assert(p3 == p)
# test V-rep -> H-rep with d+1 points
p4 = pc.qhull(np.array([[0, 0], [1, 0], [0, 1]]))
assert(p4 == pc.Polytope(
np.array([[1, 1], [0, -1], [0, -1]]),
np.array([1, 0, 0])))
def __init__(self, step_time=0.01):
self.cart = Cart()
self.obstacles = list() #list of polytopes
self.cart_poly = pt.qhull(self.cart.corners_position().T)
self.goal = pt.qhull(np.array([[1.3, 1.3], [1.3, 2], [2, 1.3], [2,
2]]))
self.frame = pt.qhull(
np.array([[-0.5, -0.5], [2, -0.5], [-0.5, 2], [2, 2]]))
self.step_time = step_time
print('init successful')
def checkObstacleFront(data_points, curr_state, waypoint):
curr_x = curr_state[0]
curr_y = curr_state[1]
# First box
[x2, y2] = waypoint.mode_parameters[0:2]
theta = np.arctan2(y2-curr_y, x2-curr_x)
x_tmp = 0.5
y_tmp = 0
dx = x_tmp*np.cos(theta+np.pi/2) - y_tmp*np.sin(theta+np.pi/2)
dy = x_tmp*np.sin(theta+np.pi/2) + y_tmp*np.cos(theta+np.pi/2)
transform_vector = np.array([[dx,dy],[dx,dy],[-dx,-dy],[-dx,-dy]])
center_vector = np.array([[x2,y2],[curr_x,curr_y],[curr_x,curr_y],[x2,y2]])
vertices = center_vector + transform_vector
poly = pc.qhull(vertices)
res1 = poly.contains(data_points[:,0:2].T)
res1 = np.any(res1)
# Second box
[x1, y1] = waypoint.mode_parameters[0:2]
[x2, y2] = waypoint.mode_parameters[2:4]
theta = np.arctan2(y2-y1, x2-x1)
x_tmp = 1
y_tmp = 0
dx = x_tmp*np.cos(theta+np.pi/2) - y_tmp*np.sin(theta+np.pi/2)
dy = x_tmp*np.sin(theta+np.pi/2) + y_tmp*np.cos(theta+np.pi/2)
transform_vector = np.array([[dx,dy],[dx,dy],[-dx,-dy],[-dx,-dy]])
center_vector = np.array([[x2,y2],[x1,y1],[x1,y1],[x2,y2]])
vertices = center_vector + transform_vector
poly = pc.qhull(vertices)
res2 = poly.contains(data_points[:,0:2].T)
res2 = np.any(res2)
# Third box
[x1, y1] = waypoint.mode_parameters[2:4]
[x2, y2] = waypoint.mode_parameters[4:6]
theta = np.arctan2(y2-y1, x2-x1)
x_tmp = 1
y_tmp = 0
dx = x_tmp*np.cos(theta+np.pi/2) - y_tmp*np.sin(theta+np.pi/2)
dy = x_tmp*np.sin(theta+np.pi/2) + y_tmp*np.cos(theta+np.pi/2)
transform_vector = np.array([[dx,dy],[dx,dy],[-dx,-dy],[-dx,-dy]])
center_vector = np.array([[x2,y2],[x1,y1],[x1,y1],[x2,y2]])
vertices = center_vector + transform_vector
poly = pc.qhull(vertices)
res3 = poly.contains(data_points[:,0:2].T)
res3 = np.any(res3)
return int(res1 or res2 or res3)
def test_expand():
from polytope import dilate
orig_poly = pc.qhull(np.array([[0, 0], [1, 0], [1, 1]]))
dil_poly = dilate(orig_poly, 0.2)
for x in np.array([[0, 0], [1, 0], [1, 1]]):
for theta in np.arange(-np.pi, np.pi, 0.1):
xt = x + np.array([0.2 * math.sin(theta), 0.2 * math.cos(theta)])
assert xt in dil_poly
def test_shrink_too_much():
from polytope import erode
orig_poly = pc.qhull(np.array([[0, 0], [1, 0], [1, 1]]))
print(orig_poly)
er_poly = erode(orig_poly, 2)
print(er_poly)
for x in np.array([[-.01, -.01], [1.01, 0], [1.01, 1]]):
for theta in np.arange(-np.pi, np.pi, 0.1):
xt = x + np.array([2 * math.sin(theta), 2 * math.cos(theta)])
assert not (xt in er_poly)
def minkowski_sum(X,Y):
v_sum = []
if isinstance(X,pt.Polytope):
X = pt.extreme(X) # make sure it is V version
if isinstance(Y,pt.Polytope):
Y = pt.extreme(Y)
for i in range(X.shape[0]):
for j in range(Y.shape[0]):
v_sum.append(X[i,:]+Y[j,:])
return pt.qhull(np.asarray(v_sum))
def test_quickhull_algorithm():
P_points = [
np.array([-6., -6.]),
np.array([-6.0, 6.0]),
np.array([6.0, 6.0]),
np.array([6.0, -6.0])
]
Q = polytope.qhull(np.array(P_points), [3, 1, 2])
print(Q)
print(Q.vertices)
assert False
def checkObstacleFrontLeft(data_points, curr_state, waypoint):
# Second box
[x1, y1] = waypoint.mode_parameters[0:2]
[x2, y2] = waypoint.mode_parameters[2:4]
theta = np.arctan2(y2-y1, x2-x1)
x_tmp = 4
y_tmp = 0
dx1 = x_tmp*np.cos(theta+np.pi/2) - y_tmp*np.sin(theta+np.pi/2)
dy1 = x_tmp*np.sin(theta+np.pi/2) + y_tmp*np.cos(theta+np.pi/2)
x_tmp = 2
y_tmp = 0
dx2 = x_tmp*np.cos(theta+np.pi/2) - y_tmp*np.sin(theta+np.pi/2)
dy2 = x_tmp*np.sin(theta+np.pi/2) + y_tmp*np.cos(theta+np.pi/2)
transform_vector = np.array([[dx1,dy1],[dx1,dy1],[dx2,dy2],[dx2,dy2]])
center_vector = np.array([[x2,y2],[x1,y1],[x1,y1],[x2,y2]])
vertices = center_vector + transform_vector
poly = pc.qhull(vertices)
res2_left = poly.contains(data_points[:,0:2].T)
res2_left = np.any(res2_left)
# Third box
[x1, y1] = waypoint.mode_parameters[2:4]
[x2, y2] = waypoint.mode_parameters[4:6]
theta = np.arctan2(y2-y1, x2-x1)
x_tmp = 4
y_tmp = 0
dx1 = x_tmp*np.cos(theta+np.pi/2) - y_tmp*np.sin(theta+np.pi/2)
dy1 = x_tmp*np.sin(theta+np.pi/2) + y_tmp*np.cos(theta+np.pi/2)
x_tmp = 2
y_tmp = 0
dx2 = x_tmp*np.cos(theta+np.pi/2) - y_tmp*np.sin(theta+np.pi/2)
dy2 = x_tmp*np.sin(theta+np.pi/2) + y_tmp*np.cos(theta+np.pi/2)
transform_vector = np.array([[dx1,dy1],[dx1,dy1],[dx2,dy2],[dx2,dy2]])
center_vector = np.array([[x2,y2],[x1,y1],[x1,y1],[x2,y2]])
vertices = center_vector + transform_vector
poly = pc.qhull(vertices)
res3_left = poly.contains(data_points[:,0:2].T)
res3_left = np.any(res3_left)
return int(res2_left or res3_left)
def plotrun(self):
Tp = self.Tp
run_Tp = self.run_Tp
n = np.shape(Tp.get("Tp.vert")[0])[1]
ax = self.h_fig
Tp_vert = Tp.get("Tp.vert")
# fig = plt.figure()
# ax = fig.add_subplot(111)
if len(run_Tp) == 0:
print("LTL formula cannot be satisfied")
return
if n == 2:
states = run_Tp[0][0]
for i in states:
k = pc.qhull(Tp_vert[int(i)])
tmp = _get_patch(k,
edgecolor="Black",
linewidth=0.15,
facecolor="gray",
fill=True)
ax.add_patch(tmp)
# plt.pause(0.1)
plt.pause(0.1)
states = [run_Tp[0][0][-1]] + run_Tp[0][1]
states = [int(i) for i in states]
for i in states[:-1]:
k = pc.qhull(Tp_vert[i])
tmp = _get_patch(k,
edgecolor="Black",
linewidth=0.15,
facecolor="pink",
fill=True)
ax.add_patch(tmp)
plt.pause(0.1)
def get_Cartesian_polytope2(W, tension_space_Vrep):
"""
:param W: Wrench matrix
:param tension_space_Vrep: tension space poltope vertices
:return: Wrench space vertices
"""
# for each vertex in the tension space project to Wrench space using the wrench matrix
Pv = np.zeros([np.shape(tension_space_Vrep)[0], np.shape(W)[0]])
for row, i in zip(tension_space_Vrep,
range(np.shape(tension_space_Vrep)[0])):
Pv[i, :] = np.matmul(W, row)
polyhull = polytope.qhull(Pv)
return polyhull
def minkowski_sum(X, Y):
"""
Minkowski sum between two polytopes based on
vertex enumeration. So, it's not fast for the
high dimensional polytopes with lots of vertices. """
V_sum = []
if isinstance(X, pt.Polytope):
V1 = pt.extreme(X)
else:
# assuming vertices are in (N x d) shape. N # of vertices, d dimension
V1 = X
if isinstance(Y, pt.Polytope):
V2 = pt.extreme(Y)
else:
V2 = Y
for i in range(V1.shape[0]):
for j in range(V2.shape[0]):
V_sum.append(V1[i,:] + V2[j,:])
return pt.qhull(np.asarray(V_sum))
def dilate(poly, eps):
"""
The function dilates a polytope.
For a given polytope a polytopic over apoproximation of the $eps$-dilated set is computed.
An e-dilated Pe set of P is defined as:
Pe = {x+n|x in P ^ n in Ball(e)}
where Ball(e) is the epsilon neighborhood with norm |n|<e
The current implementation is quite crude, hyper-boxes are placed over the original vertices
and the returned polytope is a qhull of these new vertices.
:param poly: original polytope
:param eps: positive scalar value with which the polytope is dilated
:return: polytope
"""
if isinstance(poly, polytope.Region):
dil_reg = []
for pol in poly.list_poly:
assert isinstance(pol, polytope.Polytope)
dil_reg += [dilate(pol, eps)]
return polytope.Region(dil_reg)
vertices = extreme(poly)
dim = len(vertices[0]) # this is the dimensionality of the space
dil_eps = dim * [[-eps, eps]]
dil_eps_v = [np.array(n) for n in itertools.product(*dil_eps)
] # vectors with (+- eps,+- eps, +- eps,...)
new_vertices = []
for v, d in itertools.product(vertices, dil_eps_v):
new_vertices += [[np.array(v).flatten() + np.array(d).flatten()]]
# make box
# print("add vertices part:", np.array(v).flatten() + np.array(d).flatten())
VV = np.concatenate(new_vertices)
# print("V", VV)
return qhull(VV)
def add_obstacle(self, corners): #2d array, or 2d list
p1 = pt.qhull(np.asarray(corners))
self.obstacles.append(p1)
def set_frame(self, corners):
self.frame = pt.qhull(np.asarray(corners))
def set_goal(self, corners):
self.goal = pt.qhull(np.asarray(corners))
def update_cart_polytope(self):
self.cart_poly = pt.qhull(self.cart.corners_position().T)
def set_params(T, eps_exp3):
np.random.seed()
d = 2 # dimension of space
p = 0.7
# variable is controlled by the outside file
delta = 0.05 #radius of best-responses -- implicitly affects regret
agent_type = [0] * T
agent_type = np.random.binomial(1, p, T)
true_labels = [1 if agent_type[i] else -1 for i in range(T)]
#original feature vectors for agents
x_real = []
for i in range(T):
if agent_type[i]:
x_real.append(
np.array([
np.random.normal(0.6, 0.4),
np.random.normal(0.4, 0.6), 1
]))
else:
x_real.append(
np.array([
np.random.normal(0.4, 0.6),
np.random.uniform(0.6, 0.4), 1
]))
calA_size_exp3 = 1000
noise = []
initial = []
zero = np.array([0, 0, 1])
one = np.array([1, 1, 1])
curr_size = 0
while curr_size < calA_size_exp3:
temp = np.array([
np.random.uniform(-1, 1),
np.random.uniform(-1, 1),
np.random.uniform(-1, 1)
])
dist0 = np.abs(1.0 * np.dot(temp, zero) / np.linalg.norm(temp[:d]))
dist1 = np.abs(1.0 * np.dot(temp, one) / np.linalg.norm(temp[:d]))
if dist0 <= np.sqrt(2) and dist1 <= np.sqrt(2):
initial.append(temp)
curr_size += 1
calA_size = len(initial)
# construct initial polytope, i.e., [-1,1]^{d+1}
V = np.array([
np.array([-1, -1, -1]),
np.array([-1, -1, 1]),
np.array([-1, 1, -1]),
np.array([-1, 1, 1]),
np.array([1, -1, -1]),
np.array([1, -1, 1]),
np.array([1, 1, -1]),
np.array([1, 1, 1])
])
p_init = pc.qhull(V)
# start with a prob and weight of 1 for the org polytope
calA_exp3 = [init / np.linalg.norm(init[:d]) for init in initial]
updated = [0] * T
initial_polytope = Grind_Polytope(p_init, 1.0, 1.0, 2, T, 0.0, 0.0,
updated)
calA_grind = [initial_polytope]
return (T, d, x_real, calA_exp3, calA_grind, agent_type, true_labels,
delta, noise, p)
def get_input(x0,
ssys,
abstraction,
start,
end,
R=None,
r=None,
Q=None,
ord=1,
mid_weight=0.0,
solver=None):
"""Compute continuous control input for discrete transition.
Computes a continuous control input sequence
which takes the plant:
- from state C{start}
- to state C{end}
These are states of the partition C{abstraction}.
The computed control input is such that::
f(x, u) = |Rx|_{ord} + |Qu|_{ord} + r'x +
mid_weight * |xc - x(N)|_{ord}
be minimal.
C{xc} is the chebyshev center of the final cell.
If no cost parameters are given, then the defaults are:
- Q = I
- mid_weight = 3
Notes
=====
1. The same horizon length as in reachability analysis
should be used in order to guarantee feasibility.
2. If the closed loop algorithm has been used
to compute reachability the input needs to be
recalculated for each time step
(with decreasing horizon length).
In this case only u(0) should be used as
a control signal and u(1) ... u(N-1) discarded.
3. The "conservative" calculation makes sure that
the plant remains inside the convex hull of the
starting region during execution, i.e.::
x(1), x(2) ... x(N-1) are
\in conv_hull(starting region).
If the original proposition preserving partition
is not convex, then safety cannot be guaranteed.
@param x0: initial continuous state
@type x0: numpy 1darray
@param ssys: system dynamics
@type ssys: L{LtiSysDyn}
@param abstraction: abstract system dynamics
@type abstraction: L{AbstractPwa}
@param start: index of the initial state in C{abstraction.ts}
@type start: int >= 0
@param end: index of the end state in C{abstraction.ts}
@type end: int >= 0
@param R: state cost matrix for::
x = [x(1)' x(2)' .. x(N)']'
If empty, zero matrix is used.
@type R: size (N*xdim x N*xdim)
@param r: cost vector for state trajectory:
x = [x(1)' x(2)' .. x(N)']'
@type r: size (N*xdim x 1)
@param Q: input cost matrix for control input::
u = [u(0)' u(1)' .. u(N-1)']'
If empty, identity matrix is used.
@type Q: size (N*udim x N*udim)
@param mid_weight: cost weight for |x(N)-xc|_{ord}
@param ord: norm used for cost function::
f(x, u) = |Rx|_{ord} + |Qu|_{ord} + r'x +
mid_weight *|xc - x(N)|_{ord}
@type ord: ord \in {1, 2, np.inf}
@return: array A where row k contains the
control input: u(k)
for k = 0, 1 ... N-1
@rtype: (N x m) numpy 2darray
"""
part = abstraction.ppp
regions = part.regions
ofts = abstraction.ts
original_regions = abstraction.orig_ppp
orig = abstraction._ppp2orig
params = abstraction.disc_params
N = params['N'] # horizon length
conservative = params['conservative']
closed_loop = params['closed_loop']
if closed_loop:
logger.warning('`closed_loop = True` for controller computation. '
'This option is under development: use with caution.')
if (R is None and Q is None and r is None and mid_weight == 0):
# Default behavior
Q = np.eye(N * ssys.B.shape[1])
R = np.zeros([N * x0.size, N * x0.size])
r = np.zeros([N * x0.size, 1])
mid_weight = 3
if R is None:
R = np.zeros([N * x0.size, N * x0.size])
if Q is None:
Q = np.eye(N * ssys.B.shape[1])
if r is None:
r = np.zeros([N * x0.size, 1])
if (R.shape[0] != R.shape[1]) or (R.shape[0] != N * x0.size):
raise Exception("get_input: "
"R must be square and have side N * dim(state space)")
if (Q.shape[0] != Q.shape[1]) or (Q.shape[0] != N * ssys.B.shape[1]):
raise Exception("get_input: "
"Q must be square and have side N * dim(input space)")
if ofts is not None:
start_state = start
end_state = end
if end_state not in ofts.states.post(start_state):
raise Exception('get_input: '
'no transition from state s' + str(start) +
' to state s' + str(end))
else:
print("get_input: "
"Warning, no transition matrix found, assuming feasible")
if (not conservative) & (orig is None):
print("List of original proposition preserving "
"partitions not given, reverting to conservative mode")
conservative = True
P_start = regions[start]
P_end = regions[end]
n = ssys.A.shape[1]
m = ssys.B.shape[1]
idx = range((N - 1) * n, N * n)
if conservative:
# Take convex hull or P_start as constraint
if len(P_start) > 0:
if len(P_start) > 1:
# Take convex hull
vert = pc.extreme(P_start[0])
for i in range(1, len(P_start)):
vert = np.vstack([vert, pc.extreme(P_start[i])])
P1 = pc.qhull(vert)
else:
P1 = P_start[0]
else:
P1 = P_start
else:
# Take original proposition preserving cell as constraint
P1 = original_regions[orig[start]]
# must be single polytope (ensuring convex)
assert len(P1) > 0, P1
if len(P1) == 1:
P1 = P1[0]
else:
print(P1)
raise Exception('`conservative = False` arg requires '
'that original regions be convex')
if len(P_end) > 0:
low_cost = np.inf
low_u = np.zeros([N, m])
# for each polytope in target region
for P3 in P_end:
cost = np.inf
if mid_weight > 0:
rc, xc = pc.cheby_ball(P3)
R[np.ix_(range(n * (N - 1), n * N),
range(n * (N - 1), n * N))] += mid_weight * np.eye(n)
r[idx, 0] += -mid_weight * xc
try:
u, cost = get_input_helper(x0,
ssys,
P1,
P3,
N,
R,
r,
Q,
ord,
closed_loop=closed_loop,
solver=solver)
except _InputHelperLPException as ex:
# The end state might consist of several polytopes.
# For some of them there might not be a control action that
# brings the system there. In that case the matrix
# constructed by get_input_helper will be singular and the
# LP solver cannot return a solution.
# This is not a problem unless all polytopes in the end
# region are unreachable, in which case it seems likely that
# there is something wrong with the abstraction routine.
logger.info(repr(ex))
logger.info(
("Failed to find control action from continuous "
"state {x0} in discrete state {start} "
"to a target polytope in the discrete state {end}.\n"
"Target polytope:\n{P3}").format(x0=x0,
start=start,
end=end,
P3=P3))
r[idx, 0] += mid_weight * xc
if cost < low_cost:
low_u = u
low_cost = cost
if low_cost == np.inf:
raise Exception("get_input: Did not find any trajectory")
else:
P3 = P_end
if mid_weight > 0:
rc, xc = pc.cheby_ball(P3)
R[np.ix_(range(n * (N - 1), n * N),
range(n * (N - 1), n * N))] += mid_weight * np.eye(n)
r[idx, 0] += -mid_weight * xc
low_u, cost = get_input_helper(x0,
ssys,
P1,
P3,
N,
R,
r,
Q,
ord,
closed_loop=closed_loop,
solver=solver)
return low_u
def get_input(
x0, ssys, abstraction,
start, end,
R=None, r=None, Q=None,
ord=1, mid_weight=0.0
):
"""Compute continuous control input for discrete transition.
Computes a continuous control input sequence
which takes the plant:
- from state C{start}
- to state C{end}
These are states of the partition C{abstraction}.
The computed control input is such that::
f(x, u) = |Rx|_{ord} + |Qu|_{ord} + r'x +
mid_weight * |xc - x(N)|_{ord}
be minimal.
C{xc} is the chebyshev center of the final cell.
If no cost parameters are given, then the defaults are:
- Q = I
- mid_weight = 3
Notes
=====
1. The same horizon length as in reachability analysis
should be used in order to guarantee feasibility.
2. If the closed loop algorithm has been used
to compute reachability the input needs to be
recalculated for each time step
(with decreasing horizon length).
In this case only u(0) should be used as
a control signal and u(1) ... u(N-1) discarded.
3. The "conservative" calculation makes sure that
the plant remains inside the convex hull of the
starting region during execution, i.e.::
x(1), x(2) ... x(N-1) are
\in conv_hull(starting region).
If the original proposition preserving partition
is not convex, then safety cannot be guaranteed.
@param x0: initial continuous state
@type x0: numpy 1darray
@param ssys: system dynamics
@type ssys: L{LtiSysDyn}
@param abstraction: abstract system dynamics
@type abstraction: L{AbstractPwa}
@param start: index of the initial state in C{abstraction.ts}
@type start: int >= 0
@param end: index of the end state in C{abstraction.ts}
@type end: int >= 0
@param R: state cost matrix for::
x = [x(1)' x(2)' .. x(N)']'
If empty, zero matrix is used.
@type R: size (N*xdim x N*xdim)
@param r: cost vector for state trajectory:
x = [x(1)' x(2)' .. x(N)']'
@type r: size (N*xdim x 1)
@param Q: input cost matrix for control input::
u = [u(0)' u(1)' .. u(N-1)']'
If empty, identity matrix is used.
@type Q: size (N*udim x N*udim)
@param mid_weight: cost weight for |x(N)-xc|_{ord}
@param ord: norm used for cost function::
f(x, u) = |Rx|_{ord} + |Qu|_{ord} + r'x +
mid_weight *|xc - x(N)|_{ord}
@type ord: ord \in {1, 2, np.inf}
@return: array A where row k contains the
control input: u(k)
for k = 0, 1 ... N-1
@rtype: (N x m) numpy 2darray
"""
part = abstraction.ppp
regions = part.regions
ofts = abstraction.ts
original_regions = abstraction.orig_ppp
orig = abstraction._ppp2orig
params = abstraction.disc_params
N = params['N'] # horizon length
conservative = params['conservative']
closed_loop = params['closed_loop']
if closed_loop:
logger.warning(
'`closed_loop = True` for controller computation. '
'This option is under development: use with caution.')
if (
R is None and
Q is None and
r is None and
mid_weight == 0):
# Default behavior
Q = np.eye(N * ssys.B.shape[1])
R = np.zeros([N * x0.size, N * x0.size])
r = np.zeros([N * x0.size, 1])
mid_weight = 3
if R is None:
R = np.zeros([N * x0.size, N * x0.size])
if Q is None:
Q = np.eye(N * ssys.B.shape[1])
if r is None:
r = np.zeros([N * x0.size, 1])
if (R.shape[0] != R.shape[1]) or (R.shape[0] != N * x0.size):
raise Exception("get_input: "
"R must be square and have side N * dim(state space)")
if (Q.shape[0] != Q.shape[1]) or (Q.shape[0] != N * ssys.B.shape[1]):
raise Exception("get_input: "
"Q must be square and have side N * dim(input space)")
if ofts is not None:
start_state = start
end_state = end
if end_state not in ofts.states.post(start_state):
raise Exception('get_input: '
'no transition from state s' + str(start) +
' to state s' + str(end)
)
else:
print("get_input: "
"Warning, no transition matrix found, assuming feasible")
if (not conservative) & (orig is None):
print("List of original proposition preserving "
"partitions not given, reverting to conservative mode")
conservative = True
P_start = regions[start]
P_end = regions[end]
n = ssys.A.shape[1]
m = ssys.B.shape[1]
idx = range((N - 1) * n, N * n)
if conservative:
# Take convex hull or P_start as constraint
if len(P_start) > 0:
if len(P_start) > 1:
# Take convex hull
vert = pc.extreme(P_start[0])
for i in range(1, len(P_start)):
vert = np.vstack([
vert,
pc.extreme(P_start[i])
])
P1 = pc.qhull(vert)
else:
P1 = P_start[0]
else:
P1 = P_start
else:
# Take original proposition preserving cell as constraint
P1 = original_regions[orig[start]]
# must be single polytope (ensuring convex)
assert len(P1) > 0, P1
if len(P1) == 1:
P1 = P1[0]
else:
print(P1)
raise Exception(
'`conservative = False` arg requires '
'that original regions be convex')
if len(P_end) > 0:
low_cost = np.inf
low_u = np.zeros([N, m])
# for each polytope in target region
for P3 in P_end:
if mid_weight > 0:
rc, xc = pc.cheby_ball(P3)
R[
np.ix_(
range(n * (N - 1), n * N),
range(n * (N - 1), n * N)
)
] += mid_weight * np.eye(n)
r[idx, 0] += -mid_weight * xc
u, cost = get_input_helper(
x0, ssys, P1, P3, N, R, r, Q, ord,
closed_loop=closed_loop
)
r[idx, 0] += mid_weight * xc
if cost < low_cost:
low_u = u
low_cost = cost
if low_cost == np.inf:
raise Exception("get_input: Did not find any trajectory")
else:
P3 = P_end
if mid_weight > 0:
rc, xc = pc.cheby_ball(P3)
R[
np.ix_(
range(n * (N - 1), n * N),
range(n * (N - 1), n * N)
)
] += mid_weight * np.eye(n)
r[idx, 0] += -mid_weight * xc
low_u, cost = get_input_helper(
x0, ssys, P1, P3, N, R, r, Q, ord,
closed_loop=closed_loop
)
return low_u
def projection(X,nx):
V_sum = []
V = pt.extreme(X)
for i in range(V.shape[0]):
V_sum.append(V[i,0:nx])
return pt.qhull(np.asarray(V_sum))
trace[i].extend(trajectory[i])
trace[i].extend([res_front, res_front_left, res_front_right])
print([res_front, res_front_left, res_front_right])
return trace
def TC_Simulate(waypoint, time_step, initial_point):
res = runModel(waypoint, time_step, initial_point, waypoint.time_bound)
return res
if __name__ == "__main__":
init_x = 0
init_y = 0
init_theta = 0
vertices = np.array([[10,3],[15,3],[15,4],[10,4]])
poly1 = pc.qhull(vertices)
vertices = np.array([[10,-3],[15,-3],[15,-4],[10,-4]])
poly2 = pc.qhull(vertices)
waypoint = Waypoint("follow_waypoint", [5,0,10,0,15,0], 10, 0, [poly1, poly2])
init_point = [init_x, init_y, init_theta]
res = TC_Simulate(waypoint, 0.01, init_point)
print(res)
# state = [0,1,np.pi/2]
# vertices = np.array([[10,-5],[20,-5],[20,5],[10,5]])
# poly1 = pc.qhull(vertices)
# vertices = np.array([[0,10],[5,15],[0,20],[-5,15]])
# poly2 = pc.qhull(vertices)
# point_cloud = lidarSimulator(state,[poly1, poly2])
# plt.plot(point_cloud[:,0],point_cloud[:,1],'.')
P = pt.Polytope(A,b)
if False:
fig, ax = plt.subplots(1,1)
plt.rcParams['figure.figsize'] = [20, 20]
P.plot(ax, color='r')
ax.autoscale_view()
ax.axis('equal')
plt.show()
# reduce
P = pt.reduce(P)
print(P)
# HV conversion
V=np.array([[10,10],[-10,10],[10,-10],[-10,-10]])
P = pt.qhull(V)
print(P)
V1 = pt.extreme(P)
print(V1)
# Minkwoski sum of two Polytopes
def minkowski_sum(X,Y):
v_sum = []
if isinstance(X,pt.Polytope):
X = pt.extreme(X) # make sure it is V version
if isinstance(Y,pt.Polytope):
Y = pt.extreme(Y)
def get_input(
x0, ssys, abstraction,
start, end,
R=[], r=[], Q=[], mid_weight=0.0,
test_result=False
):
"""Compute continuous control input for discrete transition.
Computes a continuous control input sequence
which takes the plant:
- from state C{start}
- to state C{end}
These are states of the partition C{abstraction}.
The computed control input is such that::
f(x, u) = x'Rx +r'x +u'Qu +mid_weight *|xc-x(0)|_2
be minimal.
C{xc} is the chebyshev center of the final cell.
If no cost parameters are given, then the defaults are:
- Q = I
- mid_weight = 3
Notes
=====
1. The same horizon length as in reachability analysis
should be used in order to guarantee feasibility.
2. If the closed loop algorithm has been used
to compute reachability the input needs to be
recalculated for each time step
(with decreasing horizon length).
In this case only u(0) should be used as
a control signal and u(1) ... u(N-1) discarded.
3. The "conservative" calculation makes sure that
the plant remains inside the convex hull of the
starting region during execution, i.e.::
x(1), x(2) ... x(N-1) are
\in conv_hull(starting region).
If the original proposition preserving partition
is not convex, then safety cannot be guaranteed.
@param x0: initial continuous state
@type x0: numpy 1darray
@param ssys: system dynamics
@type ssys: L{LtiSysDyn}
@param abstraction: abstract system dynamics
@type abstraction: L{AbstractPwa}
@param start: index of the initial state in C{abstraction.ts}
@type start: int >= 0
@param end: index of the end state in C{abstraction.ts}
@type end: int >= 0
@param R: state cost matrix for::
x = [x(1)' x(2)' .. x(N)']'
If empty, zero matrix is used.
@type R: size (N*xdim x N*xdim)
@param r: cost vector for state trajectory:
x = [x(1)' x(2)' .. x(N)']'
@type r: size (N*xdim x 1)
@param Q: input cost matrix for control input::
u = [u(0)' u(1)' .. u(N-1)']'
If empty, identity matrix is used.
@type Q: size (N*udim x N*udim)
@param mid_weight: cost weight for |x(N)-xc|_2
@param test_result: performs a simulation
(without disturbance) to make sure that
the calculated input sequence is safe.
@type test_result: bool
@return: array A where row k contains the
control input: u(k)
for k = 0,1 ... N-1
@rtype: (N x m) numpy 2darray
"""
#@param N: horizon length
#@type N: int >= 1
#@param conservative:
# if True,
# then force plant to stay inside initial
# state during execution.
#
# Otherwise, plant is forced to stay inside
# the original proposition preserving cell.
#@type conservative: bool
#@param closed_loop: should be True
# if closed loop discretization has been used.
#@type closed_loop: bool
part = abstraction.ppp
regions = part.regions
ofts = abstraction.ts
original_regions = abstraction.orig_ppp
orig = abstraction._ppp2orig
params = abstraction.disc_params
N = params['N']
conservative = params['conservative']
closed_loop = params['closed_loop']
if (len(R) == 0) and (len(Q) == 0) and \
(len(r) == 0) and (mid_weight == 0):
# Default behavior
Q = np.eye(N*ssys.B.shape[1])
R = np.zeros([N*x0.size, N*x0.size])
r = np.zeros([N*x0.size,1])
mid_weight = 3
if len(R) == 0:
R = np.zeros([N*x0.size, N*x0.size])
if len(Q) == 0:
Q = np.eye(N*ssys.B.shape[1])
if len(r) == 0:
r = np.zeros([N*x0.size,1])
if (R.shape[0] != R.shape[1]) or (R.shape[0] != N*x0.size):
raise Exception("get_input: "
"R must be square and have side N * dim(state space)")
if (Q.shape[0] != Q.shape[1]) or (Q.shape[0] != N*ssys.B.shape[1]):
raise Exception("get_input: "
"Q must be square and have side N * dim(input space)")
if ofts is not None:
start_state = start
end_state = end
if end_state not in ofts.states.post(start_state):
raise Exception('get_input: '
'no transition from state s' +str(start) +
' to state s' +str(end)
)
else:
print("get_input: "
"Warning, no transition matrix found, assuming feasible")
if (not conservative) & (orig is None):
print("List of original proposition preserving "
"partitions not given, reverting to conservative mode")
conservative = True
P_start = regions[start]
P_end = regions[end]
n = ssys.A.shape[1]
m = ssys.B.shape[1]
idx = range((N-1)*n, N*n)
if conservative:
# Take convex hull or P_start as constraint
if len(P_start) > 0:
if len(P_start) > 1:
# Take convex hull
vert = pc.extreme(P_start[0])
for i in range(1, len(P_start)):
vert = np.vstack([
vert,
pc.extreme(P_start[i])
])
P1 = pc.qhull(vert)
else:
P1 = P_start[0]
else:
P1 = P_start
else:
# Take original proposition preserving cell as constraint
P1 = original_regions[orig[start]]
if len(P_end) > 0:
low_cost = np.inf
low_u = np.zeros([N,m])
# for each polytope in target region
for P3 in P_end:
if mid_weight > 0:
rc, xc = pc.cheby_ball(P3)
R[
np.ix_(
range(n*(N-1), n*N),
range(n*(N-1), n*N)
)
] += mid_weight*np.eye(n)
r[idx, :] += -mid_weight*xc
try:
u, cost = get_input_helper(
x0, ssys, P1, P3, N, R, r, Q,
closed_loop=closed_loop
)
r[idx, :] += mid_weight*xc
except:
r[idx, :] += mid_weight*xc
continue
if cost < low_cost:
low_u = u
low_cost = cost
if low_cost == np.inf:
raise Exception("get_input: Did not find any trajectory")
else:
P3 = P_end
if mid_weight > 0:
rc, xc = pc.cheby_ball(P3)
R[
np.ix_(
range(n*(N-1), n*N),
range(n*(N-1), n*N)
)
] += mid_weight*np.eye(n)
r[idx, :] += -mid_weight*xc
low_u, cost = get_input_helper(
x0, ssys, P1, P3, N, R, r, Q,
closed_loop=closed_loop
)
if test_result:
good = is_seq_inside(x0, low_u, ssys, P1, P3)
if not good:
print("Calculated sequence not good")
return low_u
"""
from __future__ import print_function
import sys
import numpy as np
import matplotlib.pyplot as plt
import polytope
if __name__ == "__main__":
if len(sys.argv) < 2:
N = 3
else:
N = int(sys.argv[1])
V = np.random.rand(N, 2)
print("Sampled "+str(N)+" points:")
print(V)
P = polytope.qhull(V)
print("Computed the convex hull:")
print(P)
V_min = polytope.extreme(P)
print("which has extreme points:")
print(V_min)
P.plot()
plt.show()
def control_sequence(Tp, U_A, U_b, D_A, D_B, D_b, run_Tp):
'''
find sequence of controls to be applied at vertices of polytopes from run
ctrl will be a 1x2 cell;
element ctrl{1}{i} refers to the i-th state from run{1} (prefix); it is a matrix v x m (v-number of vertices of current
polytope, m-number of controls)
the order of lines (giving controls for each vertex) corresponds to the order from Tp.Vert{run{1}(i)} (vertices of current state)
ctrl{2} refers to the suffix of run, which must be repeated infinitely
this structure for control is because there may exist states with different controls at different times, depending on
their position in run
the non-emptiness of run must be tested before running this function
Speed is a structure as control, containing the resulted speeds (drift+control) at vertices of polytopes from run (Speed{1}{i}(j,:)
is a row vector with speed at vertex j)
structure Speed will be useful in plotting speeds at vertices (2D and 3D cases, function plot_vertex_speeds) and can be
used in simulation (if we want to avoid computing control at each point)
'''
ctrl_1 = [] # ctrl cell 1
ctrl_2 = [] # ctrl cell 2
Speed_1 = [] # Speed cell 1
Speed_2 = [] # Speed cell 1
n = np.shape(D_A)[0]
prec = pow(10, 5) * np.finfo(float).eps
compl_run = [
int(item) for cell in run_Tp for subcell in cell for item in subcell
] # merge subcell of cell prefix+suffix of that cell
for i in range(len(compl_run) - 1):
s_i = compl_run[i]
s_j = compl_run[i + 1]
V = Tp.get("Tp.vert")[s_i]
H = pc.qhull(V)
v_no = np.shape(V)[0]
f_no = np.shape(H.A)[0]
F_v = np.matmul(H.A, V.T) - np.tile(tranfosefor1dvector(H.b, 1),
(1, v_no))
F_v = check(np.abs(F_v), prec)
centr = np.mean(V, axis=0)
F_n = np.zeros((f_no, n))
for k in range(f_no):
for counter, x in enumerate(list(F_v[k, :]), 0):
if x == 1:
index = counter
break
vect = V[index, :] - centr
vect_transpose = np.reshape(vect, (np.shape(vect)[0], 1))
if np.shape(np.sign(np.matmul(H.A[k, :], vect_transpose)))[0] == 1:
tmp = np.asscalar(np.sign(np.matmul(H.A[k, :],
vect_transpose)))
F_n[k, :] = (tmp * H.A[k, :]) / np.linalg.norm(H.A[k, :])
else:
F_n[k, :] = (np.matmul(
np.sign(np.matmul(H.A[k, :], vect_transpose)),
H.A[k, :])) / np.linalg.norm(H.A[k, :])
controls = np.zeros((v_no, np.shape(D_B)[1]))
speeds = np.zeros((v_no, n))
if s_i != s_j:
# for j in neigh:
tmp = np.matmul(
H.A, np.transpose(np.mean(Tp.get("Tp.vert")[s_j], axis=0)))
counter = 0
for x, y in zip(tmp, H.b):
if x > y:
break
counter = counter + 1
ex_f = counter
for l in range(v_no):
index_for_in_f = []
for counter, x in enumerate(F_v[:, l], 0):
if x != 0:
index_for_in_f.append(counter)
# list_A_for_in_f = index_for_in_f
in_f = set(list(index_for_in_f)).difference({ex_f})
in_f = list(in_f)
A_Check = np.vstack((U_A, np.matmul(-1 * F_n[ex_f, :], D_B)))
if (len(in_f) == 1):
A_Check = np.vstack(
(A_Check, np.matmul(F_n[in_f[0], :], D_B)))
else:
A_Check = np.vstack(
(A_Check, np.matmul(F_n[in_f[0]:in_f[1], :], D_B)))
abc = np.matmul(D_A, tranfosefor1dvector(V[l, :], 1)) + D_b
B_Check = np.vstack(
(-1 * U_b, (np.matmul(F_n[ex_f, :], abc) - prec)))
abc = np.matmul(D_A, tranfosefor1dvector(V[l, :], 1)) + D_b
if len(in_f) == 1:
B_Check = np.vstack(
(B_Check, np.matmul(-1 * F_n[in_f[0], :], abc) - prec))
else:
B_Check = np.vstack(
(B_Check,
np.matmul(-1 * F_n[in_f[0]:in_f[1], :], abc) - prec))
sol = opt.linprog(np.matmul(-1 * F_n[ex_f, :], D_B),
A_Check,
B_Check,
None,
None,
bounds=(None, None))
x = sol.__getitem__('x') # current solution vector
fun = sol.__getitem__(
'fun') # current value of the object function
success = sol.__getitem__(
'success') # flag for optimization success or failure
controls[l, :] = tranfosefor1dvector(x, 2)
# speeds[l, :] = np.transpose(np.matmul(D_A, np.transpose(V[l, :]))
# + np.matmul(D_B, tranfosefor1dvector(x, 1)) + D_b)
speeds[l, :] = np.matmul(D_A, np.transpose(
V[l, :])) + np.ravel(
np.matmul(D_B, tranfosefor1dvector(x,
1))) + np.ravel(D_b)
if (sol.__getattr__("success") == False):
print(
"Bad Optimization for transition between states " +
str(s_i) + " and ",
str(s_j) + ".")
else:
for l in range(v_no):
# index_for_in_f = []
in_f = []
for counter, x in enumerate(F_v[:, l], 0):
if x != 0:
in_f.append(counter)
if len(in_f) == 1:
tmp = np.matmul(F_n[in_f[0], :], D_B)
else:
tmp = np.matmul(F_n[in_f[0]:in_f[1], :], D_B)
A_Check = np.vstack((U_A, tmp))
tmp = np.matmul(D_A, tranfosefor1dvector(V[l, :], 1)) + D_b
if len(in_f) == 1:
B_Check = np.vstack(
(-1 * U_b, np.matmul(-1 * F_n[in_f[0], :], tmp)))
else:
B_Check = np.vstack(
(-1 * U_b, np.matmul(-1 * F_n[in_f[0]:in_f[1], :],
tmp)))
sol = opt.linprog(V[l, :] - centr,
A_Check,
B_Check,
None,
None,
bounds=(None, None))
x = sol.__getitem__('x') # current colution vector
fun = sol.__getitem__(
'fun') # current value of the object function
success = sol.__getitem__(
'success') # flag for optimization success or failure
controls[l, :] = tranfosefor1dvector(x, 2)
speeds[l, :] = np.matmul(D_A, np.transpose(
V[l, :])) + np.ravel(
np.matmul(D_B, tranfosefor1dvector(x,
1))) + np.ravel(D_b)
if not sol.__getattr__("success"):
print("Bad Optimization for self-loop in state " +
str(s_i))
if i < len(run_Tp[0][0]):
ctrl_1.append(controls)
Speed_1.append(speeds)
else:
ctrl_2.append(controls)
Speed_2.append(speeds)
ctrl_2.append(ctrl_1[-1]) # insert at the last element
Speed_2.append(Speed_1[-1]) # insert at the last element
ctrl = [ctrl_1] + [ctrl_2]
Speed = [Speed_1] + [Speed_2]
return ctrl, Speed
(http://matplotlib.org), which is an optional dependency.
"""
import polytope
import numpy as np
import matplotlib.pyplot as plt
import sys
if __name__ == "__main__":
if len(sys.argv) < 2:
N = 3
else:
N = int(sys.argv[1])
V = np.random.rand(N, 2)
print("Sampled " + str(N) + " points:")
print(V)
P = polytope.qhull(V)
print("Computed the convex hull:")
print(P)
V_min = polytope.extreme(P)
print("which has extreme points:")
print(V_min)
P.plot()
plt.show()
def PlotTraSys(self, nargmin, accepted_Q0):
"""
Plot sub polytopes of the transition sysytem.
:param nargmin:
:param accepted_Q0:
:return:
"""
# create two different names for initial TS and for TS with valid set of initial states for a given
# specification
if nargmin:
# clear the previous frame
shutil.rmtree('./frames/')
os.makedirs('frames')
plot_name = 'ts_init'
else:
plot_name = 'ts'
Tp = self.Tp
A = self.A[-1]
b = self.b[-1]
Tp_Q = Tp.get("Tp.Q")
Tp_vert = Tp.get("Tp.vert")
updated_Tp_adj = Tp.get("Tp.adj")
print("Plotting transition system on polytopes")
nargmin = nargmin
if nargmin:
# accepted_Q = initStates.getAccept_Q0()
accepted_Q0 = accepted_Q0
if np.shape(A)[0] == np.shape(b)[0]:
pass
else:
print(
"Ensure the number of row of A and b matrix corresponding to the state space boundaries are same"
)
p = pc.Polytope(A, b)
bound = pc.extreme(p)
if isinstance(bound, type(None)):
p = pc.Polytope(A, -1 * b)
bound = pc.extreme(p)
print("Tried with reversing the signs of b matrix")
if isinstance(bound, type(None)):
print(
"PLease make sure the state space boundaries form a convex polytope"
)
sys.exit(1)
n = np.shape(A)[1] # space dimension
if n == 2:
ep = (np.amax(bound, axis=0) - np.amin(bound, axis=0)) / 20
xmin = np.amin(bound[:, 0]) - ep[0]
xmax = np.amax(bound[:, 0]) + ep[0]
ymin = np.amin(bound[:, 1]) - ep[1]
ymax = np.amax(bound[:, 1]) + ep[1]
fig = plt.figure()
time_step = 0
ax = fig.add_subplot(111)
ax.add_patch(
self._get_patch(p,
edgecolor="Black",
linewidth=0.5,
facecolor=None,
fill=False))
plt.pause(0.1)
plt.savefig(f"frames/{plot_name+str(time_step)}.png", dpi=200)
centr = np.zeros((len(Tp_Q), n))
for i in range(len(Tp_Q)):
time_step += 1
k = pc.qhull(Tp_vert[i])
tmp = self._get_patch(k,
edgecolor="Black",
linewidth=0.15,
facecolor=None,
fill=False)
ax.add_patch(tmp)
plt.xlim(xmin, xmax)
plt.ylim(ymin, ymax)
plt.pause(0.1)
plt.savefig(f"frames/grid_{time_step}.png", dpi=200)
centr[i, :] = np.mean(Tp_vert[i],
axis=0) # taking mean along the columns
if nargmin:
if self.ismember(i, accepted_Q0):
k = pc.qhull(Tp_vert[i])
tmp = self._get_patch(k,
edgecolor="Black",
linewidth=0.15,
facecolor="Green",
fill=True)
ax.add_patch(tmp)
else:
k = pc.qhull(Tp_vert[i])
tmp = self._get_patch(k,
edgecolor="Black",
linewidth=0.15,
facecolor="Blue",
fill=True)
ax.add_patch(tmp)
if not nargmin:
for i in range(len(Tp_Q)):
time_step += 1
j = [0 for k in range(len(Tp_Q))]
neigh = []
tmp_neigh = np.nonzero(updated_Tp_adj[i, :])
neig_w_smaller_index = np.where((tmp_neigh[0] < i))
neig_w_smaller_index = [
int(x) for x in neig_w_smaller_index[0]
]
for element in neig_w_smaller_index:
neigh.append(tmp_neigh[0][element])
neigh_w_larger_index = np.where(tmp_neigh[0] > i)
neigh_w_larger_index = [
int(x) for x in neigh_w_larger_index[0]
]
for element in neigh_w_larger_index:
neigh.append(tmp_neigh[0][element])
for index in neig_w_smaller_index:
j[index] = 1
for index in neigh_w_larger_index:
j[index] = 1
for counter, index_of_j in enumerate(j):
if counter < i:
if updated_Tp_adj[i, counter] != 0:
plt.plot([centr[i, 0], centr[counter, 0]],
[centr[i, 1], centr[counter, 1]],
linestyle='--',
color='r')
plt.plot(centr[i, 0], centr[counter, 0],
centr[i, 0], centr[i, 1], 'ro')
plt.pause(0.1)
# else:
# plt.plot(centr[i, 0], centr[i, 1], centr[counter, 0], centr[counter, 1], 'g.')
# plt.pause(0.1)
elif counter > i:
if updated_Tp_adj[i, counter] != 0:
plt.plot([centr[i, 0], centr[counter, 0]],
[centr[i, 1], centr[counter, 1]],
linestyle='-',
color='r')
plt.plot(centr[i, 0], centr[counter, 0],
centr[i, 0], centr[i, 1], 'ro')
plt.pause(0.1)
# else:
# plt.plot(centr[i, 0], centr[i, 1], centr[counter, 0], centr[counter, 1], 'g.')
# plt.pause(0.1)
if updated_Tp_adj[i, i] != 0:
plt.plot(centr[i, 0], centr[i, 1], 'r*')
plt.pause(0.1)
plt.savefig(f"frames/{plot_name+str(time_step)}.png",
dpi=200)
for i in range(len(Tp_Q)):
plt.text(centr[i, 0],
centr[i, 1],
"q_" + str(i),
fontsize=8,
horizontalalignment='center',
verticalalignment='center')
time_step += 1
plt.savefig(f"frames/{plot_name + str(time_step)}.png", dpi=200)
elif n == 3:
pass
else:
print("Cannot display more than 3 dimension transition system")
# only make a gif when we plotting the set of valid initial states
if nargmin:
self.make_gif('./frames/', f'./gifs/ts_init.gif')
return ax
def invalidtransitions(self):
Tp = self.Tp
U_A = self.U_A
U_b = self.U_b
D_A = self.D_A
D_B = self.D_B
D_b = self.D_b
A = self.A
n = D_A.shape[0]
m = U_A.shape[1]
prec = pow(10, 5) * np.finfo(float).eps
updated_Tp_adj = Tp.get("Tp.adj")
Tp_Q = Tp.get("Tp.Q")
Tp_vert = Tp.get("Tp.vert")
rank_method = False # this is faster but more restrictive method that eliminates lots of possible transitions
linprog_method = True # this method is slightly slower but has more spurious transitions
# a mix and match also gives a mixed result but seems to be the closest one.
for i in range(0, len(Tp_Q)):
V = Tp_vert[i]
H = pc.qhull(V)
# storing this in its transpose form. check which H.b (original is (3,)) you need in all future references
H.b = np.reshape(H.b, (np.shape(H.A)[0], 1))
v_no = np.shape(V)[0]
f_no = np.shape(H.A)[0]
# f_no = v_no #need to find alternative for this jugaad
F_v = np.matmul(H.A, V.T) - np.tile(H.b, (1, v_no))
def check(array, val):
for x in range(np.shape(array)[0]):
for y in range(np.shape(array)[1]):
if array[x][y] <= val:
array[x][y] = 1
else:
array[x][y] = 0
return array
def tranfosefor1dvector(array, val):
# val = 1 for vertical vector n x 1 matrix and 2 for 1 x n matrix
if val == 1:
ret = np.reshape(array, (np.shape(array)[0], 1))
elif val == 2:
ret = np.reshape(array, (1, np.shape(array)[0]))
return ret
F_v = check(np.abs(F_v), prec)
centr = np.mean(V, axis=0)
F_n = np.zeros((f_no, n))
for k in range(f_no):
for counter, x in enumerate(list(F_v[k, :]), 0):
if x == 1:
index = counter
break
vect = V[index, :] - centr
vect_transpose = np.reshape(vect, (np.shape(vect)[0], 1))
if np.shape(np.sign(np.matmul(H.A[k, :],
vect_transpose)))[0] == 1:
tmp = np.asscalar(
np.sign(np.matmul(H.A[k, :], vect_transpose)))
F_n[k, :] = (tmp * H.A[k, :]) / np.linalg.norm(H.A[k, :])
else:
F_n[k, :] = (np.matmul(np.sign(np.matmul(H.A[k, :], vect_transpose)), H.A[k, :]))\
/ np.linalg.norm(H.A[k, :])
# test f exits to neighbours are feasible (due to control restrictions and drift)
index_for_neigh = []
for counter, x in enumerate(updated_Tp_adj[i, :], 0):
if x != 0:
index_for_neigh.append(counter)
neigh = set(list(index_for_neigh)).difference({i})
# for each neighbour find if transition is possible
# first find the common fact of polytope si and sj (which is also the exit facet for si)
for j in neigh:
tmp = np.matmul(H.A, np.transpose(np.mean(Tp_vert[j], axis=0)))
counter = 0
# tmp_T = np.reshape(tmp, (np.shape(tmp)[0], 1))
for x, y in zip(tmp, H.b):
if x > y:
break
counter = counter + 1
ex_f = counter
# for all vertices of si check feasibility by checking non-emptiness of optimisation set
# (imposed by restrictions)
for l in range(v_no):
index_for_in_f = []
for counter, x in enumerate(F_v[:, l], 0):
if x != 0:
index_for_in_f.append(counter)
# list_A_for_in_f = index_for_in_f
in_f = set(list(index_for_in_f)).difference({ex_f})
in_f = list(in_f)
if rank_method:
H_rep_A = np.vstack(
(U_A, np.matmul(-1 * F_n[ex_f, :], D_B)))
if len(in_f) == 1:
H_rep_A = np.vstack(
(H_rep_A, np.matmul(F_n[in_f[0], :], D_B)))
else:
H_rep_A = np.vstack(
(H_rep_A,
np.matmul(F_n[in_f[0]:in_f[1], :], D_B)))
abc = np.matmul(D_A, tranfosefor1dvector(V[l, :],
1)) + D_b
H_rep_B = np.vstack(
(-1 * U_b, (np.matmul(F_n[ex_f, :], abc) - prec)))
abc = np.matmul(D_A, tranfosefor1dvector(V[l, :],
1)) + D_b
if len(in_f) == 1:
H_rep_B = np.vstack(
(H_rep_B,
np.matmul(-1 * F_n[in_f[0], :], abc) - prec))
else:
H_rep_B = np.vstack(
(H_rep_B,
np.matmul(-1 * F_n[in_f[0]:in_f[1], :], abc) -
prec))
p = pc.Polytope(H_rep_A, H_rep_B)
try:
V_rep = pc.extreme(p)
except:
#if error from v-rep of polytope then disable this transition
updated_Tp_adj[i, j] = 0
# abc = np.ones((np.shape(V_rep)[0],1))
# if(np.linalg.matrix_rank(np.vstack((V_rep, abc)))) != (m+1):
# the above two lines a re a better alternative than this
if isinstance(V_rep, type(None)):
updated_Tp_adj[i, j] = 0
break
if linprog_method:
if len(in_f) == 1:
abcd1 = np.vstack(
(np.matmul(-1 * F_n[ex_f, :], D_B),
np.matmul(F_n[in_f[0], :], D_B)))
else:
abcd1 = np.vstack(
(np.matmul(-1 * F_n[ex_f, :], D_B),
np.matmul(F_n[in_f[0]:in_f[1], :], D_B)))
A_check = np.vstack((U_A, abcd1))
tmp = np.matmul(D_A, tranfosefor1dvector(V[l, :],
1)) + D_b
# tmp1 = np.matmul(-1*F_n[in_f,:],tmp) + ReadData.D_b
if len(in_f) == 1:
last_stack = np.matmul(-1 * F_n[in_f[0], :],
tmp) - prec
else:
last_stack = np.matmul(
-1 * F_n[in_f[0]:in_f[1], :], tmp) - prec
tmp = np.matmul(D_A, tranfosefor1dvector(V[l, :],
1)) + D_b
second_last_stack = np.matmul(F_n[ex_f, :], tmp) - prec
abcd2 = np.vstack((second_last_stack, last_stack))
B_check = np.vstack((-1 * U_b, abcd2))
sol = opt.linprog(np.matmul(-1 * F_n[ex_f, :], D_B),
A_check,
B_check.flatten(),
None,
None,
bounds=(None, None))
if not sol.__getattr__("success"):
updated_Tp_adj[i, j] = 0
break
for m in range(0, v_no):
in_f = np.nonzero(F_v[:, m])
if rank_method:
if len(in_f) == 1:
H_rep_A = np.vstack(
(U_A, np.matmul(F_n[in_f[0], :], D_B)))
tmp = np.matmul(D_A, tranfosefor1dvector(V[m, :],
1)) + D_b
H_rep_B = np.vstack(
(-1 * U_b, np.matmul(-1 * F_n[in_f[0], :], tmp)))
else:
H_rep_A = np.vstack(
(U_A, np.matmul(F_n[in_f[0]:in_f[1], :], D_B)))
tmp = np.matmul(D_A, tranfosefor1dvector(V[m, :],
1)) + D_b
H_rep_B = np.vstack(
(-1 * U_b,
np.matmul(-1 * F_n[in_f[0]:in_f[1], :], tmp)))
p = pc.Polytope(H_rep_A, H_rep_B)
try:
V_rep = pc.extreme(p)
except:
updated_Tp_adj[i, i] = 0
if isinstance(V_rep, type(None)):
# trans_sys.Tp_adj[i,i] = 0
updated_Tp_adj[i, i] = 0
if linprog_method:
if len(in_f) == 1:
tmp = np.matmul(F_n[in_f[0], :], D_B)
else:
tmp = np.matmul(F_n[in_f[0]:in_f[1], :], D_B)
A_check = np.vstack((U_A, tmp))
tmp = np.matmul(D_A, tranfosefor1dvector(V[m, :], 1)) + D_b
if len(in_f) == 1:
B_check = np.vstack(
(-1 * U_b, np.matmul(-1 * F_n[in_f[0], :], tmp)))
else:
B_check = np.vstack(
(-1 * U_b,
np.matmul(-1 * F_n[in_f[0]:in_f[1], :], tmp)))
sol = opt.linprog(V[m, :] - centr,
A_check,
B_check.flatten(),
None,
None,
bounds=(None, None))
if not sol.__getattr__("success"):
updated_Tp_adj[i, i] = 0
break
Tp_new = {"Tp.adj": updated_Tp_adj}
Tp.update(Tp_new)
return Tp
