def test_connection():
T0 = np.array([[0, 1], [1, 0]])
T1 = np.array([[1, 0], [0, 1]])
mdp1 = MDP([T0, T1],
input_fcn=lambda m: m,
output_fcn=lambda n: n,
input_name='u',
output_name='z')
T0 = np.array([[1, 0], [1, 0]])
T1 = np.array([[0, 1], [0, 1]])
mdp2 = MDP([T0, T1],
input_fcn=lambda m: m,
output_fcn=lambda n: n,
input_name='z',
output_name='y')
pmdp = mdp1.product(mdp2, lambda n1: set([n1]))
np.testing.assert_almost_equal(
pmdp.T(0).todense(),
np.array([[0, 0, 0, 1], [0, 0, 0, 1], [1, 0, 0, 0], [1, 0, 0, 0]]))
np.testing.assert_almost_equal(
pmdp.T(1).todense(),
np.array([[1, 0, 0, 0], [1, 0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 1]]))
vals1, _ = pmdp.solve_reach(accept=lambda s: s[0] == 0 and s[1] == 1)
np.testing.assert_almost_equal(vals1[0], [0, 1, 0, 0])
vals2, _ = pmdp.solve_reach(accept=lambda s: s[0] == 0 and s[1] == 0)
np.testing.assert_almost_equal(vals2[0], [1, 1, 1, 1])
def test_reach():
T0 = np.array([[0.5, 0.25, 0.25], [0, 1, 0], [0, 0, 1]])
mdp = MDP([T0])
V, _ = mdp.solve_reach(accept=lambda y: y == 2)
np.testing.assert_almost_equal(V[0], [0.5, 0, 1], decimal=4)
def environment_belief_model(p0, levels, name):
# Create map belief MDP with prior p0 and qw quality of weak measurements
if p0 == 0:
# no dynamics
return MDP([np.array([1])],
input_name=name + '_u',
output_name=name + '_b',
input_fcn=lambda n: 0,
output_fcn=lambda s: 0)
elif p0 == 1:
return MDP([np.array([1])],
input_name=name + '_u',
output_name=name + '_b',
input_fcn=lambda n: 0,
output_fcn=lambda s: 1)
else:
pm = levels[0]
pp = levels[1]
Tnone = np.eye(5)
Tweak = np.array([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0],
[0, 1 - p0, 0, p0, 0], [0, 0, 0, 1, 0],
[0, 0, 0, 0, 1]])
Tstrong = np.array([[1, 0, 0, 0, 0], [(1 - pm), 0, 0, 0, pm],
[(1 - p0), 0, 0, 0, p0], [(1 - pp), 0, 0, 0, pp],
[0, 0, 0, 0, 1]])
def output_fcn(s):
return [0, pm, p0, pp, 1][s]
return MDP([Tnone, Tweak, Tstrong],
input_name=name + '_u',
output_name=name + '_b',
output_fcn=output_fcn)
def test_ltl_synth():
T1 = np.array([[0.25, 0.25, 0.25, 0.25], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
T2 = np.array([[0, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0], [0.9, 0, 0, 0.1]])
def connection(n):
# map Y1 -> 2^(2^AP)
if n == 1:
return set((('s1', ), )) # { {s1} }
elif n == 3:
return set((('s2', ), )) # { {s2} }
else:
return set(((), ), ) # { { } }
system = MDP([T1, T2])
formula = '( ( F s1 ) & ( F s2 ) )'
dfsa, init, final, _ = formula_to_mdp(formula)
prod = system.product(dfsa, connection)
V, _ = prod.solve_reach(accept=lambda s: s[1] in final)
np.testing.assert_almost_equal(V[0][:, 0], [0.5, 0, 0, 0.5], decimal=4)
def test_prune():
T0 = np.array([[0.5, 0.05, 0.45], [0, 1, 0], [0, 0.01, 0.99]])
mdp = MDP([T0])
mdp.prune(thresh=0.06)
TprunedA = np.array([[0.5 / 0.95, 0, 0.45 / 0.95], [0, 1, 0], [0, 0, 1]])
Tpruned = mdp.T(0).todense()
print Tpruned
np.testing.assert_almost_equal(Tpruned, TprunedA)
def test_reach_constrained():
T0 = np.array([[0, 0.5, 0.5], [0, 1, 0], [0, 0, 1]])
mdp = MDP([T0])
Vacc = np.array([0, 1, 1])
Vcon = np.array([0, 1, 0])
vlist, plist = mdp.solve_reach_constrained(Vacc, Vcon, 0.4, 4)
np.testing.assert_almost_equal(vlist[0], [1, 1, 0])
vlist, plist = mdp.solve_reach_constrained(Vacc, Vcon, 0.6, 4)
np.testing.assert_almost_equal(vlist[0], [0, 1, 0])
def test_connection():
T0 = np.array([[0.5, 0.5], [0, 1.]])
T1 = np.array([[0.2, 0.8], [1, 0]])
T2 = np.array([[0.2, 0.8], [1, 0]])
mdp1 = MDP([T0, T1, T2])
mdp2 = MDP([T0, T1, T2])
mdp3 = MDP([T0, T1, T2])
prod = mdp1.product(mdp2, connection=lambda n: set([1]))
prod = prod.product(mdp3, connection=lambda n: set([2]))
pT = np.kron(np.kron(T0, T1), T2)
np.testing.assert_almost_equal(prod.T(0).todense(), pT)
def formula_to_mdp(formula):
'''convert a co-safe LTL formula to a DFSA represented as a
special case of an MPD'''
fsa = Fsa()
fsa.from_formula(formula)
fsa.add_trap_state()
# mapping state -> state index
N = len(fsa.g)
dict_fromstate = dict([(sstate, s) for s, sstate in enumerate(sorted(fsa.g.nodes()))])
inputs = set.union(*[attr['input'] for _,_,attr in fsa.g.edges(data=True)])
M = len(inputs)
assert(inputs == set(range(M)))
T = [np.zeros((N, N)) for m in range(M)]
for (s1, s2, attr) in fsa.g.edges(data=True):
for u in attr['input']:
T[u][dict_fromstate[s1], dict_fromstate[s2]] = 1
mdp = MDP(T, input_name='ap', input_fcn=fsa.bitmap_of_props,
output_name='mu')
init_states = set(map(lambda state: dict_fromstate[state], [state for (state, key) in fsa.init.items() if key == 1]))
final_states = set(map(lambda state: dict_fromstate[state], fsa.final))
return mdp, init_states, final_states, fsa.props
def abstract(self):
def move(s0, dim, direction):
# which state is in direction along dim from s0?
midx_s0 = idx_to_midx(s0, self.n_list)
midx_s1 = list(midx_s0)
midx_s1[dim] += direction
midx_s1[dim] = max(0, midx_s1[dim])
midx_s1[dim] = min(self.n_list[dim]-1, midx_s1[dim])
return midx_to_idx(midx_s1, self.n_list)
T_list = [sp.eye(self.N)]
for d in range(len(self.n_list)):
vals = np.ones(self.N)
n0 = np.arange(self.N)
npl = [move(s0, d, 1) for s0 in np.arange(self.N) ]
npm = [move(s0, d, -1) for s0 in np.arange(self.N) ]
T_pm = sp.coo_matrix((vals, (n0, npm)), shape=(self.N, self.N))
T_list.append(T_pm)
T_pl = sp.coo_matrix((vals, (n0, npl)), shape=(self.N, self.N))
T_list.append(T_pl)
self.T_list = T_list
output_fcn = lambda s: self.x_low + self.eta_list/2 + self.eta_list * idx_to_midx(s, self.n_list)
return MDP(T_list, output_name='xc', output_fcn=output_fcn)
def environment_belief_model2(p0, levels, name):
pmm = levels[0]
pm = levels[1]
pp = levels[2]
ppp = levels[3]
if p0 == 0:
# no dynamics
return MDP([np.array([1])],
input_fcn=lambda n: 0,
output_fcn=lambda s: 0)
elif p0 == 1:
# no dynamics
return MDP([np.array([1])],
input_fcn=lambda n: 0,
output_fcn=lambda s: 1)
else:
Tnone = np.eye(7)
Tweak = np.array([[1, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0], [0, 0, (1 - p0), 0, p0, 0, 0],
[0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 1]])
Tstrong = np.array([[1, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0],
[0, (1 - pm), 0, 0, 0, pm, 0],
[0, (1 - p0), 0, 0, 0, p0, 0],
[0, (1 - pp), 0, 0, 0, pp, 0],
[0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 1]])
Texact = np.array([[1, 0, 0, 0, 0, 0, 0],
[(1 - pmm), 0, 0, 0, 0, 0, pmm],
[(1 - pm), 0, 0, 0, 0, 0, pm],
[(1 - p0), 0, 0, 0, 0, 0, p0],
[(1 - pp), 0, 0, 0, 0, 0, pp],
[(1 - ppp), 0, 0, 0, 0, 0, ppp],
[0, 0, 0, 0, 0, 0, 1]])
def output_fcn(s):
return [0, pmm, pm, p0, pp, ppp, 1][s]
return MDP([Tnone, Tweak, Tstrong],
input_name=name + '_u',
output_name=name + '_b',
output_fcn=output_fcn)
def test_parallel():
T0 = np.eye(3)
T1 = np.array([[0, 0.5, 0.5], [0, 1, 0], [0, 0, 1]])
def output_fcn(n):
if n == 0:
return 'init' # label unknown
if n == 1:
return 'safe' # can traverse region
if n == 2:
return 'unsafe' # can not traverse region
map1 = MDP([T0, T1],
input_fcn=lambda meas1: meas1,
input_name='meas1',
output_fcn=output_fcn,
output_name='label1')
map2 = MDP([T0, T1],
input_fcn=lambda meas2: meas2,
input_name='meas2',
output_fcn=output_fcn,
output_name='label2')
map3 = MDP([T0, T1],
input_fcn=lambda meas3: meas3,
input_name='meas3',
output_fcn=output_fcn,
output_name='label3')
prod = ParallelMDP([map1, map2, map3])
for i1 in range(2):
for i2 in range(2):
for i3 in range(2):
np.testing.assert_equal(
(i1, i2, i3), prod.local_controls(prod.input(
(i1, i2, i3))))
for k in range(8):
T = prod.T(k).todense()
for i in range(27):
for j in range(27):
np.testing.assert_almost_equal(T[i, j], prod.t(k, i, j))
def test_reach_finitetime():
T0 = np.array([[0.9, 0, 0.1], [0, 1, 0], [0, 0, 1]])
T1 = np.array([[0, 0.5, 0.5], [0, 1, 0], [0, 0, 1]])
mdp = MDP([T0, T1])
accept = lambda n: n == 2
vlist, plist = mdp.solve_reach(accept, horizon=3)
np.testing.assert_almost_equal(vlist[0][0], 0.1 + 0.9 * 0.1 + 0.9**2 * 0.5)
np.testing.assert_almost_equal(vlist[1][0], 0.1 + 0.9 * 0.5)
np.testing.assert_almost_equal(vlist[2][0], 0.5)
np.testing.assert_almost_equal(plist[0][0], 0)
np.testing.assert_almost_equal(plist[1][0], 0)
np.testing.assert_almost_equal(plist[2][0], 1)
def test_mdp_dfsa():
def output(n1):
if n1 == 2:
return 1
else:
return 0
T0 = np.array([[0.5, 0.25, 0.25], [0, 1, 0], [0, 0, 1]])
mdp = MDP([T0], output_fcn=output)
T1 = np.array([[1, 0], [0, 1]])
T2 = np.array([[0, 1], [0, 1]])
fsa = MDP([T1, T2])
connection = lambda n1: set([n1])
prod = mdp.product(fsa, connection)
V, _ = prod.solve_reach(accept=lambda y: y[1] == 1)
np.testing.assert_almost_equal(V[0], [[0.5, 1], [0, 1], [1, 1]], decimal=4)
def test_mdp_dfsa_nondet():
def connection(n1):
if n1 == 2:
return set([1])
elif n1 == 1:
return set([1, 0])
else:
return set([0])
T0 = np.array([[0.5, 0.25, 0.25], [0, 1, 0], [0, 0, 1]])
mdp = MDP([T0])
T1 = np.array([[1, 0], [0, 1]])
T2 = np.array([[0, 1], [0, 1]])
fsa = MDP([T1, T2])
prod = mdp.product(fsa, connection)
V, _ = prod.solve_reach(accept=lambda y: y[1] == 1)
np.testing.assert_almost_equal(V[0], [[0.5, 1], [0, 1], [1, 1]], decimal=4)
def test_reach_constrained2():
T0 = np.array([[0, 0.98, 0, 0, 0.01, 0.01], [0, 0, 0.98, 0, 0.01, 0.01],
[0, 0, 0, 0.98, 0.01, 0.01], [0, 0, 0, 0.98, 0.01, 0.01],
[0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]])
T1 = np.array([[0, 0.5, 0, 0, 0.5, 0], [0, 0, 0.5, 0, 0.5, 0],
[0, 0, 0, 0, 1, 0], [0, 0, 0, 0.99, 0.01, 0],
[0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]])
mdp = MDP([T1, T0])
Vacc = np.array([0, 0, 0, 1, 1, 0])
Vcon = np.array([0, 0, 0, 1, 0, 0])
vlist, plist = mdp.solve_reach_constrained(Vacc, Vcon, 0.94, 4)
np.testing.assert_array_less([0.95, 0.95, 0.95, 0.999, -0.0001, -0.0001],
vlist[0])
vlist, plist = mdp.solve_reach_constrained(Vacc, Vcon, 0.95, 4)
np.testing.assert_array_less(vlist[0],
[0.01, 1.01, 1.01, 1.01, 0.01, 0.01])
def test_ltl_synth2():
T1 = np.array([[0.25, 0.25, 0.25, 0.25], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
T2 = np.array([[0, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0], [0.9, 0, 0, 0.1]])
def connection(n):
# map Y1 -> 2^(2^AP)
if n == 1:
return set((('s1', ), )) # { {s1} }
elif n == 3:
return set((('s2', ), )) # { {s2} }
else:
return set(((), ), ) # { { } }
system = MDP([T1, T2])
formula = '( ( F s1 ) & ( F s2 ) )'
pol = solve_ltl_cosafe(system, formula, connection)
def __init__(self, lti_syst_orig, d, un=3, verbose = True, Accuracy=True):
self.s_finite = None
# Diagonalize
lti_syst = lti_syst_orig.normalize()
self.T2x = lti_syst.T2x
## Unpack LTI
d = d.flatten()
A = lti_syst.a
B = lti_syst.b
C = lti_syst.c
U = lti_syst.setU()
# check that Bw is a diagonal
assert np.sum(np.absolute(lti_syst.W)) - np.trace(np.absolute(lti_syst.W)) == 0
vars = np.diag(lti_syst.W)
X = lti_syst.setX()
n = lti_syst.dim
rad = np.linalg.norm(d, 2)
lx, ux = pc.bounding_box(lti_syst.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((lti_syst.dim, 1)), np.array([[-1, 1]]))))
M_min, K_min, eps_min = eps_err(lti_syst, Dist)
else:
M_min, K_min, eps_min = None, None, None
# 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 input
urep = tuple()
lu, uu = pc.bounding_box(lti_syst.U) # lower and upperbounds over all dimensions
for i, low in enumerate(lu):
urep += (np.linspace(lu[i], uu[i], un, endpoint=True),)
un_list = [len(ur) for ur in urep]
un = prod(un_list) # number of finite states
sn_list = [len(sr) for sr in srep]
sn = prod(sn_list) # number of finite states
transition_list = [np.zeros((sn+1, sn+1)) for m in range(un)]
# extract all transitions
for u_index, u in enumerate(itertools.product(*urep)):
P = tuple()
for s, sstate in enumerate(itertools.product(*srep)):
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_list[u_index] = p_local
self.srep = srep
self.urep = urep
self.sedge = sedge
self.mdp = MDP(transition_list, input_name='u_d', output_name='(s, x_d)',
output_fcn=lambda s: (s, self.s_to_x(s)))
self.M = M_min
self.K = K_min
self.eps = eps_min
self.K_refine = np.zeros((len(self.urep), len(self.srep)))
self.output_cst = dict([(s, sstate) for s, sstate in enumerate(itertools.product(*srep))])
self.input_cst = dict([(u, uvalue) for u, uvalue in enumerate(itertools.product(*urep))])
