def find_discrete_state(x0, part):
"""Return index identifying the discrete state
to which the continuous state x0 belongs to.
Notes
=====
1. If there are overlapping partitions
(i.e., x0 belongs to more than one discrete state),
then return the first discrete state ID
@param x0: initial continuous state
@type x0: numpy 1darray
@param part: state space partition
@type part: L{PropPreservingPartition}
@return: if C{x0} belongs to some
discrete state in C{part},
then return the index of that state
Otherwise return None, i.e., in case
C{x0} does not belong to any discrete state.
@rtype: int
"""
for (i, region) in enumerate(part):
if pc.is_inside(region, x0):
return i
return None
def find_discrete_state(x0, part):
"""Return index identifying the discrete state
to which the continuous state x0 belongs to.
Notes
=====
1. If there are overlapping partitions
(i.e., x0 belongs to more than one discrete state),
then return the first discrete state ID
@param x0: initial continuous state
@type x0: numpy 1darray
@param part: state space partition
@type part: L{PropPreservingPartition}
@return: if C{x0} belongs to some
discrete state in C{part},
then return the index of that state
Otherwise return None, i.e., in case
C{x0} does not belong to any discrete state.
@rtype: int
"""
for (i, region) in enumerate(part):
if pc.is_inside(region, x0):
return i
return None
def is_inside_test(self):
box = [[0.0, 1.0], [0.0, 2.0]]
p = pc.Polytope.from_box(box)
point = np.array([0.0, 1.0])
abs_tol = 0.01
assert pc.is_inside(p, point)
assert pc.is_inside(p, point, abs_tol)
region = pc.Region([p])
assert pc.is_inside(region, point)
assert pc.is_inside(region, point, abs_tol)
point = np.array([2.0, 0.0])
assert not pc.is_inside(p, point)
assert not pc.is_inside(p, point, abs_tol)
region = pc.Region([p])
assert not pc.is_inside(region, point)
assert not pc.is_inside(region, point, abs_tol)
abs_tol = 1.2
assert pc.is_inside(p, point, abs_tol)
assert pc.is_inside(region, point, abs_tol)
def is_seq_inside(x0, u_seq, ssys, P0, P1):
"""Checks if the plant remains inside P0 for time t = 1, ... N-1
and that the plant reaches P1 for time t = N.
Used to test a computed input sequence.
No disturbance is taken into account.
@param x0: initial point for execution
@param u_seq: (N x m) array where row k is input for t = k
@param ssys: dynamics
@type ssys: L{LtiSysDyn}
@param P0: C{Polytope} where we want x(k) to remain for k = 1, ... N-1
@return: C{True} if x(k) \in P0 for k = 1, .. N-1 and x(N) \in P1.
C{False} otherwise
"""
N = u_seq.shape[0]
x = x0.reshape(x0.size, 1)
A = ssys.A
B = ssys.B
if len(ssys.K) == 0:
K = np.zeros(x.shape)
else:
K = ssys.K
inside = True
for i in range(N - 1):
u = u_seq[i, :].reshape(u_seq[i, :].size, 1)
x = A.dot(x) + B.dot(u) + K
if not pc.is_inside(P0, x):
inside = False
un_1 = u_seq[N - 1, :].reshape(u_seq[N - 1, :].size, 1)
xn = A.dot(x) + B.dot(un_1) + K
if not pc.is_inside(P1, xn):
inside = False
return inside
def is_seq_inside(x0, u_seq, ssys, P0, P1):
"""Checks if the plant remains inside P0 for time t = 1, ... N-1
and that the plant reaches P1 for time t = N.
Used to test a computed input sequence.
No disturbance is taken into account.
@param x0: initial point for execution
@param u_seq: (N x m) array where row k is input for t = k
@param ssys: dynamics
@type ssys: L{LtiSysDyn}
@param P0: C{Polytope} where we want x(k) to remain for k = 1, ... N-1
@return: C{True} if x(k) \in P0 for k = 1, .. N-1 and x(N) \in P1.
C{False} otherwise
"""
N = u_seq.shape[0]
x = x0.reshape(x0.size, 1)
A = ssys.A
B = ssys.B
if len(ssys.K) == 0:
K = np.zeros(x.shape)
else:
K = ssys.K
inside = True
for i in range(N - 1):
u = u_seq[i, :].reshape(u_seq[i, :].size, 1)
x = A.dot(x) + B.dot(u) + K
if not pc.is_inside(P0, x):
inside = False
un_1 = u_seq[N - 1, :].reshape(u_seq[N - 1, :].size, 1)
xn = A.dot(x) + B.dot(un_1) + K
if not pc.is_inside(P1, xn):
inside = False
return inside
print('Discrete time: k = ' +str(i) )
print('\t u[' +str(i) +"]' = " +str(u.T) )
print('\t x[' +str(i) +"]' = " +str(x.T) +'\n')
print('completed continuous transition iteration')
return x
x0 = np.array([0.5, 0.6])
start = find_discrete_state(x0, disc_dynamics.ppp)
end = 14
start_poly = disc_dynamics.ppp.regions[start]
end_poly = disc_dynamics.ppp.regions[end]
if not is_inside(start_poly, x0):
raise Exception('x0 \\notin start_poly')
start_state = start
end_state = end
post = disc_dynamics.ts.states.post(start_state)
print(post)
if not end_state in post:
raise Exception('end \\notin post(start)')
u_seq = get_input(x0, sys_dyn, disc_dynamics,
start, end)
print('Computed input sequence: u = ')
print(u_seq)
print('Discrete time: k = ' + str(i))
print('\t u[' + str(i) + "]' = " + str(u.T))
print('\t x[' + str(i) + "]' = " + str(x.T) + '\n')
print('completed continuous transition iteration')
return x
x0 = np.array([0.5, 0.6])
start = find_discrete_state(x0, disc_dynamics.ppp)
end = 14
start_poly = disc_dynamics.ppp.regions[start]
end_poly = disc_dynamics.ppp.regions[end]
if not is_inside(start_poly, x0):
raise Exception('x0 \\notin start_poly')
start_state = 's' + str(start)
end_state = 's' + str(end)
post = disc_dynamics.ts.states.post(start_state)
print(post)
if not end_state in post:
raise Exception('end \\notin post(start)')
u_seq = get_input(x0, sys_dyn, disc_dynamics, start, end)
print('Computed input sequence: u = ')
print(u_seq)
x = integrate(sys_dyn, x0, u_seq)
