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