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 pick_initial_state(ctrl, disc_dynamics):
"""Construct initial discrete and continuous state
for `qinit == '\A \A'`.
"""
# pick initial discrete state
init_edges = ctrl.edges('Sinit', data=True)
u, v, edge_data = next(iter(init_edges))
assert u == 'Sinit', u
d_init = edge_data
# pick initial continuous state
s0_part = edge_data['loc']
init_poly = disc_dynamics.ppp.regions[s0_part].list_poly[0]
x_init = pick_point_in_polytope(init_poly)
s0_part_ = find_controller.find_discrete_state(x_init, disc_dynamics.ppp)
assert s0_part == s0_part_, (s0_part, s0_part_)
return d_init, x_init
# 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,
start=s0_part,
end=disc_dynamics.ppp2ts.index(dum['loc']),
ord=1,
mid_weight=5)
for ind in range(N):
s_now = np.dot(
# 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']
print('s0_part: ', s0_part)
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]]
x_init = 1.5
y_init = 1
print("x: ", x_init)
N = disc_dynamics.disc_params['N']
s0_part = find_controller.find_discrete_state([x_init, y_init],
disc_dynamics.ppp)
ctrl = synth.determinize_machine_init(ctrl, {'loc': s0_part})
(s, dum) = ctrl.reaction(1, {'park': randParkSignal[0]})
start = s0_part
end = disc_dynamics.ppp2ts.index(dum['loc'])
print("start: ", start)
print("end: ", end)
u = find_controller.get_input(x0=np.array([x_init, y_init]),
ssys=sys_dyn,
abstraction=disc_dynamics,
start=start,
end=end,
ord=1,
mid_weight=5)
# 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,
start=s0_part,
end=disc_dynamics.ppp2ts.index(dum['loc']),
ord=1,
mid_weight=5)
for ind in range(N):
s_now = np.dot(
def simulate2d(
env_inputs, sys_dyn, ctrl, disc_dynamics, T,
qinit='\E \A',
d_init=None,
x_init=None,
show_traj=True):
r"""Simulation for systems with two-dimensional continuous dynamics.
The first item in `env_inputs` is used as the initial environment
discrete state, if `qinit == '\E \A' or qinit == \A \E'`.
This item is used to find the initial transition in `ctrl`.
The initial continuous state is selected within the initial polytope
of the partition, if `qinit \in ('\E \E', '\E \A', '\A \E')`.
The initial continuous state is `x_init` if `qinit == '\A \A'`,
and is asserted to correspond to `d_init['loc']`.
The initial discrete state is `d_init` if `qinit == '\A \A'`,
and is used to find the initial transition in `ctrl`.
@param env_inputs: `list` of `dict`, with length `T + 1`
@param sys_dyn: system dynamics
@type sys_dyn: `tulip.hybrid.LtiSysDyn`
@type ctrl: `tulip.transys.machines.MealyMachine`
@type disc_dynamics: `tulip.abstract.discretization.AbstractPwa`
@param T: number of simulation steps
@param qinit: quantification of initial conditions
@param d_init: initial discrete state,
given when `qinit == '\A \A'`
@param x_init: initial continuous state,
given when `qinit == '\A \A'`
@param show_traj: plot trajectory
"""
N = disc_dynamics.disc_params['N']
# initialization:
# pick initial continuous state consistent with
# initial controller state (discrete)
if qinit == '\E \A' or qinit == '\A \E':
# pick an initial discrete system state given the#
# initial discrete environment state
(nd, out) = ctrl.reaction('Sinit', env_inputs[0])
init_edges = ctrl.edges('Sinit', data=True)
for u, v, edge_data in init_edges:
assert u == 'Sinit', u
if v == nd:
break
assert v == nd, (v, nd)
s0_part = edge_data['loc']
assert s0_part == out['loc'], (s0_part, out)
elif qinit == '\E \E':
# pick an initial discrete state
init_edges = ctrl.edges('Sinit', data=True)
u, v, edge_data = next(iter(init_edges))
assert u == 'Sinit', u
s0_part = edge_data['loc']
nd = v
elif qinit == '\A \A':
assert d_init is not None
assert x_init is not None
s0_part = find_controller.find_discrete_state(
x_init, disc_dynamics.ppp)
assert s0_part == d_init['loc'], (s0_part, d_init)
# find the machine node with `d_init` as discrete state
init_edges = ctrl.edges('Sinit', data=True)
for u, v, edge_data in init_edges:
assert u == 'Sinit', u
if edge_data == d_init:
break
assert edge_data == d_init, (edge_data, d_init)
nd = v
# initialize continuous state for the cases that the initial
# continuous state is not given
if qinit in ('\E \E', '\E \A', '\A \E'):
# find initial polytope
init_poly = disc_dynamics.ppp.regions[s0_part].list_poly[0]
# disc_dynamics.ppp[s0_part][0]
#
# pick an initial continuous state
x_init = pick_point_in_polytope(init_poly)
# assert equal
s0_part_ = find_controller.find_discrete_state(
x_init, disc_dynamics.ppp)
assert s0_part == s0_part_, (s0_part, s0_part_)
x = [x_init[0]]
y = [x_init[1]]
for i in range(T):
(nd, out) = ctrl.reaction(nd, env_inputs[i + 1])
x0 = np.array([x[i * N], y[i * N]])
start = s0_part
end = disc_dynamics.ppp2ts.index(out['loc'])
u = find_controller.get_input(
x0=x0,
ssys=sys_dyn,
abstraction=disc_dynamics,
start=start,
end=end,
ord=1,
mid_weight=5)
# continuous trajectory for interval `i`
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])
point = [x[-1], y[-1]]
s0_part = find_controller.find_discrete_state(
point, disc_dynamics.ppp)
s0_loc = disc_dynamics.ppp2ts[s0_part]
assert s0_loc == out['loc'], (s0_loc, out['loc'])
print('outputs:\n {out}\n'.format(out=out))
if show_traj:
from matplotlib import pyplot as plt
plt.plot(x, label='x')
plt.plot(y, '--', label='y')
plt.xlabel('time')
plt.legend()
plt.grid()
plt.show()