def _get_bigM_dynamics(self, tol=1.e-8):
"""
Computes all the smallMs and bigMs for the dynamics of the PWA system.
When the system is in mode j, n_x smallMs and n_x bigMs are needed to drop the equations of motion for the mode i.
With s number of modes, m_dynamics and M_dynamics are two lists containing s lists, each list then constains s bigM vector of size n_x.
m_dynamics[j][i] and M_dynamics[j][i] are used to drop the equations of motion of the mode i when the system is in mode j.
"""
self._M_dynamics = []
self._m_dynamics = []
for i in range(self.sys.n_sys):
M_i = []
m_i = []
lhs_i = np.hstack(
(self.sys.affine_systems[i].A, self.sys.affine_systems[i].B))
rhs_i = self.sys.affine_systems[i].c
for domain_j in self.sys.domains:
M_ij = []
m_ij = []
for k in range(lhs_i.shape[0]):
sol = linear_program(-lhs_i[k, :], domain_j.lhs_min,
domain_j.rhs_min)
M_ijk = (-sol.min + rhs_i[k])[0]
M_ij.append(M_ijk)
sol = linear_program(lhs_i[k, :], domain_j.lhs_min,
domain_j.rhs_min)
m_ijk = (sol.min + rhs_i[k])[0]
m_ij.append(m_ijk)
M_ij = np.reshape(M_ij, (len(M_ij), 1))
m_ij = np.reshape(m_ij, (len(m_ij), 1))
M_i.append(M_ij)
m_i.append(np.array(m_ij))
self._M_dynamics.append(M_i)
self._m_dynamics.append(m_i)
return
def compute_big_M_dynamics(self):
self.big_M_dynamics = []
self.small_m_dynamics = []
for i in range(self.sys.n_sys):
big_M_i = []
small_m_i = []
lhs_i = np.hstack((self.sys.affine_systems[i].A, self.sys.affine_systems[i].B))
rhs_i = self.sys.affine_systems[i].c
for j in range(self.sys.n_sys):
big_M_ij = []
small_m_ij = []
lhs_j = linalg.block_diag(self.sys.state_domains[j].lhs_min, self.sys.input_domains[j].lhs_min)
rhs_j = np.vstack((self.sys.state_domains[j].rhs_min, self.sys.input_domains[j].rhs_min))
for k in range(lhs_i.shape[0]):
big_M_ijk = - linear_program(-lhs_i[k,:], lhs_j, rhs_j)[1] + rhs_i[k]
big_M_ij.append(big_M_ijk[0])
small_m_ijk = linear_program(lhs_i[k,:], lhs_j, rhs_j)[1] + rhs_i[k]
small_m_ij.append(small_m_ijk[0])
big_M_ij = np.reshape(big_M_ij, (len(big_M_ij), 1))
small_m_ij = np.reshape(small_m_ij, (len(small_m_ij), 1))
big_M_i.append(big_M_ij)
small_m_i.append(np.array(small_m_ij))
self.big_M_dynamics.append(big_M_i)
self.small_m_dynamics.append(small_m_i)
return
def solve(self, x0, u_length=None):
x0 = np.reshape(x0, (x0.shape[0], 1))
sol = linear_program(self.f, self.A, self.B.dot(x0) + self.c)
u_star = sol.argmin[0:self.F_u.shape[1]]
if u_length is not None:
if not float(u_star.shape[0] / u_length).is_integer():
raise ValueError('Uncoherent dimension of the input u_length.')
u_star = [
u_star[i * u_length:(i + 1) * u_length, :]
for i in range(u_star.shape[0] / u_length)
]
return u_star, sol.min
def moas(A, X):
"""
Returns the maximum output admissible set (see Gilbert, Tan - Linear Systems with State and Control Constraints, The Theory and Application of Maximal Output Admissible Sets) for a non-actuated linear system with state constraints (the output vector is supposed to be the entire state of the system, i.e. y=x and C=I).
INPUTS:
A: state transition matrix
X: constraint polytope X.lhs * x <= X.rhs
OUTPUTS:
moas: maximum output admissible set (instatiated as a polytope)
"""
# ensure that the system is stable (otherwise the algorithm doesn't converge)
eig_max = np.max(np.absolute(np.linalg.eig(A)[0]))
if eig_max > 1:
raise ValueError('Cannot compute MOAS for unstable systems')
# Gilber and Tan algorithm
[n_constraints, n_variables] = X.lhs_min.shape
t = 0
convergence = False
while convergence == False:
# cost function gradients for all i
J = X.lhs_min.dot(np.linalg.matrix_power(A, t + 1))
# constraints to each LP
cons_lhs = np.vstack([
X.lhs_min.dot(np.linalg.matrix_power(A, k))
for k in range(0, t + 1)
])
cons_rhs = np.vstack([X.rhs_min for k in range(0, t + 1)])
# list of all minima
J_sol = []
for i in range(0, n_constraints):
J_sol_i = linear_program(np.reshape(-J[i, :], (n_variables, 1)),
cons_lhs, cons_rhs)[1]
J_sol.append(-J_sol_i - X.rhs_min[i])
# convergence check
if np.max(J_sol) < 0:
convergence = True
else:
t += 1
# define polytope
moas = Polytope(cons_lhs, cons_rhs)
moas.assemble()
return moas
def compute_big_M_domains(self):
self.big_M_domains = []
for i in range(self.sys.n_sys):
big_M_i = []
lhs_i = linalg.block_diag(self.sys.state_domains[i].lhs_min, self.sys.input_domains[i].lhs_min)
rhs_i = np.vstack((self.sys.state_domains[i].rhs_min, self.sys.input_domains[i].rhs_min))
for j in range(self.sys.n_sys):
big_M_ij = []
if i != j:
lhs_j = linalg.block_diag(self.sys.state_domains[j].lhs_min, self.sys.input_domains[j].lhs_min)
rhs_j = np.vstack((self.sys.state_domains[j].rhs_min, self.sys.input_domains[j].rhs_min))
for k in range(lhs_i.shape[0]):
big_M_ijk = - linear_program(-lhs_i[k,:], lhs_j, rhs_j)[1] - rhs_i[k]
big_M_ij.append(big_M_ijk[0])
big_M_ij = np.reshape(big_M_ij, (len(big_M_ij), 1))
big_M_i.append(big_M_ij)
self.big_M_domains.append(big_M_i)
return
def _get_bigM_domains(self, tol=1.e-8):
"""
Computes all the bigMs for the domains of the PWA system.
When the system is in mode j, n_i bigMs are needed to drop each one the n_i inequalities of the polytopic domain for the mode i.
With s number of modes, M_domains is a list containing s lists, each list then constains s bigM vector of size n_i.
M_domains[j][i] is used to drop the inequalities of the domain i when the system is in mode j.
"""
self._M_domains = []
for i, domain_i in enumerate(self.sys.domains):
M_i = []
for j, domain_j in enumerate(self.sys.domains):
M_ij = []
if i != j:
for k in range(domain_i.lhs_min.shape[0]):
sol = linear_program(-domain_i.lhs_min[k, :],
domain_j.lhs_min,
domain_j.rhs_min)
M_ijk = (-sol.min - domain_i.rhs_min[k])[0]
M_ij.append(M_ijk)
M_ij = np.reshape(M_ij, (len(M_ij), 1))
M_i.append(M_ij)
self._M_domains.append(M_i)
return
def solve(self, x0):
x0 = np.reshape(x0, (x0.shape[0], 1))
u_star, V_star = linear_program(self.f, self.A, self.B.dot(x0)+self.c)
u_star = u_star[0:self.F_u.shape[1]]
return u_star, V_star
