def test_plot():
A_desired = np.vstack((np.eye(3, 3), -1 * np.eye(3, 3)))
B_desired = np.ones([6, 1])
print(A_desired)
print(B_desired)
desired_twist = polytope.Polytope(A_desired, B_desired)
print(desired_twist.volume)
V = polytope.extreme(desired_twist)
print("polytope vertices=", polytope.extreme(desired_twist))
print("desired_twist vertices=", V)
polytope_functions.plot_polytope_3d(desired_twist)
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 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 is_feasible_alternative(
from_region, to_region, sys, N,
):
"""Return True if to_region is reachable from_region.
An alternative implementation of feasibility of transitions via an
open loop policy.
Supposed to be faster as it does not require set difference
Conservative for non-convex regions (might say infeasible when feasible)
"""
# solve a set of LP feasibility problems for each corner
for f1 in from_region: # from all
count = 0
for f2 in to_region: # to some
for vert in pc.extreme(f1):
bad_vert = False
u = exists_input(vert, sys, f1, f2, N)
if u is None:
bad_vert = True # not possible to reach f2
break
if bad_vert == False: # possible to reach to_region from f1
count = count+1
break
if count == len(from_region):
res = True
else:
res = False
return res
def plot_regions(regions, xlim, ylim, tikz=False):
"""
:param regions: dictionary with name: polytope
:param xlim: x axis limits
:param ylim: y axis limits
:param tikz: generate tikz tex code
:return: figure
"""
fig = plt.figure()
ax = fig.add_subplot(111)
for name in regions.keys():
patch = matplotlib.patches.Polygon(pc.extreme(regions[name]))
lx, ux = pc.bounding_box(regions[name]) # lower and upperbounds over all dimensions
#patch = patch_ellips(Meps, pos=None, number=number)
ax.add_patch(patch)
plt.text(ux[0], ux[1], name)
#return
#ax.add_patch(patch)
plt.xlim(xlim)
plt.ylim(ylim)
if tikz:
from matplotlib2tikz import save as tikz_save
tikz_save('regions.tex', figureheight='\\figureheight', figurewidth='\\figurewidth')
# plt.tight_layout()
plt.show()
def precursor(Sset, A, Uset=pt.Polytope(), B=np.array([])):
""" The Pre(S) is the set of states which evolve into the target set S in one time step. """
if not B.any():
return pt.Polytope(Sset.A @ A, Sset.b) # (HA, b)
else:
tmp = minkowski_sum( Sset, pt.extreme(Uset) @ -B.T )
return pt.Polytope(tmp.A @ A, tmp.b)
def precursor(Sset,A,Uset = pt.Polytope(),B = np.array([])):
# see definition of Pre(S) in slides
if not B.any(): # if B is nothing
return pt.Polytope(Sset.A @ A ,Sset.b)
else:
tmp = minkowski_sum(Sset,pt.extreme(Uset) @ -B.T)
return pt.Polytope(tmp.A @ A, tmp.b)
def check_crash(self): # Ture: cart hasn't crash into the obstacles.
for o in self.obstacles:
#check whether the corners of the cart are in the obstacle
A = o.A
b = o.b
for point in pt.extreme(self.cart_poly):
if (np.all(A @ point - b <= 0)):
return False
#check corners of the obstacle is in the car
A = self.cart_poly.A
b = self.cart_poly.b
for point in pt.extreme(o):
if (np.all(A @ point - b <= 0)):
return False
return True
def check_goal(self):
A = self.goal.A
b = self.goal.b
for point in pt.extreme(self.cart_poly):
#if there is at least one extreme of the cart is out of the goal area, return false.
if (not (np.all(A @ point - b <= 0))):
return False
return True
def plot_facets_2d(facet_list, alpha=1.0,
xylim=5, ax=None, linestyle='solid', linewidth=3, color='black'):
"""Plots a list of facets which exist as line segments in R^2
"""
if ax is None:
ax = plt.axes()
if(np.size(xylim)==1):
xlim = [0, xylim]
ylim = [0, xylim]
else:
xlim = xylim
ylim = xylim
for facet in facet_list:
P = Polytope_2(facet.ub_A, facet.ub_b)
vertices = ptope.extreme(P)
facet_vertices = []
if vertices is not None and np.shape(vertices)[0] > 1:
for vertex in vertices:
equal = fuzzy_equal(np.dot(facet.a_eq[0], vertex), facet.b_eq[0])
if equal:
facet_vertices.append(vertex)
x1 = facet_vertices[0][0]; x2 = facet_vertices[1][0]
y1 = facet_vertices[0][1]; y2 = facet_vertices[1][1]
x = [x1, x2]; y = [y1, y2]
ax.plot(x, y, c=color, linestyle=linestyle, linewidth=linewidth)
else:
new_ub_A = np.vstack((facet.ub_A, [[1, 0], [-1, 0], [0, 1], [0, -1]]))
new_ub_b = np.hstack((facet.ub_b, [xlim[1], -1 * xlim[0], ylim[1], -1 * ylim[0]]))
P2 = Polytope_2(new_ub_A, new_ub_b)
V2 = ptope.extreme(P2)
if V2 is not None:
P2.plot(ax, color=color, alpha=alpha, linestyle=linestyle, linewidth=linewidth)
print('an unbounded facet was plotted imperfectly')
else:
print('facet not plotted')
plt.xlim(xlim[0], xlim[1])
plt.ylim(ylim[0], ylim[1])
def tension_space_polytope(t_min, t_max):
"""
:param t_min: minimum allowable tension value
:param t_max: maxmimum allowable tension value
:return: tension space polytope
"""
A_desired = np.vstack((np.eye(4, 4), -1 * np.eye(4, 4)))
B_desired = np.vstack((t_max * np.ones([4, 1]), -t_min * np.ones([4, 1])))
tension_space_Hrep = polytope.Polytope(A_desired, B_desired)
tension_space_Vrep = polytope.extreme(tension_space_Hrep)
return tension_space_Vrep
def __le__(self, other):
"""Return True if self is enclosed by other."""
XY = pc.extreme(
self.get_confidence_sets(SCALING_FOR_INCLUSION)[0].to_poly())
A, b = other.get_confidence_sets(SCALING_FOR_INCLUSION)[0].to_H()
n_points = XY.shape[0]
ineq = A.dot(XY.T)
for i in range(n_points):
if not np.all(ineq[:, i] < b):
return False
return True
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 tension_space_polytope(t_min_, t_max_, n):
"""
:param n: number of cables
:param t_min_: minimum allowable tension value
:param t_max_: maxmimum allowable tension value
:return: tension space polytope
"""
A_desired = np.vstack((np.eye(n, n), -1 * np.eye(n, n)))
B_desired = np.vstack(
(t_max_ * np.ones([n, 1]), -t_min_ * np.ones([n, 1])))
tension_space_Hrep = polytope.Polytope(A_desired, B_desired)
tension_space_Vrep = polytope.extreme(tension_space_Hrep)
return tension_space_Vrep
def plot_polytopes_2d(poly_list, colors=None, alpha=1.0,
xylim=5, ax=None, linestyle='dashed', linewidth=0):
"""Plots a list of polytopes which exist in R^2.
"""
if ax is None:
ax = plt.axes()
if colors == None:
colors = [np.random.rand(3) for _ in range(0, len(poly_list))]
if(np.size(xylim)==1):
xlim = [0, xylim]
ylim = [0, xylim]
else:
xlim = xylim
ylim = xylim
for poly, color in zip(poly_list, colors):
P = Polytope_2(poly.ub_A, poly.ub_b)
V = ptope.extreme(P)
if V is not None:
P.plot(ax, color=color, alpha=alpha, linestyle=linestyle, linewidth=linewidth)
else:
# Polytope may be unbounded, thus add additional constraints x in [-xylim, xylim]
# and y in [-xylim, xylim]
new_ub_A = np.vstack((poly.ub_A, [[1,0],[-1,0],[0,1],[0,-1]]))
new_ub_b = np.hstack((poly.ub_b, [xlim[1], -1*xlim[0], ylim[1], -1*ylim[0]]))
P2 = Polytope_2(new_ub_A, new_ub_b)
V2 = ptope.extreme(P2)
if V2 is not None:
P2.plot(ax, color=color, alpha=alpha, linestyle=linestyle, linewidth=linewidth)
print('an unbounded polytope was plotted imperfectly')
else:
print('polytope not plotted')
plt.xlim(xlim[0], xlim[1])
plt.ylim(ylim[0], ylim[1])
def plotdrift(self):
A = self.A_bound
b = self.b_bound
D_A = self.D_A
D_b = self.D_b
ax = self.init_fig
Bound = pc.Polytope(A, -1 * b)
Bound_V = pc.extreme(Bound)
n = np.shape(A)[1]
# ax = trans_plot.plt
p_no = 0.4
x_start = np.amin(Bound_V[:, 0], axis=0)
x_stop = np.amax(Bound_V[:, 0], axis=0)
y_start = np.amin(Bound_V[:, 1], axis=0)
y_stop = np.amax(Bound_V[:, 1], axis=0)
lin_x = np.arange(x_start, x_stop, p_no)
lin_y = np.arange(y_start, y_stop, p_no)
if n == 2:
X, Y = np.meshgrid(lin_x, lin_y)
Gx = D_A[0, 0] * X + D_A[0, 1] * Y + D_b[0, 0]
Gy = D_A[1, 0] * X + D_A[1, 1] * Y + D_b[1, 0]
ax.quiver(X, Y, Gx, Gy)
# ax.xaxis.set_ticks([])
# ax.yaxis.set_ticks([])
# ax.show()
plt.pause(0.01)
if n == 3:
z_start = np.amin(Bound_V[:, 2], axis=0)
z_stop = np.amax(Bound_V[:, 2], axis=0)
lin_z = np.array(z_start, z_stop, p_no)
X, Y, Z = np.meshgrid(lin_x, lin_y, lin_z)
Gx = D_A[0, 0] * X + D_A[0, 1] * Y + D_b[0, 0]
Gy = D_A[1, 0] * X + D_A[1, 1] * Y + D_b[1, 0]
Gz = D_A[2, 0] * X + D_A[2, 1] * Y + D_b[2, 0]
ax.quiver(X, Y, Z, Gx, Gy, Gz)
# ax.show()
plt.pause(0.01)
plt.savefig(f"frames/grid_{1000}.png", dpi=200)
self.make_gif('./frames/', f'./gifs/ts.gif')
return ax
def plot_region(ax,
poly,
name,
prob,
color='red',
alpha=0.5,
hatch=False,
fill=True):
ax.add_patch(
patches.Polygon(pc.extreme(poly),
color=color,
alpha=alpha,
hatch=hatch,
fill=fill))
_, xc = pc.cheby_ball(poly)
ax.text(xc[0] - 0.4, xc[1] - 0.43,
'${}_{}$\n$p={}$'.format(name[0].upper(), name[1], prob))
def simulate(randParkSignal, sys_dyn, ctrl, disc_dynamics, T):
# initialization:
# pick initial continuous state consistent with
# initial controller state (discrete)
u, v, edge_data = list(ctrl.edges('Sinit', data=True))[1]
s0_part = edge_data['loc']
init_poly_v = pc.extreme(disc_dynamics.ppp[s0_part][0])
x_init = sum(init_poly_v) / init_poly_v.shape[0]
x = [x_init[0]]
y = [x_init[1]]
N = disc_dynamics.disc_params['N']
s0_part = find_controller.find_discrete_state([x[0], y[0]],
disc_dynamics.ppp)
ctrl = synth.determinize_machine_init(ctrl, {'loc': s0_part})
(s, dum) = ctrl.reaction('Sinit', {'park': randParkSignal[0]})
print(dum)
for i in range(0, T):
(s, dum) = ctrl.reaction(s, {'park': randParkSignal[i]})
u = find_controller.get_input(x0=np.array([x[i * N], y[i * N]]),
ssys=sys_dyn,
abstraction=disc_dynamics,
start=s0_part,
end=disc_dynamics.ppp2ts.index(
dum['loc']),
ord=1,
mid_weight=5)
for ind in range(N):
s_now = np.dot(sys_dyn.A, [x[-1], y[-1]]) + np.dot(
sys_dyn.B, u[ind])
x.append(s_now[0])
y.append(s_now[1])
s0_part = find_controller.find_discrete_state([x[-1], y[-1]],
disc_dynamics.ppp)
s0_loc = disc_dynamics.ppp2ts[s0_part]
print(s0_loc)
print(dum['loc'])
print(dum)
show_traj = True
if show_traj:
assert plt, 'failed to import matplotlib'
plt.plot(x)
plt.plot(y)
plt.show()
def _get_patch(poly1, **kwargs):
"""Return matplotlib patch for given Polytope.
Example::
> # Plot Polytope objects poly1 and poly2 in the same plot
> import matplotlib.pyplot as plt
> fig = plt.figure()
> ax = fig.add_subplot(111)
> p1 = _get_patch(poly1, color="blue")
> p2 = _get_patch(poly2, color="yellow")
> ax.add_patch(p1)
> ax.add_patch(p2)
> ax.set_xlim(xl, xu) # Optional: set axis max/min
> ax.set_ylim(yl, yu)
> plt.show()
@type poly1: L{Polytope}
@param kwargs: any keyword arguments valid for
matplotlib.patches.Polygon
"""
import matplotlib as mpl
V = ptope.extreme(poly1)
if (V is not None):
rc, xc = ptope.cheby_ball(poly1)
x = V[:, 1] - xc[1]
y = V[:, 0] - xc[0]
mult = np.sqrt(x**2 + y**2)
x = x / mult
angle = np.arccos(x)
corr = np.ones(y.size) - 2 * (y < 0)
angle = angle * corr
ind = np.argsort(angle)
# create patch
patch = mpl.patches.Polygon(V[ind, :], True, **kwargs)
patch.set_zorder(0)
else:
patch = mpl.patches.Polygon([], True, **kwargs)
patch.set_zorder(0)
return patch
def plot_it(desired_twist, c, ax):
VV = polytope.extreme(desired_twist)
# ax = fig.gca(projection='3d')
#ax = fig.gca()
hull = ConvexHull(VV, qhull_options='Qs QJ')
ax.plot(hull.points[hull.vertices, 0],
hull.points[hull.vertices, 1],
hull.points[hull.vertices, 2],
'ko',
markersize=4)
s = ax.plot_trisurf(hull.points[:, 0],
hull.points[:, 1],
hull.points[:, 2],
triangles=hull.simplices,
color=c,
alpha=0.2,
edgecolor='k')
def get_inter_prob(self, X, scaling_factors):
"""
Compute intersection probability
In the notation of Matthias' thesis, the scaling_factors are:
gamma=m(0), m(1), ..., m(k-1)
"""
n = self.G.shape[0]
assert n == 2 # what follows is only valid in 2D
scaling_factors = np.array(scaling_factors)
k = scaling_factors.shape[0]
# calling pc.extreme on this one is fast, maybe because it is a plain reactangle?
X = Polygon(pc.extreme(X.to_poly()))
confid_sets = self.get_confidence_sets(scaling_factors)
# append m=0 to the set of scaling factors
scaling_factors = np.append(scaling_factors, 0.0)
h = ((2.0 * np.pi)**(-n / 2.0) * np.linalg.det(self.Sig)**(-0.5) *
np.exp(-0.5 * np.array(scaling_factors)**2.0))
# compute intersection volumes
V = np.zeros((k, ))
for i in range(k):
A, b = confid_sets[i].to_H()
vertices = compute_polytope_vertices(
A, b) # fast C code for vertex enumeration
vertices.sort(
key=lambda c: math.atan2(c[0], c[1])) # sort those vertices
X2 = Polygon(vertices) # construct a Polygon
V[i] = X2.intersection(
X).area # this is faster than intersect of polytope
# compute intersection prob
prob = 1 - erf(scaling_factors[0] / np.sqrt(2.0))**(2.0 * n)
prob += h[0] * V[0]
for i in range(k):
prob += (h[i + 1] - h[i]) * V[i]
return min(prob, 1.0)
def plot_polytope_3d(poly):
V = polytope.extreme(poly)
fig = plt.figure()
#ax = fig.gca(projection='3d')
ax = fig.gca()
hull = ConvexHull(V, qhull_options='Qs QJ')
ax.plot(hull.points[hull.vertices, 0],
hull.points[hull.vertices, 1],
hull.points[hull.vertices, 2],
'ko',
markersize=4)
s = ax.plot_trisurf(hull.points[:, 0],
hull.points[:, 1],
hull.points[:, 2],
triangles=hull.simplices,
color='red',
alpha=0.2,
edgecolor='k')
plt.show()
return ax
def get_max_extreme(G, D, N):
"""Calculate the array d_hat such that::
d_hat = max(G*DN_extreme),
where DN_extreme are the vertices of the set D^N.
This is used to describe the polytope::
L*x <= M - G*d_hat.
Calculating d_hat is equivalen to taking the intersection
of the polytopes::
L*x <= M - G*d_i
for every possible d_i in the set of extreme points to D^N.
@param G: The matrix to maximize with respect to
@param D: Polytope describing the disturbance set
@param N: Horizon length
@return: d_hat: Array describing the maximum possible
effect from the disturbance
"""
D_extreme = pc.extreme(D)
nv = D_extreme.shape[0]
dim = D_extreme.shape[1]
DN_extreme = np.zeros([dim * N, nv**N])
for i in xrange(nv**N):
# Last N digits are indices we want!
ind = np.base_repr(i, base=nv, padding=N)
for j in xrange(N):
DN_extreme[range(j * dim, (j + 1) * dim),
i] = D_extreme[int(ind[-j - 1]), :]
d_hat = np.amax(np.dot(G, DN_extreme), axis=1)
return d_hat.reshape(d_hat.size, 1)
def get_max_extreme(G,D,N):
"""Calculate the array d_hat such that::
d_hat = max(G*DN_extreme),
where DN_extreme are the vertices of the set D^N.
This is used to describe the polytope::
L*x <= M - G*d_hat.
Calculating d_hat is equivalen to taking the intersection
of the polytopes::
L*x <= M - G*d_i
for every possible d_i in the set of extreme points to D^N.
@param G: The matrix to maximize with respect to
@param D: Polytope describing the disturbance set
@param N: Horizon length
@return: d_hat: Array describing the maximum possible
effect from the disturbance
"""
D_extreme = pc.extreme(D)
nv = D_extreme.shape[0]
dim = D_extreme.shape[1]
DN_extreme = np.zeros([dim*N, nv**N])
for i in xrange(nv**N):
# Last N digits are indices we want!
ind = np.base_repr(i, base=nv, padding=N)
for j in xrange(N):
DN_extreme[range(j*dim,(j+1)*dim),i] = D_extreme[int(ind[-j-1]),:]
d_hat = np.amax(np.dot(G,DN_extreme), axis=1)
return d_hat.reshape(d_hat.size,1)
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)
(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()
# Simulation
print('\n Simulation starts \n')
T = 100
# let us pick an environment signal
randParkSignal = [random.randint(0, 1) for b in range(1, T + 1)]
# Set up parameters for get_input()
disc_dynamics.disc_params['conservative'] = True
disc_dynamics.disc_params['closed_loop'] = False
# initialization:
# pick initial continuous state consistent with
# initial controller state (discrete)
u, v, edge_data = list(ctrl.edges('Sinit', data=True))[1]
s0_part = edge_data['loc']
init_poly_v = pc.extreme(disc_dynamics.ppp[s0_part][0])
x_init = sum(init_poly_v) / init_poly_v.shape[0]
x = [x_init[0]]
y = [x_init[1]]
N = disc_dynamics.disc_params['N']
s0_part = find_controller.find_discrete_state(
[x[0], y[0]], disc_dynamics.ppp)
ctrl = synth.determinize_machine_init(ctrl, {'loc': s0_part})
(s, dum) = ctrl.reaction('Sinit', {'park': randParkSignal[0]})
print(dum)
for i in range(0, T):
(s, dum) = ctrl.reaction(s, {'park': randParkSignal[i]})
u = find_controller.get_input(
x0=np.array([x[i * N], y[i * N]]),
ssys=sys_dyn,
abstraction=disc_dynamics,
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)
for i in range(X.shape[0]):
for j in range(Y.shape[0]):
v_sum.append(X[i,:]+Y[j,:])
hull.points[:, 2],
triangles=hull.simplices,
color=c,
alpha=0.2,
edgecolor='k')
# Press the green button in the gutter to run the script.
if __name__ == '__main__':
A_desired = np.vstack((np.random.rand(6, 3), -1 * np.eye(6, 3)))
B_desired = np.random.rand(12, 1) * 2.0
desired_twist = polytope.Polytope(A_desired, B_desired)
V = polytope.extreme(desired_twist)
print("V", V)
A2_desired = np.vstack((np.eye(3, 3), -1 * np.eye(3, 3)))
B2_desired = np.ones([
6,
])
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plot_it(desired_twist, 'red', ax)
A2_desired = np.vstack((np.eye(3, 3), -1 * np.eye(3, 3)))
B2_desired = np.ones([6, 1]) * 5.0
B2_desired[1] = 0.001
B2_desired[4] = 0.001
desired_twist2 = polytope.Polytope(A2_desired, B2_desired)
"""
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 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))
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
assert ctrl is not None, 'unrealizable'
# Generate a graphical representation of the controller for viewing
if not ctrl.save('continuous.png'):
print(ctrl)
#
# Simulation
print('\n Simulation starts \n')
T = 100
# let us pick an environment signal
randParkSignal = [random.randint(0, 1) for b in range(1, T + 1)]
# initialization:
# pick initial continuous state consistent with
# initial controller state (discrete)
u, v, edge_data = ctrl.edges('Sinit', data=True)[1]
s0_part = edge_data['loc']
init_poly_v = pc.extreme(disc_dynamics.ppp[s0_part][0])
x_init = sum(init_poly_v) / init_poly_v.shape[0]
x = [x_init[0]]
y = [x_init[1]]
N = disc_dynamics.disc_params['N']
s0_part = find_controller.find_discrete_state(
[x[0], y[0]], disc_dynamics.ppp)
ctrl = synth.determinize_machine_init(ctrl, {'loc': s0_part})
(s, dum) = ctrl.reaction('Sinit', {'park': randParkSignal[0]})
print(dum)
for i in range(0, T):
(s, dum) = ctrl.reaction(s, {'park': randParkSignal[i]})
u = find_controller.get_input(
x0=np.array([x[i * N], y[i * N]]),
ssys=sys_dyn,
abstraction=disc_dynamics,
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
