def test_intialization(self):
np.random.seed(1)
# different number of systems and domains
A = np.ones((3, 3))
B = np.ones((3, 2))
c = np.ones(3)
S = AffineSystem(A, B, c)
affine_systems = [S] * 5
F = np.ones((9, 5))
g = np.ones(9)
D = Polyhedron(F, g)
domains = [D] * 4
self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems,
domains)
# incopatible number of states in affine systems
domains += [D, D]
A = np.ones((2, 2))
B = np.ones((2, 2))
c = np.ones(2)
affine_systems.append(AffineSystem(A, B, c))
self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems,
domains)
# incopatible number of inputs in affine systems
del affine_systems[-1]
A = np.ones((3, 3))
B = np.ones((3, 1))
c = np.ones(3)
affine_systems.append(AffineSystem(A, B, c))
self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems,
domains)
# different dimensinality of the domains and the systems
del affine_systems[-1]
affine_systems += [S, S]
F = np.ones((9, 4))
g = np.ones(9)
domains.append(Polyhedron(F, g))
self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems,
domains)
# different dimensinality of the domains and the systems
F = np.ones((9, 4))
g = np.ones(9)
D = Polyhedron(F, g)
domains = [D] * 7
self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems,
domains)
def test_is_well_posed(self):
# domains
D0 = Polyhedron.from_bounds(-np.ones(3), np.ones(3))
D1 = Polyhedron.from_bounds(np.zeros(3), 2.*np.ones(3))
domains = [D0, D1]
# pwa system
A = np.ones((2,2))
B = np.ones((2,1))
c = np.ones(2)
S = AffineSystem(A, B, c)
affine_systems = [S]*2
S_pwa = PieceWiseAffineSystem(affine_systems, domains)
# check if well posed
self.assertFalse(S_pwa.is_well_posed())
# make it well posed
D1.add_lower_bound(np.ones(3))
D2 = Polyhedron.from_bounds(2.*np.ones(3), 3.*np.ones(3))
domains = [D0, D1, D2]
affine_systems = [S]*3
S_pwa = PieceWiseAffineSystem(affine_systems, domains)
self.assertTrue(S_pwa.is_well_posed())
def _condense_program(self):
"""
Generates and stores the optimal control problem in condensed form.
Returns
----------
instance of MultiParametricQuadraticProgram
Condensed mpQP.
"""
# create fake PWA system and use PWA condenser
c = np.zeros((self.S.nx, 1))
S = AffineSystem(self.S.A, self.S.B, c)
S = PieceWiseAffineSystem([S], [self.D])
mode_sequence = [0] * self.N
return condense_optimal_control_problem(S, self.Q, self.R, self.P,
self.X_N, mode_sequence)
def test_condense_and_simulate(self):
# random systems
for i in range(10):
n = np.random.randint(1, 10)
m = np.random.randint(1, 10)
N = np.random.randint(10, 50)
x0 = np.random.rand(n)
u = [np.random.rand(m) / 10. for j in range(N)]
A = np.random.rand(n, n) / 10.
B = np.random.rand(n, m) / 10.
c = np.random.rand(n) / 10.
S = AffineSystem(A, B, c)
# simulate vs condense
x = S.simulate(x0, u)
A_bar, B_bar, c_bar = S.condense(N)
np.testing.assert_array_almost_equal(
np.concatenate(x),
A_bar.dot(x0) + B_bar.dot(np.concatenate(u)) + c_bar)
def test_condense_and_simulate_and_get_mode(self):
np.random.seed(1)
# test with random systems
for i in range(10):
# test dimensions
n = np.random.randint(1, 10)
m = np.random.randint(1, 10)
N = np.random.randint(10, 50)
# initial state
x0 = np.random.rand(n)
# initialize loop variables
x_t = x0
x = x0
u = np.zeros(0)
u_list = []
affine_systems = []
domains = []
# simulate for N steps
for t in range(N):
# generate random dynamics
A_t = np.random.rand(n,n)/10.
B_t = np.random.rand(n,m)/10.
c_t = np.random.rand(n)/10.
# simulate with random input
u_t = np.random.rand(m)/10.
u = np.concatenate((u, u_t))
u_list.append(u_t)
x_tp1 = A_t.dot(x_t) + B_t.dot(u_t) + c_t
x = np.concatenate((x, x_tp1))
# create a domain that contains x and u (it has to be super-tight so that the pwa system is well posed!)
D = Polyhedron.from_bounds(
np.concatenate((x_t/1.01, u_t/1.01)),
np.concatenate((x_t*1.01, u_t*1.01))
)
domains.append(D)
affine_systems.append(AffineSystem(A_t, B_t, c_t))
# reset state
x_t = x_tp1
# construct pwa system
S = PieceWiseAffineSystem(affine_systems, domains)
# test condense
mode_sequence = range(N)
A_bar, B_bar, c_bar = S.condense(mode_sequence)
np.testing.assert_array_almost_equal(
x,
A_bar.dot(x0) + B_bar.dot(u) + c_bar
)
# test simulate
x_list, mode_sequence = S.simulate(x0, u_list)
np.testing.assert_array_almost_equal(x, np.concatenate(x_list))
# test get mode
for t in range(N):
self.assertTrue(S.get_mode(x_list[t], u_list[t]) == t)