def test_correct_lti_construction(self):
LTI1 = hybrid.LtiSysDyn(A=self.A1,
B=self.B1,
Uset=self.Uset,
domain=self.poly1,
time_semantics='discrete',
timestep=None)
LTI2 = hybrid.LtiSysDyn(A=self.A2,
B=self.B2,
Uset=self.Uset,
domain=self.poly2,
time_semantics='sampled',
timestep=.1)
LTI3 = hybrid.LtiSysDyn(A=self.A2,
B=self.B2,
Uset=self.Uset,
domain=self.poly1)
LTI4 = hybrid.LtiSysDyn(A=self.A1,
B=self.B2,
Uset=self.Uset,
domain=self.poly2)
assert (LTI1.time_semantics == 'discrete')
assert (LTI2.time_semantics == 'sampled')
assert (LTI1.timestep is None)
assert (LTI2.timestep == .1)
assert (LTI3.time_semantics is None)
assert (LTI3.timestep is None)
assert (LTI4.time_semantics is None)
assert (LTI4.timestep is None)
def setUp(self):
self.A1 = np.eye(2)
self.A2 = np.array([[0, 1], [0, 0]])
self.B1 = np.array([[0] ,[1]])
self.B2 = np.array([[1], [0]])
self.poly1 = pc.Polytope.from_box([[0, 1], [0, 1]])
self.poly2 = pc.Polytope.from_box([[1, 2], [0, 1]])
self.total_box = pc.Region(list_poly=[self.poly1, self.poly2])
self.Uset = pc.Polytope.from_box([[0, 1]])
self.env_labels = ('hi', 'hello')
self.sys_labels = ('mode1',)
self.disc_domain_size = (2, 1)
self.LTI1 = hybrid.LtiSysDyn(A=self.A1, B=self.B1,
Uset=self.Uset, domain=self.poly1,
time_semantics='sampled', timestep=.1)
self.LTI2 = hybrid.LtiSysDyn(A=self.A2, B=self.B2,
Uset=self.Uset, domain=self.poly2,
time_semantics='sampled', timestep=.1)
self.LTI3 = hybrid.LtiSysDyn(A=self.A1, B=self.B1,
Uset=self.Uset, domain=self.poly2,
time_semantics='sampled', timestep=.1)
self.LTI4 = hybrid.LtiSysDyn(A=self.A2, B=self.B2,
Uset=self.Uset, domain=self.poly1,
time_semantics='sampled', timestep=.1)
self.PWA1 = hybrid.PwaSysDyn(list_subsys=[self.LTI1, self.LTI2],
domain=self.total_box,
time_semantics='sampled', timestep=.1)
self.PWA2 = hybrid.PwaSysDyn(list_subsys=[self.LTI3, self.LTI4],
domain=self.total_box,
time_semantics='sampled', timestep=.1)
self.dynamics1 = {(self.env_labels[0], self.sys_labels[0]): self.PWA1,
(self.env_labels[1], self.sys_labels[0]): self.PWA2}
def test_correct_pwa_construction(self):
# Putting pwa together successfully, without time overwrite
LTI1 = hybrid.LtiSysDyn(A=self.A1,
B=self.B1,
Uset=self.Uset,
domain=self.poly1,
time_semantics='sampled',
timestep=.1)
LTI2 = hybrid.LtiSysDyn(A=self.A2,
B=self.B2,
Uset=self.Uset,
domain=self.poly2,
time_semantics='sampled',
timestep=.1)
LTI3 = hybrid.LtiSysDyn(A=self.A2,
B=self.B2,
Uset=self.Uset,
domain=self.poly1)
LTI4 = hybrid.LtiSysDyn(A=self.A1,
B=self.B2,
Uset=self.Uset,
domain=self.poly2)
PWA1 = hybrid.PwaSysDyn(list_subsys=[LTI3, LTI4],
domain=self.total_box,
overwrite_time=False)
# Putting pwa together successfully, with time overwrite
PWA2 = hybrid.PwaSysDyn(list_subsys=[LTI1, LTI2],
domain=self.total_box,
time_semantics='sampled',
timestep=.1,
overwrite_time=True)
def test_pwa_invalid_semantics(self):
LTI1 = hybrid.LtiSysDyn(A=self.A1, B=self.B1,
Uset=self.Uset, domain=self.poly1,
time_semantics='sampled', timestep=.1)
LTI2 = hybrid.LtiSysDyn(A=self.A2, B=self.B2,
Uset=self.Uset, domain=self.poly2,
time_semantics='sampled', timestep=.1)
PWA = hybrid.PwaSysDyn(list_subsys=[LTI1, LTI2], domain=self.total_box,
time_semantics='hello')
def test_pwa_difftseman_from_subsys(self):
"""LtiSysDyn subsystems time semantics do not match that of PwaSysDyn"""
LTI1 = hybrid.LtiSysDyn(A=self.A1, B=self.B1,
Uset=self.Uset, domain=self.poly1,
time_semantics='sampled', timestep=.1)
LTI2 = hybrid.LtiSysDyn(A=self.A2, B=self.B2,
Uset=self.Uset, domain=self.poly2,
time_semantics='sampled', timestep=.1)
PWA = hybrid.PwaSysDyn(list_subsys=[LTI1, LTI2], domain=self.total_box,
time_semantics='discrete', overwrite_time=False)
def test_pwa_difftstep_among_subsys(self):
"""Different timesteps among LtiSysDyn subsystems of PwaSysDyn"""
LTI1 = hybrid.LtiSysDyn(A=self.A1, B=self.B1,
Uset=self.Uset, domain=self.poly1,
time_semantics='sampled', timestep=.1)
LTI2 = hybrid.LtiSysDyn(A=self.A2, B=self.B2,
Uset=self.Uset, domain=self.poly2,
time_semantics='sampled', timestep=.2)
PWA = hybrid.PwaSysDyn(list_subsys=[LTI1, LTI2], domain=self.total_box,
time_semantics='sampled', timestep=.1,
overwrite_time=False)
def _import_ltisys(node):
A = _import_xml(node.findall('A')[0])
B = _import_xml(node.findall('B')[0])
E = _import_xml(node.findall('E')[0])
K = _import_xml(node.findall('K')[0])
Uset = node.findall('Uset')
Wset = node.findall('Wset')
domain = node.findall('domain')
if not Uset:
Uset = None
else:
Uset = _import_xml(Uset[0])
if not Wset:
Wset = None
else:
Wset = _import_xml(Wset[0])
if not domain:
domain = None
else:
domain = _import_xml(domain[0])
return hybrid.LtiSysDyn(A=A,
B=B,
E=E,
K=K,
Uset=Uset,
Wset=Wset,
domain=domain)
def test_lti_invalid_semantics(self):
LTI = hybrid.LtiSysDyn(A=self.A1,
B=self.B1,
Uset=self.Uset,
domain=self.poly1,
time_semantics='hello',
timestep=.1)
def test_nonpositive_timestep(self):
LTI = hybrid.LtiSysDyn(A=self.A1,
B=self.B1,
Uset=self.Uset,
domain=self.poly1,
time_semantics='sampled',
timestep=0)
def drifting_dynamics(dom):
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([[1.0], [0.0]])
sys = hybrid.LtiSysDyn(A, B, None, K, U, None, dom)
return sys
def test_timestep_wrong_type(self):
LTI = hybrid.LtiSysDyn(A=self.A1,
B=self.B1,
Uset=self.Uset,
domain=self.poly1,
time_semantics='sampled',
timestep='.1')
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 test_disctime_errtstep(self):
"""Discrete time semantics yet given timestep"""
LTI = hybrid.LtiSysDyn(A=self.A1,
B=self.B1,
Uset=self.Uset,
domain=self.poly1,
time_semantics='discrete',
timestep=.1)
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:
input_bound = 1.0
cont_state_space = box2poly([[-1, 1], [-1, 1]])
A = np.array([[0.5, 1.0], [0.75, -1.0]])
B = np.array([[1, 0.], [0., 1]])
U = input_bound * np.array([[-1., 1.], [-1., 1.]])
U = box2poly(U)
sys_dyn = hybrid.LtiSysDyn(A, B, None, None, U, None, cont_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]])
cont_partition = prop2part(cont_state_space, cont_props)
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 subsys1():
dom = pc.box2poly([[0., 3.], [0., 2.]])
A = np.eye(2)
B = np.eye(2)
U = pc.box2poly([[0., 0.], [-1., 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 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
np.array([0, 0, 2 * fuel_consumption]))
## Dynamics
A = np.eye(2)
B = np.array([[-1], [1]])
E = np.array([[0], [1]])
K1 = np.array([[0.], [-fuel_consumption]])
K2 = np.array([[refill_rate], [-fuel_consumption]])
U1 = pc.Polytope(np.array([[1, 0, 0], [-1, 0, 0], [1, -1, 0]]),
np.array([input_ub, -input_lb, 0]))
U2 = pc.Polytope(np.array([[1, 0, 0], [-1, 0, 0], [1, -1, 0]]),
np.array([input_ub, -input_lb, refill_rate]))
W = pc.Polytope(np.array([[1], [-1]]), np.array([disturbance, disturbance]))
# Normal operation dynamics
cont_dyn_normal = hybrid.LtiSysDyn(A, B, E, K1, U1, W, domain=cont_ss)
# Aerial refueling mode dynamics
cont_dyn_refuel = hybrid.LtiSysDyn(A, B, E, K2, U2, W, domain=cont_ss)
## Switched Dynamics
env_modes = ('normal', 'refuel')
sys_modes = ('fly', )
pwa_normal = hybrid.PwaSysDyn([cont_dyn_normal], domain=cont_ss)
pwa_refuel = hybrid.PwaSysDyn([cont_dyn_refuel], domain=cont_ss)
dynamics_dict = {('normal', 'fly'): pwa_normal, ('refuel', 'fly'): pwa_refuel}
switched_dynamics = hybrid.SwitchedSysDyn(cts_ss=cont_ss,
disc_domain_size=(len(env_modes),
# 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)
# @dynamics_section_end@
# @partition_section@
# 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 the proposition preserving partition of the continuous state space
cont_partition = prop2part(cont_state_space, cont_props)
plot_partition(cont_partition) if show else None
# @partition_section_end@
# @discretize_section@
# Given dynamics & proposition-preserving partition, find feasible transitions
def load_lti(A, B, K, domainA, domainB, UsetA, UsetB):
domain = polytope.Polytope(domainA, domainB)
Uset = polytope.Polytope(UsetA, UsetB)
lti = hybrid.LtiSysDyn(A=A, B=B, K=K, domain=domain, Uset=Uset)
return lti
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
A = np.array([[1., 0, 2., 0], [0, 1., 0, 2], [0, 0, 0.5, 0], [0, 0, 0, 0.5]])
B = np.array([[0, 0], [0, 0], [5, 0], [0, 5]])
E = np.array([[1., 0, 0, 0], [0, 1., 0, 0], [0, 0, 1., 0], [0, 0, 0, 1.]])
# $x^+=Ax+Bu+E W$
# Size of the sets
X = box2poly([[0, 100.], [0, 100.], [-5, 5.], [-5, 5.]])
U = box2poly(input_bound * np.array([[-1, 1.], [-1, 1.]]))
W = box2poly(disturbance_bound *
np.array([[-0.5, 0.5], [-0.5, 0.5], [-0.1, 0.1], [-0.1, 0.1]]))
print(
"----------------------------------\n Define system\n----------------------------------"
)
# Intermezzo polytope tutorial
# https://github.com/tulip-control/polytope/blob/master/doc/tutorial.md
sys_dyn = hybrid.LtiSysDyn(A, B, E, None, U, W, X)
print(str(sys_dyn))
print(
"----------------------------------\n Define labelling \n----------------------------------"
)
cprops = {}
cprops['inA'] = box2poly([[0, 10], [45, 55], [-5, 5], [-5, 5]])
cprops['inB'] = box2poly([[90, 100], [45, 55], [-5, 5], [-5, 5]])
cprops['inG'] = box2poly([[45, 55], [45, 55], [-5, 5], [-5, 5]])
cprops['inObj1'] = box2poly([[15, 35], [30, 70], [-5, 5], [-5, 5]])
cprops['inObj2'] = box2poly([[65, 85], [30, 70], [-5, 5], [-5, 5]])
#
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)
pwa_heat = hybrid.PwaSysDyn(list_subsys=[sdyn_heat], domain=cont_state_space)
pwa_cool = hybrid.PwaSysDyn(list_subsys=[sdyn_cool], domain=cont_state_space)
pwa_on = hybrid.PwaSysDyn(list_subsys=[sdyn_on], domain=cont_state_space)
#print pwa_off
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 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)
