def define_partition(dom):
p = dict()
p["a"] = pc.box2poly([[0.0, 10.0], [15.0, 18.0]])
p["b"] = pc.box2poly([[0.0, 1.0], [0.0, 20.0]])
ppp = abstract.prop2part(dom, p)
ppp, new2old_reg = abstract.part2convex(ppp)
return ppp
def test_plot_transition_arrow():
p0 = pc.box2poly([[0.0, 1.0], [0.0, 2.0]])
p1 = pc.box2poly([[0.1, 2.0], [0.0, 2.0]])
# matplotlib.patches is loaded also by matplotlib.pyplot
# and .figures, so instantiating real Axes w/o
# loading patches is impossible
ax = Axes()
arrow = plot.plot_transition_arrow(p0, p1, ax=ax)
assert(isinstance(arrow, matplotlib.patches.Arrow))
def subsys1():
dom = pc.box2poly([[0.0, 3.0], [0.0, 2.0]])
A = np.eye(2)
B = np.eye(2)
U = pc.box2poly([[0.0, 0.0], [-1.0, 0.0]])
U.scale(input_bound)
sys_dyn = hybrid.LtiSysDyn(A, B, Uset=U, domain=dom)
return sys_dyn
def define_dynamics(dom):
A = np.eye(2)
B = np.array([[1.0, -1.0], [0.0, +1.0]])
U = pc.box2poly([[0.0, 3.0], [-3.0, 3.0]])
E = np.array([[0.0], [-1.0]])
W = pc.box2poly([[-1.0, 1.0]])
W.scale(0.4)
K = np.array([[0.0], [-0.4]])
sys = hybrid.LtiSysDyn(A, B, E, K, U, W, dom)
return sys
def subsys1(h):
A = np.array([[0.9948, 0.], [0., 1.1052]])
B = np.array([[-1.1052, 0.], [0., 1.1052]])
E = np.array([[1, 0], [0, 1]])
U = box2poly([[-1., 1.], [-1., 1.]])
U.scale(input_bound)
W = box2poly([[-1., 1.], [-1., 1.]])
W.scale(uncertainty)
dom = box2poly([[0., 3.], [0., h]])
sys_dyn = hybrid.LtiSysDyn(A, B, E, None, U, W, dom)
return sys_dyn
def cont_predicates():
p = dict()
p["safe"] = pc.box2poly([[0.5, 3.5], [0.5, 2.5]])
ppp = abstract.prop2part(dom, p)
ppp, new2old_reg = abstract.part2convex(ppp)
ppp.plot()
return ppp
def subsys0():
A = np.array([[1.1052, 0.], [ 0., 1.1052]])
B = np.array([[1.1052, 0.], [ 0., 1.1052]])
E = np.array([[1,0], [0,1]])
U = box2poly([[-1., 1.], [-1., 1.]])
U.scale(input_bound)
W = box2poly([[-1., 1.], [-1., 1.]])
W.scale(uncertainty)
dom = box2poly([[0., 3.], [0.5, 2.]])
sys_dyn = LtiSysDyn(A, B, E, None, U, W, dom)
#sys_dyn.plot()
return sys_dyn
def switched_system_test():
subsystems = []
# subsystem 0
A = np.eye(2)
B = np.eye(2)
Uset = pc.box2poly([[0.0, 1.0], [0.0, 1.0]])
domain0 = pc.box2poly([[0.0, 2.0], [0.0, 2.0]])
subsystems += [hybrid.LtiSysDyn(A, B, Uset=Uset, domain=domain0)]
# subsystem 1
domain1 = pc.box2poly([[2.0, 4.0], [0.0, 2.0]])
subsystems += [hybrid.LtiSysDyn(A, B, Uset=Uset, domain=domain1)]
# PWA system
domain = domain0.union(domain1)
pwa = hybrid.PwaSysDyn(subsystems, domain)
# Switched system (mode dynamics the same, just testing code)
dom = (2, 2)
dyn = {
('a', 'c'):pwa,
('a', 'd'):pwa,
('b', 'c'):pwa,
}
env_labels = ['a', 'b']
sys_labels = ['c', 'd']
hyb = hybrid.SwitchedSysDyn(
disc_domain_size=dom,
dynamics=dyn,
cts_ss=domain,
env_labels=env_labels,
disc_sys_labels=sys_labels
)
print(hyb)
assert(hyb.disc_domain_size == dom)
assert(hyb.dynamics == dyn)
assert(hyb.env_labels == env_labels)
assert(hyb.disc_sys_labels == sys_labels)
assert(hyb.cts_ss == domain)
def drifting_dynamics():
A = np.array([[1.0, 0.0], [0.0, 1.0]])
B = np.array([[1.0], [0.0]])
U = pc.box2poly([[0.0, 1.0]])
K = np.array([[-100.0], [0.0]])
sys = hybrid.LtiSysDyn(A, B, None, K, U, None, dom)
return sys
def test_abstract_the_dynamics():
dom = pc.box2poly([[0.0, 10.0], [0.0, 20.0]])
ppp = define_partition(dom)
sys = define_dynamics(dom)
logger.info(sys)
disc_options = {"N": 3, "trans_length": 2, "min_cell_volume": 1.5}
ab = abstract.discretize(ppp, sys, plotit=False, save_img=False, **disc_options)
assert ab.ppp.compute_adj()
assert ab.ppp.is_partition()
def test_transient_regions():
"""drift is too strong, so no self-loop must exist
This bug caused when running union is taken between Presets
during solve_feasible, as happened with old use_all_horizon,
cf:
- 5b1e9681918739b276a221fcc1fd6eebfd058ce3
- f5f4934ab9d21062f633eef3861ad935c3d3b54b
"""
dom = pc.box2poly([[0.0, 4.0], [0.0, 3.0]])
def cont_predicates():
p = dict()
p["safe"] = pc.box2poly([[0.5, 3.5], [0.5, 2.5]])
ppp = abstract.prop2part(dom, p)
ppp, new2old_reg = abstract.part2convex(ppp)
ppp.plot()
return ppp
def drifting_dynamics():
A = np.array([[1.0, 0.0], [0.0, 1.0]])
B = np.array([[1.0], [0.0]])
U = pc.box2poly([[0.0, 1.0]])
K = np.array([[-100.0], [0.0]])
sys = hybrid.LtiSysDyn(A, B, None, K, U, None, dom)
return sys
ppp = cont_predicates()
sys = drifting_dynamics()
logger.info(sys)
with assert_raises(ValueError):
ab = abstract.discretize(ppp, sys, N=1, use_all_horizon=True, trans_length=1)
ab = abstract.discretize(ppp, sys, N=1, use_all_horizon=False, trans_length=1)
logger.debug(ab.ts)
self_loops = {i for i, j in ab.ts.transitions() if i == j}
logger.debug("self loops at states: " + str(self_loops))
assert not self_loops
def generateObservedDynamics(self, L, epsilon):
"""Generates the dynamics of the linear observer::
\hat{s}[t+1] = (A-LC)\hat{s}[t] + Bu[t] + K + Ly[t]
where L is an observer chosen so as to guarantee that
|A-LC| <= 1-max(E\delta)/epsilon.
"""
n = self.A.shape[0]
xbounds = [-epsilon, epsilon]
for i in range(n - 1):
xbounds = np.vstack((xbounds, [-epsilon, epsilon]))
Wset = pc.box2poly(xbounds)
return LtiSysDyn(
A=self.A,
B=self.B,
E=np.dot(L, self.C),
K=self.K,
Uset=self.Uset,
Wset=Wset,
domain=self.domain,
time_semantics=self.time_semantics,
timestep=self.timestep,
)
def plot_orthotopes(u, abx, axvars, fol, ax):
"""Plot a polytope for each orthotope in `u`.
@param axvars: `list` that defines which variable
spans each dimension of the plot.
"""
try:
import polytope as poly
except ImportError:
raise ImportError(
'`orthotopes` failed to import `polytope`.\n'
'No plotting of orthotopes.')
c = _orthotopes_iter(u, fol)
eps = 0.1
cycol = cycle('bgrcmk')
for product in c:
x, y = axvars
a_x = abx[x]['a']
b_x = abx[x]['b']
a_y = abx[y]['a']
b_y = abx[y]['b']
xmin = product[a_x]
xmax = product[b_x]
ymin = product[a_y]
ymax = product[b_y]
# if a = b add a small amount
if xmin == xmax:
xmin -= eps
xmax += eps
if ymin == ymax:
ymin -= eps
ymax += eps
size = [[xmin, xmax], [ymin, ymax]]
p = poly.box2poly(size)
color = next(cycol)
p.plot(ax=ax, color=color, alpha=0.5)
def specify_discretize_synthesize():
"""Return PWA partition and controller, dump them to pickle files."""
# Problem parameters
input_bound = 1.0
uncertainty = 0.01
# Continuous state space
cont_state_space = box2poly([[0., 3.], [0., 2.]])
# Continuous dynamics
A = np.array([[1.0, 0.], [0., 1.0]])
B = np.array([[0.1, 0.], [0., 0.1]])
E = np.array([[1., 0.], [0., 1.]])
# Available control, possible disturbances
U = input_bound * np.array([[-1., 1.], [-1., 1.]])
W = uncertainty * np.array([[-1., 1.], [-1., 1.]])
# Convert to polyhedral representation
U = box2poly(U)
W = box2poly(W)
# Construct the LTI system describing the dynamics
sys_dyn = hybrid.LtiSysDyn(A, B, E, None, U, W, cont_state_space)
# Define atomic propositions for relevant regions of state space
cont_props = {}
cont_props['home'] = box2poly([[0., 1.], [0., 1.]])
cont_props['lot'] = box2poly([[2., 3.], [1., 2.]])
# Compute proposition preserving partition of the continuous state space
cont_partition = prop2part(cont_state_space, cont_props)
pwa = discretize(
cont_partition, sys_dyn, closed_loop=True,
N=8, min_cell_volume=0.1, plotit=False)
"""Specifications"""
# Environment variables and assumptions
env_vars = {'park'}
env_init = set()
env_prog = '!park'
env_safe = set()
# System variables and requirements
sys_vars = {'X0reach'}
sys_init = {'X0reach'}
sys_prog = {'home'} # []<>home
sys_safe = {'(X(X0reach) <-> lot) || (X0reach && !park)'}
sys_prog |= {'X0reach'}
# Create the specification
specs = spec.GRSpec(env_vars, sys_vars, env_init, sys_init,
env_safe, sys_safe, env_prog, sys_prog)
specs.qinit = '\A \E'
specs.moore = False
specs.plus_one = False
"""Synthesize"""
ctrl = synth.synthesize(
specs, sys=pwa.ts, ignore_sys_init=True, solver='gr1c')
# store the result for future use
if len(BUILDDIR) > 0 and not os.path.exists(BUILDDIR):
os.mkdir(BUILDDIR)
pickle.dump(ctrl, open(BUILDDIR + 'FSM.p', 'wb'))
pickle.dump(pwa, open(BUILDDIR + 'AbstractPwa.p', 'wb'))
return pwa, ctrl
import polytope as pc
import matplotlib.pyplot as plt
print('Initialise values')
# Define the linear time invariant system
A = np.array([[0, -0.8572], [0.1, 0.5]])
B = np.eye(2) #array([[1],[0.1]])
C = np.eye(
2) # defines metric for error (||y_finite-y||< epsilon with y= cx )
sys = LTI(A, B, C, None) # LTI system with D = None
# define noise (Diagonal ONLY!)
sys.setBw(np.array([[.9, 0], [0.0, .9]]))
# Define spaces
poly = pc.box2poly(np.kron(np.ones((sys.dim, 1)), np.array([[-15, 15]])))
sys.setX(poly) # X space
sys.setU(pc.box2poly(np.kron(np.ones((sys.m, 1)), np.array([[-3, 3]]))))
# continuous set of inputs
Dist = pc.box2poly(np.kron(np.ones((sys.dim, 1)), np.array([[-.1, .1]])))
## step 1: tune gridding ratio (find optimal horizontal d_1, and vertical d_2)
# currently only available for 2D
print('1. Tune gridding ratio')
d_opt, d_vals, eps_values = tune_dratio(sys)
# d_opt has optimal ratio with grid diameter of 1
# choose grid sizes (factor of d_opt)
d = 1 * d_opt # with distance measure
print('Choose grid ribs as', d)
abs_tol=1e-7
A_off=np.array([[0.9998,0.],[0.,1.]])
A_heat=np.array([[0.9998,0.0002],[0.,1.]])
A_cool=np.array([[0.9998,0.0002],[0.,1.]])
A_on=np.array([[0.9998,0.0002],[0.,1.]])
K_off=np.array([[0.0032],[0.]])
K_heat=np.array([[0.],[0.01]])
K_cool=np.array([[0.],[-0.01]])
K_on=np.array([[0.],[0.]])
B_zero= np.array([[0., 0.], [ 0., 0.]])
cont_state_space = box2poly([[15., 24.],[15., 24.]])
cont_props = {}
cont_props['LOW'] = box2poly([[17., 19.], [20., 22.]])
cont_props['HIGH'] = box2poly([[21., 22.], [20., 22.]])
cont_props['OUTSIDE'] = box2poly([[24.,25.],[24.,25.]])
orig_props=set(cont_props)
out=[]
out.append(box2poly([[24.,25.],[24.,25.]]))
sdyn_off = hybrid.LtiSysDyn(A_off, B_zero, None, K_off, None, None, cont_state_space)
sdyn_heat = hybrid.LtiSysDyn(A_heat, B_zero, None, K_heat, None, None, cont_state_space)
sdyn_cool = hybrid.LtiSysDyn(A_cool,B_zero, None, K_cool, None, None, cont_state_space)
sdyn_on = hybrid.LtiSysDyn(A_on, B_zero, None, K_on, None, None, cont_state_space)
pwa_off = hybrid.PwaSysDyn(list_subsys=[sdyn_off],domain=cont_state_space)#,time_semantics='sampled',timestep=0.1)
from tulip.abstract.plot import plot_strategy from tulip.hybrid import LtiSysDyn from tulip.hybrid import PwaSysDyn from tulip import spec from tulip import synth # set to `True` if `matplotlib.pyplot` is available plotting = False # Problem parameters input_bound = 0.4 uncertainty = 0.05 # Continuous state space cont_state_space = box2poly([[0., 3.], [0., 2.]]) # Assume, for instance, our robot is traveling on # a nonhomogenous surface (xy plane), # resulting in different dynamics at different # parts of the plane. # # Since the continuous state space in this example # is just xy position, different dynamics in # different parts of the surface can be modeled # using LtiSysDyn subsystems subsys0 and subsys1. # # Together they comprise a Piecewise Affine System: # @subsystem0@ def subsys0():
def transition_directions_test():
"""
unit test for correctness of abstracted transition directions, with:
- uni-directional control authority
- no disturbance
"""
modes = []
modes.append(('normal', 'fly'))
modes.append(('refuel', 'fly'))
env_modes, sys_modes = zip(*modes)
cont_state_space = pc.box2poly([[0., 3.], [0., 2.]])
pwa_sys = dict()
pwa_sys[('normal', 'fly')] = hybrid.PwaSysDyn(
[subsys0()], cont_state_space
)
pwa_sys[('refuel', 'fly')] = hybrid.PwaSysDyn(
[subsys1()], cont_state_space
)
switched_dynamics = hybrid.SwitchedSysDyn(
disc_domain_size=(len(env_modes), len(sys_modes)),
dynamics=pwa_sys,
env_labels=env_modes,
disc_sys_labels=sys_modes,
cts_ss=cont_state_space
)
cont_props = {}
cont_props['home'] = pc.box2poly([[0., 1.], [0., 1.]])
cont_props['lot'] = pc.box2poly([[2., 3.], [1., 2.]])
ppp = abstract.prop2part(cont_state_space, cont_props)
ppp, new2old = abstract.part2convex(ppp)
N = 8
trans_len=1
disc_params = {}
for mode in modes:
disc_params[mode] = {'N':N, 'trans_length':trans_len}
swab = abstract.discretize_switched(
ppp, switched_dynamics, disc_params,
plot=True, show_ts=True, only_adjacent=False
)
ts = swab.modes[('normal', 'fly')].ts
edges = {(0, 0), (1, 1), (2, 2), (3, 3),
(4, 4), (5, 5),
(1, 2), (1, 4), (1, 5),
(2, 3), (2, 5), (2, 0),
(3, 0),
(4, 5),
(5, 0)}
logger.debug(set(ts.edges() ).symmetric_difference(edges) )
assert(set(ts.edges() ) == edges)
ts = swab.ts
assert(set(ts.edges() ) == edges)
for i, j in edges:
assert(ts[i][j][0]['env_actions'] == 'normal')
assert(ts[i][j][0]['sys_actions'] == 'fly')
def main():
print('Initialise values')
# Define the linear time invariant system
#A = np.array([[0,-0.8572],[0.1,0.5]])
dim = 2
A = np.eye(2) #np.array([[.9,-0.32],[0.1,0.9]])
B = np.eye(dim) #array([[1], [0.1]])
Tr = .5 * np.array([[-1, 1], [1, -1]])
W = 2 * Tr.dot(np.eye(dim)).dot(Tr) # noise on transitions
print(W)
# Accuracy
C = np.array([[
1, 0
]]) # defines metric for error (||y_finite-y||< epsilon with y= cx )
sys = LTI(A, B, C, None, W=W) # LTI system with D = None
# Define spaces
sys.setU(pc.box2poly(np.kron(np.ones(
(sys.m, 1)), np.array([[-3, 3]])))) # continuous set of inputs
sys.setX(pc.box2poly(np.kron(np.ones((sys.dim, 1)),
np.array([[-10, 10]])))) # X space
# Define regions of interest for the labeling
regions = dict()
regions['target'] = pc.box2poly(
np.kron(np.ones((2, 1)), np.array([[5, 10]])))
# add avoid
regions['avoid'] = pc.box2poly(np.array([[-5, 3], [-10, 5]]))
print('1. Transform to normalised state space')
sys_n = sys.normalize()
## step 1: tune gridding ratio (find optimal horizontal d_1, and vertical d_2)
# currently only available for 2D
print('2. Tune gridding ratio')
d_opt, d_vals, eps_values = tune_dratio(sys_n)
# d_opt has optimal ratio with grid diameter of 1
# choose grid sizes (factor of d_opt)
d = d_opt # with distance measure
print('Choose grid ribs as', d) # *Grid space
print('3. Grid Gaussian process')
print(sys_n.T2x)
mdp_grid = sys_n.abstract_io(d, un=7, verbose=False) # do the gridding
print('--- done gridding')
print('4. Define formula and compute DFA')
#('output',system.output(4))
formula = '( ( ! avoid U target ) )'
# figure out a map dict_input2prop from numeric inputs to name based inputs
dfsa, init, final, dict_input2prop = formula_to_mdp(formula)
mdp_grid.map_dfa_inputs(dict_input2prop, regions)
mdp_grid.setdfa(dfsa, final)
print('5. Compute recursions')
V, policy, W = mdp_grid.reach_dfa(recursions=10)
print('6. Plot normalized systen')
xi, yi = np.meshgrid(*mdp_grid.srep)
plt.pcolor(mdp_grid.sedge[0], mdp_grid.sedge[1], W[:-1].reshape(xi.shape,
order='F'))
plt.colorbar()
plt.xlim(np.array([mdp_grid.srep[0][0], mdp_grid.srep[0][-1]]))
plt.ylim(np.array([mdp_grid.srep[1][0], mdp_grid.srep[1][-1]]))
#plt.show()
pol = Rpol(mdp_grid, V, W, policy)
xi, yi = np.meshgrid(
np.linspace(mdp_grid.srep[0][0], mdp_grid.srep[0][-1], 10),
np.linspace(mdp_grid.srep[1][0], mdp_grid.srep[1][-1], 10))
# compute inputs
u = sys_n.b.dot(pol(np.block([[xi.flatten()], [yi.flatten()]])))
delx = (-np.block([[xi.flatten()], [yi.flatten()]]) +
sys_n.a.dot(np.block([[xi.flatten()], [yi.flatten()]])) +
sys_n.b.dot(pol(np.block([[xi.flatten()], [yi.flatten()]]))))
x_tr = (np.block([[xi.flatten()], [yi.flatten()]]))
#plt.quiver(xi.flatten(), yi.flatten(),u[0],u[1])
plt.quiver(x_tr[0], x_tr[1], delx[0], delx[1], color='r')
plt.show()
print('6. Plot concrete systen')
x_edge = np.linspace(-10, 10, 80)
x_del = np.diff(x_edge).max()
y_edge = np.linspace(-10, 10, 80)
y_del = np.diff(y_edge).max()
xi, yi = np.meshgrid(x_edge[:-1] + x_del / 2, y_edge[:-1] + y_del / 2)
values = pol.val_concrete(np.block([[xi.flatten()], [yi.flatten()]]))
plt.pcolor(x_edge, y_edge, values.reshape(xi.shape))
plt.colorbar()
plt.xlim(np.array([-10, 10]))
plt.ylim(np.array([-10, 10]))
plt.show()
def __init__(self, lti_syst, eta, un=3, T2x=None, MKeps=None):
'''Construct a grid abstraction of a LTI Gaussian system
:param lti_syst: A LTI system (noise matrix must be diagonal)
:param eta: abstraction grid size (one for each dimension)
:param un: number of discrete inputs per dimension
:param T2x=None: transformation matrix (use for rotated systems for easy access to original coordinates)
:param MKeps=None: tuple (M, K, eps) defining a simulation relation. if None one will be computed
'''
# check that W is diagonal
if not np.all(lti_syst.W == np.diag(np.diagonal(lti_syst.W))):
raise Exception('system noise must be diagonal')
# store state transformation matrix
if lti_syst.T2x is None:
self.T2x = np.eye(lti_syst.dim) # identity
else:
self.T2x = lti_syst.T2x
# compute/store simulation relation
if MKeps is None:
dist = pc.box2poly(np.diag(eta).dot(np.kron(np.ones((lti_syst.dim, 1)), np.array([[-1, 1]]))))
self.M, self.K, self.eps = eps_err(lti_syst, dist)
else:
self.M = MKeps[0]
self.K = MKeps[1]
self.eps = MKeps[2]
# state discretization information
lx, ux = pc.bounding_box(lti_syst.X)
lx = lx.flatten()
ux = ux.flatten()
remainx = eta - np.remainder(ux-lx, eta) # center slack
lx -= remainx/2
ux += remainx/2
self.x_low = lx
self.x_up = ux
self.eta_list = eta.flatten()
self.n_list = tuple(np.ceil((self.x_up - self.x_low)/self.eta_list).astype(int))
# save input discretization information: place inputs on boundary
# NOTE: bounding box may give infeasible inputs..
lu, uu = pc.bounding_box(lti_syst.U)
self.u_low = lu.flatten()
self.m_list = tuple(un for i in range(lti_syst.m))
self.eta_u_list = (uu.flatten() - self.u_low)/(np.array(self.m_list)-1)
transition_list = [np.zeros((self.N+1, self.N+1)) for m in range(prod(self.m_list))] # one dummy state
# extract all transitions
for ud in range(prod(self.m_list)):
Pmat = np.zeros((self.N+1, self.N+1))
for s in range(self.N):
s_diag = super(LTIGrid, self).s_to_x(s)
mean = np.dot(lti_syst.a, s_diag) + np.dot(lti_syst.b, self.ud_to_u(ud)) # Ax
P = np.ravel(grid_cdf_nd(mean, lti_syst.W, self.x_low, self.x_up, self.eta_list))
Pmat[s, 0:self.N] = P
Pmat[s, self.N] = 1 - sum(P)
Pmat[self.N, self.N] = 1
transition_list[ud] = Pmat
self.mdp = POMDP(transition_list, input_names=['u_d'], state_name='s',
output_trans=lambda s: (s, self.s_to_x(s)), output_name='(s,xc)')
def define_dynamics_dual():
# Continuous state space
cont_state_space = pc.box2poly([[-1.5, 1.5]])
# Continuous dynamics
# (continuous-state, discrete-time)
A = np.array([[2]])
B = np.array([[1]])
# Available control, possible disturbances
U = np.array([[-2.0, 2.0]])
# Convert to polyhedral representation
U = pc.box2poly(U)
# Construct the LTI system describing the dynamics
sys_dyn = hybrid.LtiSysDyn(A, B, None, None, U, None, cont_state_space)
# @dynamics_section_end@
# @partition_section@
# Define atomic propositions for relevant regions of state space
cont_props = {}
cont_props['a'] = pc.box2poly([[-1.5, -1]])
cont_props['b'] = pc.box2poly([[-1, 1]])
cont_props['c'] = pc.box2poly([[1, 1.5]])
part = []
part.append(pc.box2poly([[-1.5, -1]]))
part.append(pc.box2poly([[-1, 1]]))
part.append(pc.box2poly([[1, 1.5]]))
part.append(pc.box2poly([[-1, 0.5]]))
part.append(pc.box2poly([[-0.5, 1]]))
part.append(pc.box2poly([[-0.5, 0.5]]))
part.append(pc.box2poly([[-1.25, -1]]))
part.append(pc.box2poly([[1, 1.25]]))
# Compute the proposition preserving partition of the continuous state
# space
cont_partition = abstract.prop2part(cont_state_space, cont_props)
return sys_dyn, cont_partition, part
def find_equilibria(ssd, eps=0):
""" Finds the polytope that contains the equilibrium points
@param ssd: The dynamics of the switched system
@type ssd: L{SwitchedSysDyn}
@param eps: The value by which the width of all polytopes
containing equilibrium points is increased.
@type eps: float
@return param cont_props: The polytope representations of the atomic
propositions of the state space to be used in partitiong
@type cont_props: dict of polytope.Polytope
Warning: Currently, there is no condition for points where
the polytope is critically stable.
Warning: if there is a region outside the domain, then it is
unstable. It seems to ignore the regions outside the domain.
"""
def normalize(A, B):
""" Normalizes set of equations of the form Ax<=B
"""
if A.size > 0:
Anorm = np.sqrt(np.sum(A * A, 1)).flatten()
pos = np.nonzero(Anorm > 1e-10)[0]
A = A[pos, :]
B = B[pos]
Anorm = Anorm[pos]
mult = 1 / Anorm
for i in xrange(A.shape[0]):
A[i, :] = A[i, :] * mult[i]
B = B.flatten() * mult
return A, B
cont_ss = ssd.cts_ss
min_outx = cont_ss.b[0]
min_outy = cont_ss.b[1]
max_outx = min_outx + 1
max_outy = min_outy + 1
abs_tol = 1e-7
cont_props = dict()
for mode in ssd.modes:
cont_dyn = ssd.dynamics[mode].list_subsys[0]
A = cont_dyn.A
K = cont_dyn.K.T[0]
I = np.eye(len(A), dtype=float)
rank_IA = np.linalg.matrix_rank(I - A)
concat = np.hstack((I - A, K.reshape(len(A), 1)))
rank_concat = np.linalg.matrix_rank(concat)
soln = pc.Polytope()
props_sym = "eqpnt_" + str(mode[1])
if rank_IA == rank_concat:
if rank_IA == len(A):
equil = np.dot(np.linalg.inv(I - A), K)
if (
equil[0] >= (-cont_ss.b[2])
and equil[0] <= cont_ss.b[0]
and equil[1] >= (-cont_ss.b[3])
and equil[1] <= cont_ss.b[1]
):
delta = eps + equil / 100
soln = pc.box2poly(
[[equil[0] - delta[0], equil[0] + delta[0]], [equil[1] - delta[1], equil[1] + delta[1]]]
)
else:
soln = pc.box2poly([[min_outx, max_outx], [min_outy, max_outy]])
elif rank_IA < len(A):
if eps == 0:
eps = abs(min(np.amin(-K), np.amin(A - I)))
IAn, Kn = normalize(I - A, K)
soln = pc.Polytope(np.vstack((IAn, -IAn)), np.hstack((Kn + eps, -Kn + eps)))
relevantsoln = pc.intersect(soln, cont_ss, abs_tol)
if pc.is_empty(relevantsoln) & ~pc.is_empty(soln):
soln = pc.box2poly([[min_outx, max_outx], [min_outy, max_outy]])
else:
soln = relevantsoln
else:
# Assuming trajectories go to infinity as there are no
# equilibrium points
soln = pc.box2poly([[min_outx, max_outx], [min_outy, max_outy]])
print str(mode) + " trajectories go to infinity! No solution"
cont_props[props_sym] = soln
return cont_props
from tulip.abstract.plot import plot_partition from tulip.abstract.prop2partition import shrinkPartition, shrinkPoly from tulip.hybrid import generateFilter from cvxopt import matrix # @import_section_end@ show = False # @dynamics_section@ # Problem parameters input_bound = 6.0 uncertainty = 0.001 epsilon = 0.02 filter_bound = 1 - uncertainty / epsilon # Continuous state space cont_state_space = box2poly([[0., 3.], [0., 2.]]) # Continuous dynamics A = np.array([[0.95, 0.2], [0., 0.95]]) #need (A,C) observable B = np.array([[0.2, 0.], [0., 0.2]]) C = np.array([[1.0, 1.0]]) E = np.array([[1.0, 0.], [0., 1.0]]) # Available control, possible disturbances U = input_bound * np.array([[-1., 1.], [-1., 1.]]) W = uncertainty * np.array([[-1., 1.], [-1., 1.]]) # Convert to polyhedral representation U = box2poly(U) W = box2poly(W)
from tulip.abstract.plot import plot_partition from tulip.abstract.prop2partition import shrinkPartition, shrinkPoly from tulip.hybrid import generateFilter from cvxopt import matrix # @import_section_end@ show = False # @dynamics_section@ # Problem parameters input_bound = 6.0 uncertainty = 0.001 epsilon = 0.02 filter_bound = 1 - uncertainty/epsilon # Continuous state space cont_state_space = box2poly([[0., 3.], [0., 2.]]) # Continuous dynamics A = np.array([[0.95, 0.2], [ 0., 0.95]]) #need (A,C) observable B = np.array([[0.2, 0.], [ 0., 0.2]]) C = np.array([[1.0, 1.0]]) E = np.array([[1.0,0.], [0.,1.0]]) # Available control, possible disturbances U = input_bound *np.array([[-1., 1.], [-1., 1.]]) W = uncertainty *np.array([[-1., 1.], [-1., 1.]]) # Convert to polyhedral representation U = box2poly(U) W = box2poly(W)
def build_FTS(index):
# build test FTS
# simple test
if index == 0:
ts = FTS()
ts.atomic_propositions.add_from({'a', 'b', 'c', 'd'})
ts.states.add_from([('q1', {
'ap': {'a'}
}), ('q2', {
'ap': {'a'}
}), ('q3', {
'ap': {'b'}
}), ('q4', {
'ap': {'b'}
}), ('q5', {
'ap': {'b'}
}), ('q6', {
'ap': {'c'}
}), ('q7', {
'ap': {'d'}
})])
ts.transitions.add('q1', 'q3')
ts.transitions.add('q1', 'q4')
ts.transitions.add('q3', 'q6')
ts.transitions.add('q4', 'q6')
ts.transitions.add('q2', 'q4')
ts.transitions.add('q2', 'q5')
ts.transitions.add('q5', 'q7')
ts.transitions.add('q6', 'q6')
ts.transitions.add('q7', 'q7')
# FTS generated by continuous system
elif index == 1:
# Problem parameters
input_bound = 1.0
# Continuous state space
cont_state_space = box2poly([[-1, 1], [-1, 1]])
# Continuous dynamics
# (continuous-state, discrete-time)
A = np.array([[0.5, 1.0], [0.75, -1.0]])
B = np.array([[1, 0.], [0., 1]])
# Available control, possible disturbances
U = input_bound * np.array([[-1., 1.], [-1., 1.]])
# Convert to polyhedral representation
U = box2poly(U)
# Construct the LTI system describing the dynamics
sys_dyn = hybrid.LtiSysDyn(A, B, None, None, U, None, cont_state_space)
# @dynamics_section_end@
# @partition_section@
# Define atomic propositions for relevant regions of state space
cont_props = {}
cont_props['a'] = box2poly([[-0.5, 0.5], [-0.5, 0.5]])
cont_props['b'] = box2poly([[-1, -0.5], [-1, 1]])
cont_props['c'] = box2poly([[-0.5, 1], [0.5, 1]])
cont_props['d'] = box2poly([[0.5, 1], [-1, 0.5]])
cont_props['e'] = box2poly([[-0.5, 0.5], [-1, -0.5]])
# Compute the proposition preserving partition of the continuous state space
cont_partition = prop2part(cont_state_space, cont_props)
# @partition_section_end@
# @discretize_section@
# Given dynamics & proposition-preserving partition, find feasible transitions
sys = discretize(cont_partition,
sys_dyn,
closed_loop=False,
conservative=True,
trans_length=1,
N=1,
use_all_horizon=False,
min_cell_volume=0.0,
abs_tol=1e-7)
ts = sys.ts
return ts
def transition_directions_test():
"""
unit test for correctness of abstracted transition directions, with:
- uni-directional control authority
- no disturbance
"""
modes = []
modes.append(("normal", "fly"))
modes.append(("refuel", "fly"))
env_modes, sys_modes = zip(*modes)
cont_state_space = pc.box2poly([[0.0, 3.0], [0.0, 2.0]])
pwa_sys = dict()
pwa_sys[("normal", "fly")] = hybrid.PwaSysDyn([subsys0()], cont_state_space)
pwa_sys[("refuel", "fly")] = hybrid.PwaSysDyn([subsys1()], cont_state_space)
switched_dynamics = hybrid.SwitchedSysDyn(
disc_domain_size=(len(env_modes), len(sys_modes)),
dynamics=pwa_sys,
env_labels=env_modes,
disc_sys_labels=sys_modes,
cts_ss=cont_state_space,
)
cont_props = {}
cont_props["home"] = pc.box2poly([[0.0, 1.0], [0.0, 1.0]])
cont_props["lot"] = pc.box2poly([[2.0, 3.0], [1.0, 2.0]])
ppp = abstract.prop2part(cont_state_space, cont_props)
ppp, new2old = abstract.part2convex(ppp)
N = 8
trans_len = 1
disc_params = {}
for mode in modes:
disc_params[mode] = {"N": N, "trans_length": trans_len}
swab = abstract.discretize_switched(
ppp, switched_dynamics, disc_params, plot=True, show_ts=True, only_adjacent=False
)
ts = swab.modes[("normal", "fly")].ts
edges = {
(0, 0),
(1, 1),
(2, 2),
(3, 3),
(4, 4),
(5, 5),
(1, 2),
(1, 4),
(1, 5),
(2, 3),
(2, 5),
(2, 0),
(3, 0),
(4, 5),
(5, 0),
}
logger.debug(set(ts.edges()).symmetric_difference(edges))
assert set(ts.edges()) == edges
ts = swab.ts
assert set(ts.edges()) == edges
for i, j in edges:
assert ts[i][j][0]["env_actions"] == "normal"
assert ts[i][j][0]["sys_actions"] == "fly"
def specify_discretize_synthesize():
"""Return PWA partition and controller, dump them to pickle files."""
# Problem parameters
input_bound = 1.0
uncertainty = 0.01
# Continuous state space
cont_state_space = box2poly([[0., 3.], [0., 2.]])
# Continuous dynamics
A = np.array([[1.0, 0.], [0., 1.0]])
B = np.array([[0.1, 0.], [0., 0.1]])
E = np.array([[1., 0.], [0., 1.]])
# Available control, possible disturbances
U = input_bound * np.array([[-1., 1.], [-1., 1.]])
W = uncertainty * np.array([[-1., 1.], [-1., 1.]])
# Convert to polyhedral representation
U = box2poly(U)
W = box2poly(W)
# Construct the LTI system describing the dynamics
sys_dyn = hybrid.LtiSysDyn(A, B, E, None, U, W, cont_state_space)
# Define atomic propositions for relevant regions of state space
cont_props = {}
cont_props['home'] = box2poly([[0., 1.], [0., 1.]])
cont_props['lot'] = box2poly([[2., 3.], [1., 2.]])
# Compute proposition preserving partition of the continuous state space
cont_partition = prop2part(cont_state_space, cont_props)
pwa = discretize(cont_partition,
sys_dyn,
closed_loop=True,
N=8,
min_cell_volume=0.1,
plotit=False)
"""Specifications"""
# Environment variables and assumptions
env_vars = {'park'}
env_init = set()
env_prog = '!park'
env_safe = set()
# System variables and requirements
sys_vars = {'X0reach'}
sys_init = {'X0reach'}
sys_prog = {'home'} # []<>home
sys_safe = {'(X(X0reach) <-> lot) || (X0reach && !park)'}
sys_prog |= {'X0reach'}
# Create the specification
specs = spec.GRSpec(env_vars, sys_vars, env_init, sys_init, env_safe,
sys_safe, env_prog, sys_prog)
specs.qinit = r'\A \E'
specs.moore = False
specs.plus_one = False
"""Synthesize"""
ctrl = synth.synthesize(specs,
sys=pwa.ts,
ignore_sys_init=True,
solver='gr1c')
# store the result for future use
if len(BUILDDIR) > 0 and not os.path.exists(BUILDDIR):
os.mkdir(BUILDDIR)
pickle.dump(ctrl, open(BUILDDIR + 'FSM.p', 'wb'))
pickle.dump(pwa, open(BUILDDIR + 'AbstractPwa.p', 'wb'))
return pwa, ctrl
def test_find_controller_non_convex():
"""Test that it is enough that one polytope in the target region is
reachable"""
# Continuous state space
cont_state_space = pc.box2poly([[0., 1.], [0., 2.]])
# System dynamics (continuous state, discrete time): simple double integrator
# but no reversing.
h = 1.0
A = np.array([[1.0, h], [0., 1.0]])
B = np.array([[h**2 / 2.0], [h]])
E = None
# Available control, no disturbances. Can brake or accelerate.
U = pc.box2poly(np.array([[-1., 1.]]))
W = None
# Construct the LTI system describing the dynamics
sys_dyn = hybrid.LtiSysDyn(A, B, E, None, U, W, cont_state_space)
# Define atomic propositions for relevant regions of state space
cont_props = dict()
cont_props['target'] = pc.box2poly([[0., 1.], [0., 2.]])
# Compute the proposition preserving partition of the continuous state space
# noinspection PyTypeChecker
cont_partition = abstract.prop2part(cont_state_space, cont_props)
# Abstraction
disc_dynamics = abstract.discretize(cont_partition,
sys_dyn,
closed_loop=False,
conservative=True,
N=1,
min_cell_volume=0.01,
plotit=False,
simu_type='bi',
trans_length=1)
# Setup a start point and a target point in a region that are problematic
c_start_state = np.array([0.5, 0.0])
c_end_desired = np.array([1.0, 2.0])
# Find the discrete states of the start state and the target region
d_start_state = abstract.find_discrete_state(c_start_state,
disc_dynamics.ppp)
d_end_state = abstract.find_discrete_state(c_end_desired,
disc_dynamics.ppp)
# Try to find a control policy
u = abstract.find_controller.get_input(x0=c_start_state,
ssys=sys_dyn,
abstraction=disc_dynamics,
start=d_start_state,
end=d_end_state,
ord=1,
mid_weight=5)
assert 0.5 <= u <= 1.0, "u was " + str(u)
# Try to find a control policy in the other direction and ascertain
# an error is thrown
assert_raises(Exception,
abstract.find_controller.get_input,
x0=c_end_desired,
ssys=sys_dyn,
abstraction=disc_dynamics,
start=d_end_state,
end=d_start_state,
ord=1,
mid_weight=5)
def add_grid(ppp, grid_size=None, num_grid_pnts=None, abs_tol=1e-10):
""" This function takes a proposition preserving partition ppp and the size
of the grid or the number of grids, and returns a refined proposition
preserving partition with grids.
Input:
- `ppp`: a L{PropPreservingPartition} object
- `grid_size`: the size of the grid,
type: float or list of float
- `num_grid_pnts`: the number of grids for each dimension,
type: integer or list of integer
Output:
- A L{PropPreservingPartition} object with grids
Note: There could be numerical instabilities when the continuous
propositions in ppp do not align well with the grid resulting in very small
regions. Performace significantly degrades without glpk.
"""
if (grid_size!=None)&(num_grid_pnts!=None):
raise Exception("add_grid: Only one of the grid size or number of \
grid points parameters is allowed to be given.")
if (grid_size==None)&(num_grid_pnts==None):
raise Exception("add_grid: At least one of the grid size or number of \
grid points parameters must be given.")
dim=len(ppp.domain.A[0])
domain_bb = ppp.domain.bounding_box
size_list=list()
if grid_size!=None:
if isinstance( grid_size, list ):
if len(grid_size) == dim:
size_list=grid_size
else:
raise Exception(
"add_grid: grid_size isn't given in a correct format."
)
elif isinstance( grid_size, float ):
for i in xrange(dim):
size_list.append(grid_size)
else:
raise Exception("add_grid: "
"grid_size isn't given in a correct format.")
else:
if isinstance( num_grid_pnts, list ):
if len(num_grid_pnts) == dim:
for i in xrange(dim):
if isinstance( num_grid_pnts[i], int ):
grid_size=(
float(domain_bb[1][i]) -float(domain_bb[0][i])
) /num_grid_pnts[i]
size_list.append(grid_size)
else:
raise Exception("add_grid: "
"num_grid_pnts isn't given in a correct format.")
else:
raise Exception("add_grid: "
"num_grid_pnts isn't given in a correct format.")
elif isinstance( num_grid_pnts, int ):
for i in xrange(dim):
grid_size=(
float(domain_bb[1][i])-float(domain_bb[0][i])
) /num_grid_pnts
size_list.append(grid_size)
else:
raise Exception("add_grid: "
"num_grid_pnts isn't given in a correct format.")
j=0
list_grid=dict()
while j<dim:
list_grid[j] = compute_interval(
float(domain_bb[0][j]),
float(domain_bb[1][j]),
size_list[j],
abs_tol
)
if j>0:
if j==1:
re_list=list_grid[j-1]
re_list=product_interval(re_list, list_grid[j])
else:
re_list=product_interval(re_list, list_grid[j])
j+=1
new_list = []
parent = []
for i in xrange(len(re_list)):
temp_list=list()
j=0
while j<dim*2:
temp_list.append([re_list[i][j],re_list[i][j+1]])
j=j+2
for j in xrange(len(ppp.regions)):
tmp = pc.box2poly(temp_list)
isect = tmp.intersect(ppp.regions[j], abs_tol)
#if pc.is_fulldim(isect):
rc, xc = pc.cheby_ball(isect)
if rc > abs_tol/2:
if rc < abs_tol:
print("Warning: "
"One of the regions in the refined PPP is too small"
", this may cause numerical problems")
if len(isect) == 0:
isect = pc.Region([isect], [])
isect.props = ppp.regions[j].props.copy()
new_list.append(isect)
parent.append(j)
adj = sp.lil_matrix((len(new_list), len(new_list)), dtype=np.int8)
for i in xrange(len(new_list)):
adj[i,i] = 1
for j in xrange(i+1, len(new_list)):
if (ppp.adj[parent[i], parent[j]] == 1) or \
(parent[i] == parent[j]):
if pc.is_adjacent(new_list[i], new_list[j]):
adj[i,j] = 1
adj[j,i] = 1
return PropPreservingPartition(
domain = ppp.domain,
regions = new_list,
adj = adj,
prop_regions = ppp.prop_regions
)
def add_collection(self, unit_regions, label=None):
# check if already existing
if label in self.index:
if 1<len(self.index):
raise RuntimeError('cannot overwrite a collection if other collections have already been defined')
self.__reset__()
#
i0 = len(self.unit_region)
self.unit_region += list(unit_regions)
# first group overlapping unit regions in the collection
current_index = max(self.group.keys())+1 if self.group else 0
not_an_index = -1
n = len(unit_regions)
assignment = np.full(n, not_an_index, dtype=int)
groups = dict()
for i in range(n):
region_i = unit_regions[i]
if isinstance(region_i, pt.Polytope):
region_i = pt.Region([region_i])
_min_i = _max_i = None
elif isinstance(region_i, pt.Region):
_min_i = _max_i = None
else:#if isinstance(region_i, (tuple, list)):
_min_i, _max_i = region_i
region_i = None
group_with = set()
if assignment[i] == not_an_index:
group_index = current_index
group = set([i0+i])
else:
group_index = assignment[i]
group_with.add(group_index)
group = groups[group_index]
assert i0+i in group
for j in range(i+1,n):
if i0+j in group:
continue
region_j = unit_regions[j]
if isinstance(region_j, (pt.Polytope, pt.Region)):
if region_i is None:
region_i = pt.box2poly(list(zip(_min_i,_max_i)))
self._unit_polytope[i0+i] = region_i
i_and_j_are_adjacent = pt.is_adjacent(region_i, region_j)
else:#if isinstance(region_j, (tuple, list)):
_min_j,_max_j = region_j
if _min_i is None:
region_j = pt.box2poly(list(zip(_min_j,_max_j)))
self._unit_polytope[i0+j] = region_j
i_and_j_are_adjacent = pt.is_adjacent(region_i, region_j)
else:
i_and_j_are_adjacent = np.all(_min_i<=_max_j) and np.all(_min_j<=_max_i)
if i_and_j_are_adjacent:
if assignment[j]==not_an_index:
group.add(i0+j)
else:
other_group_index = assignment[j]
group_with.add(other_group_index)
group |= groups.pop(other_group_index)
if group_with:
group_index = min(group_with)
else:
current_index += 1
groups[group_index] = group
group = np.array(list(group)) - i0 # indices in `assignment`
assignment[group] = group_index
#
# merge the new and existing groups together
for g in list(groups.keys()):
adjacent = set()
for h in self.group:
g_and_h_are_adjacent = False
for i in groups[g]:
for j in self.group[h]:
if self.adjacent(i,j):
g_and_h_are_adjacent = True
break
if g_and_h_are_adjacent:
adjacent.add(h)
break
if adjacent:
h = min(adjacent)
for i in adjacent - {h}:
self.group[h] |= self.group.pop(i)
self.group[h] |= groups.pop(g)
assignment[assignment==g] = h
if groups:
self.group.update(groups)
#
if label is None:
label = ''
self.index[label] = assignment
#
self.reverse_index = np.c_[
np.repeat(np.arange(len(self.index)), [ len(self.index[s]) for s in self.index ]),
np.concatenate([ np.arange(len(self.index[s])) for s in self.index ]) ]
Red_trans = ()
for Case in Cases:
print(
"----------------------------------\n System Constants \n----------------------------------"
)
epsilon = 1.
input_bound = 2
disturbance_bound = 0.01
A = np.array([[1.]])
B = np.array([[1.]])
E = np.array([[1.]])
X = box2poly([[0., 20]])
U = box2poly(input_bound * np.array([[-1., 1]]))
W = box2poly(disturbance_bound * np.array([[-1., 1]]))
if Case == '1 Sensor':
# -------------Simple specification
# example with only 4 labels
cprops = dict()
cprops["init"] = box2poly([[0., 5.]])
cprops["slow"] = box2poly([[5.0, 10]])
cprops["moderate"] = box2poly([[10.0, 15]])
cprops["fast"] = box2poly([[15.0, 20]])
env_vars = {'lidon'} # one new environment variable
env_init = {'!lidon'}
env_safe, env_prog = set(), set()
def abstract_io(self, d, un=3, verbose=True, Accuracy=True):
from ApprxSimulation.LTI_simrel import eps_err
## Unpack LTI
d = d.flatten()
A = self.a
B = self.b
C = self.c
#Bw = self.setBw()
U = self.setU()
# check that Bw is a diagonal
# = np.sum(np.absolute(np.dot(Bw,Bw.transpose()))) - np.trace(np.absolute(np.dot(Bw,Bw.transpose())))
assert np.sum(np.absolute(self.W)) - np.trace(np.absolute(self.W)) == 0
vars = np.diag(self.W)
X = self.setX()
n = self.dim
rad = LA.norm(d, 2)
lx, ux = pc.bounding_box(
self.X) # lower and upperbounds over all dimensions
remainx = np.remainder((ux - lx).flatten(), d.flatten())
remainx = np.array([
d.flatten()[i] - r if r != 0 else 0 for i, r in enumerate(remainx)
]).flatten()
lx = lx.flatten() - remainx / 2
ux = ux.flatten() + d
if Accuracy:
Dist = pc.box2poly(
np.diag(d).dot(
np.kron(np.ones((self.dim, 1)), np.array([[-1, 1]]))))
M_min, K_min, eps_min = eps_err(self, Dist)
else:
M_min, K_min, eps_min = None, None, None
if verbose == True and n == 2:
# plot figure of x
figure = plt.figure()
figure.add_subplot('111')
axes = figure.axes[0]
axes.axis('equal') # sets aspect ration to 1
# compute limits X
plt.xlim(np.array([lx[0], ux[0]]))
plt.ylim(np.array([lx[1], ux[1]]))
# GRID state
srep = tuple()
sedge = tuple()
for i, dval in enumerate(d):
srep += (np.arange(lx[i], ux[i], dval) + dval / 2, )
sedge += (np.arange(lx[i], ux[i] + dval, dval), )
grid = np.meshgrid(*srep)
xn = np.size(grid[0]) # number of finite states
# grid input
urep = tuple()
lu, uu = pc.bounding_box(
self.U) # lower and upperbounds over all dimensions
for i, low in enumerate(lu):
urep += (np.linspace(lu[i], uu[i], un, endpoint=True), )
ugrid = np.meshgrid(*urep)
un = np.size(ugrid[0]) # number of finite states
transition = np.zeros((un, xn + 1, xn + 1))
transition.fill(-10)
# def tostate(self, s):
# state = tuple()
# for s, i in enumerate(srep):
# for xx in range(len(srep[i]))
#
# state += (srep[i][index] ,)
#
# print(np.prod(lengths[::-2]))
# print(s % (np.prod(lengths[::-2])) % (np.prod(lengths[::-3])))
# print(srep[2][s % (np.prod(lengths[0:2])) % (np.prod(lengths[1:2]))])
# print(srep[1][(s / lengths[2]) % (np.prod(lengths[::-3]))])
# print(srep[0][(s / lengths[1] / lengths[2])])
# make dictionary
for u_index, u in enumerate(itertools.product(*urep)):
P = tuple()
for s, sstate in enumerate(itertools.product(*srep)):
#print(s,(sstate))
#
# # print(np.prod(lengths[::-2]))
# print(prod_len, lengths)
# print(srep[0][(s/(lengths[1]*lengths[2]))]) #1
# print(srep[1][(s/lengths[2])%lengths[1]]) #2
# print(srep[2][(s %(lengths[1]*lengths[2])) %lengths[2]])
mean = np.dot(A,
np.array(sstate).reshape(-1, 1)) + np.dot(
B,
np.array(u).reshape(-1, 1)) # Ax
# compute probability in each dimension
Pi = tuple()
for i in range(n):
if vars[i] > np.finfo(np.float32).eps:
Pi += (np.diff(norm.cdf(sedge[i], mean[i],
vars[i]**.5)).reshape(-1),
) # probabilities for different dimensions
else:
abs_dis = np.array(
map(lambda s: abs(s - mean[i]), srep[i]))
p_loc = np.zeros(srep[i].shape)
p_loc[abs_dis.argmin()] = 1
Pi += (p_loc, )
# multiply over dimensions
P += (np.array([[
reduce(operator.mul, p, 1) for p in itertools.product(*Pi)
]]), )
prob = np.concatenate(P, axis=0)
p_local = np.block(
[[prob, 1 - prob.dot(np.ones((prob.shape[1], 1)))],
[np.zeros((1, prob.shape[1])),
np.ones((1, 1))]])
transition[u_index] = p_local
# add dummy state that represents exiting the set of allowed states
mdp_grid = Markov(transition,
srep,
urep,
sedge,
M=M_min,
K=K_min,
eps=eps_min,
T2x=self.T2x)
if verbose == True and n == 2:
fig = plt.figure()
ax = fig.add_subplot(111)
ax.scatter(grid[0].flatten(),
grid[1].flatten(),
label='Finite states',
color='k',
s=10,
marker="o")
plt.xlabel('x')
plt.ylabel('y')
if Accuracy:
patch = patch_ellips((eps_min**-2) * M_min,
pos=None,
number=20)
ax.add_patch(patch)
# if B is lower dimension than A, give some info on the stationary points.
if self.m < self.dim:
A1 = np.eye(2) - self.a
AB = np.hstack((A1, self.b))
ratio = LA.inv(AB[:, 1:]).dot(AB[:, 0])
stablepoints = ratio[0] * srep[0]
ax.scatter(srep[0].flatten(),
stablepoints.flatten(),
label='Equilibrium states',
color='r',
s=20,
marker="+")
plt.legend()
# plt.tight_layout()
plt.show()
return mdp_grid
def tune_dratio(lti):
"""
Quantify accuracy of simulation with respect to disturbance given as a polytope
:param lti: contains dynamics matrix lti.a, lti.b
:param Dist: The disturbance given as a polytope
:return: Invariant set R and epsilon
"""
n = lti.dim
m = lti.m
A = lti.a
B = lti.b
C = lti.c
Dist = pc.box2poly(np.kron(np.ones((lti.dim, 1)), np.array([[-1, 1]])))
Vertices = pc.extreme(Dist)
d = cvx.Parameter(2, 1)
# define variables
Minv = cvx.Semidef(n)
L = cvx.Variable(m, n)
eps2 = cvx.Semidef(1)
lam = cvx.Parameter(sign="positive")
basic = cvx.bmat([[
cvx.diag(np.ones((n, 1)) * lam) * Minv,
np.zeros((n, 1)), Minv * A.T + L.T * B.T
], [np.zeros((1, n)), (1 - lam) * eps2,
np.zeros((1, n))], [A * Minv + B * L,
np.zeros((n, 1)), Minv]])
cmat = cvx.bmat([[Minv, Minv * C.T], [C * Minv, np.eye(C.shape[0])]])
constraintstup = (cmat >> 0, )
ri = np.zeros((n, 1))
for i in range(Vertices.shape[0]):
ri = Vertices[i].reshape((n, 1))
rmat = cvx.bmat(
[[np.zeros((n, n)),
np.zeros((n, 1)),
np.zeros((n, n))],
[np.zeros((1, n)),
np.zeros((1, 1)), ri.T * cvx.diag(d)],
[np.zeros((n, n)),
cvx.diag(d) * ri,
np.zeros((n, n))]])
constraintstup += (basic + rmat >> 0, )
constraints = list(constraintstup)
obj = cvx.Minimize(eps2)
prob = cvx.Problem(obj, constraints)
lam_vals = np.logspace(-.01, -2) # values to try
eps_values = [] # values for which there is a solution
eps_min = [] # track minimum value
M_min = []
K_min = []
optval = np.array([np.inf])
d_opt = []
d_vals = [] # values to try
for alpha in np.linspace(0.01 * math.pi, 0.4 * math.pi, 20):
eps_min = []
d_val = np.array([[math.cos(alpha)], [math.sin(alpha)]])
d_vals.append(d_val)
d.value = d_val
for val in lam_vals:
lam.value = val
try:
prob.solve()
except cvx.error.SolverError:
pass #print('cvx.error.SolverError')
# Use expr.value to get the numerical value of
# an expression in the problem.
if prob.status == cvx.OPTIMAL:
if eps2.value**.5 < eps_min:
eps_min = eps2.value**.5
eps_values.append(eps_min)
cost = d_val[0]**-1 * d_val[1]**-1 * eps_min**2
if cost[0] <= optval:
optval = cost
d_opt = d_val
# Plot entries of x vs. gamma.
plt.subplot(212)
plt.plot([dval[1] for dval in d_vals], [xi for xi in eps_values],
label='Epsilon')
plt.tight_layout()
plt.plot([dval[1] for dval in d_vals],
[dval[0]**-1 * dval[1]**-1 * eps_values[i]**2 for dval in d_vals],
label='Cost gridding a square')
plt.xlabel(r'd[1]', fontsize=16)
plt.ylabel(r'epsilon', fontsize=16)
plt.title(r' Tune grid ratio ', fontsize=16)
#plt.yscale('log')
plt.show()
return d_opt, d_vals, eps_values
from tulip import spec, synth, hybrid from polytope import box2poly from tulip.abstract import prop2part, discretize, find_controller # @import_section_end@ visualize = False from tulip.abstract.plot import plot_partition # @dynamics_section@ # Problem parameters input_bound = 1.0 uncertainty = 0.01 # Continuous state space cont_state_space = box2poly([[0.0, 3.0], [0.0, 2.0]]) # Continuous dynamics A = np.array([[1.0, 0.0], [0.0, 1.0]]) B = np.array([[0.1, 0.0], [0.0, 0.1]]) E = np.array([[1, 0], [0, 1]]) # Available control, possible disturbances U = input_bound * np.array([[-1.0, 1.0], [-1.0, 1.0]]) W = uncertainty * np.array([[-1.0, 1.0], [-1.0, 1.0]]) # Convert to polyhedral representation U = box2poly(U) W = box2poly(W) # Construct the LTI system describing the dynamics
from tulip import spec, synth, hybrid from polytope import box2poly from tulip.abstract import prop2part, discretize # @import_section_end@ visualize = False from tulip.abstract.plot import plot_partition # @dynamics_section@ # Problem parameters input_bound = 1.0 uncertainty = 0.01 # Continuous state space cont_state_space = box2poly([[0., 3.], [0., 2.]]) # Continuous dynamics A = np.array([[1.0, 0.], [ 0., 1.0]]) B = np.array([[0.1, 0.], [ 0., 0.1]]) E = np.array([[1,0], [0,1]]) # Available control, possible disturbances U = input_bound *np.array([[-1., 1.], [-1., 1.]]) W = uncertainty *np.array([[-1., 1.], [-1., 1.]]) # Convert to polyhedral representation U = box2poly(U) W = box2poly(W) # Construct the LTI system describing the dynamics
def find_equilibria(ssd, eps=0):
""" Finds the polytope that contains the equilibrium points
@param ssd: The dynamics of the switched system
@type ssd: L{SwitchedSysDyn}
@param eps: The value by which the width of all polytopes
containing equilibrium points is increased.
@type eps: float
@return param cont_props: The polytope representations of the atomic
propositions of the state space to be used in partitiong
@type cont_props: dict of polytope.Polytope
Warning: Currently, there is no condition for points where
the polytope is critically stable.
Warning: if there is a region outside the domain, then it is
unstable. It seems to ignore the regions outside the domain.
"""
def normalize(A, B):
""" Normalizes set of equations of the form Ax<=B
"""
if A.size > 0:
Anorm = np.sqrt(np.sum(A * A, 1)).flatten()
pos = np.nonzero(Anorm > 1e-10)[0]
A = A[pos, :]
B = B[pos]
Anorm = Anorm[pos]
mult = 1 / Anorm
for i in xrange(A.shape[0]):
A[i, :] = A[i, :] * mult[i]
B = B.flatten() * mult
return A, B
cont_ss = ssd.cts_ss
min_outx = cont_ss.b[0]
min_outy = cont_ss.b[1]
max_outx = min_outx + 1
max_outy = min_outy + 1
abs_tol = 1e-7
cont_props = dict()
for mode in ssd.modes:
cont_dyn = ssd.dynamics[mode].list_subsys[0]
A = cont_dyn.A
K = cont_dyn.K.T[0]
I = np.eye(len(A), dtype=float)
rank_IA = np.linalg.matrix_rank(I - A)
concat = np.hstack((I - A, K.reshape(len(A), 1)))
rank_concat = np.linalg.matrix_rank(concat)
soln = pc.Polytope()
props_sym = 'eqpnt_' + str(mode[1])
if (rank_IA == rank_concat):
if (rank_IA == len(A)):
equil = np.dot(np.linalg.inv(I - A), K)
if (equil[0] >= (-cont_ss.b[2]) and equil[0] <= cont_ss.b[0]
and equil[1] >= (-cont_ss.b[3])
and equil[1] <= cont_ss.b[1]):
delta = eps + equil / 100
soln = pc.box2poly(
[[equil[0] - delta[0], equil[0] + delta[0]],
[equil[1] - delta[1], equil[1] + delta[1]]])
else:
soln = pc.box2poly([[min_outx, max_outx],
[min_outy, max_outy]])
elif (rank_IA < len(A)):
if eps == 0:
eps = abs(min(np.amin(-K), np.amin(A - I)))
IAn, Kn = normalize(I - A, K)
soln = pc.Polytope(np.vstack((IAn, -IAn)),
np.hstack((Kn + eps, -Kn + eps)))
relevantsoln = pc.intersect(soln, cont_ss, abs_tol)
if (pc.is_empty(relevantsoln) & ~pc.is_empty(soln)):
soln = pc.box2poly([[min_outx, max_outx],
[min_outy, max_outy]])
else:
soln = relevantsoln
else:
#Assuming trajectories go to infinity as there are no
#equilibrium points
soln = pc.box2poly([[min_outx, max_outx], [min_outy, max_outy]])
print str(mode) + " trajectories go to infinity! No solution"
cont_props[props_sym] = soln
return cont_props
