def test_from_continuous_and_symbolic(self):
# generate random matrices
h = .01
for i in range(10):
n = np.random.randint(1, 10)
m = np.random.randint(1, 10)
A = np.random.rand(n,n)
B = np.random.rand(n,m)
# test .from_continuous(), explicit euler vs zoh
convergence = False
while not convergence:
S_ee = LinearSystem.from_continuous(A, B, h, 'explicit_euler')
S_zoh = LinearSystem.from_continuous(A, B, h, 'zero_order_hold')
convergence = np.allclose(S_ee.A, S_zoh.A) and np.allclose(S_ee.B, S_zoh.B)
if not convergence:
h /= 10.
self.assertTrue(convergence)
# test .from_symbolic()
x = sp.Matrix([sp.Symbol('x'+str(i), real=True) for i in range(n)])
u = sp.Matrix([sp.Symbol('u'+str(i), real=True) for i in range(m)])
x_next = sp.Matrix(A)*x + sp.Matrix(B)*u
S = LinearSystem.from_symbolic(x, u, x_next)
np.testing.assert_array_almost_equal(S.A, A)
np.testing.assert_array_almost_equal(S.B, B)
# test .from_symbolic_continuous()
S = LinearSystem.from_symbolic_continuous(x, u, x_next, h, 'explicit_euler')
np.testing.assert_array_almost_equal(S.A, S_ee.A)
np.testing.assert_array_almost_equal(S.B, S_ee.B)
S = LinearSystem.from_symbolic_continuous(x, u, x_next, h, 'zero_order_hold')
np.testing.assert_array_almost_equal(S.A, S_zoh.A)
np.testing.assert_array_almost_equal(S.B, S_zoh.B)
# negative time step
self.assertRaises(ValueError, LinearSystem.from_continuous, A, B, -.1, 'explicit_euler')
self.assertRaises(ValueError, LinearSystem.from_symbolic_continuous, x, u, x_next, -.1, 'explicit_euler')
self.assertRaises(ValueError, LinearSystem.from_continuous, A, B, -.1, 'zero_order_hold')
self.assertRaises(ValueError, LinearSystem.from_symbolic_continuous, x, u, x_next, -.1, 'zero_order_hold')
# wrong input size
x_wrong = sp.Matrix([sp.Symbol('x_wrong'+str(i), real=True) for i in range(n+1)])
u_wrong = sp.Matrix([sp.Symbol('u_wrong'+str(i), real=True) for i in range(m+1)])
self.assertRaises(TypeError, LinearSystem.from_symbolic, x_wrong, u, x_next)
self.assertRaises(TypeError, LinearSystem.from_symbolic, x, u_wrong, x_next)
self.assertRaises(TypeError, LinearSystem.from_symbolic_continuous, x_wrong, u, x_next, h, 'explicit_euler')
self.assertRaises(TypeError, LinearSystem.from_symbolic_continuous, x, u_wrong, x_next, h, 'explicit_euler')
self.assertRaises(TypeError, LinearSystem.from_symbolic_continuous, x_wrong, u, x_next, h, 'zero_order_hold')
self.assertRaises(TypeError, LinearSystem.from_symbolic_continuous, x, u_wrong, x_next, h, 'zero_order_hold')
# from continuous wrong method
self.assertRaises(ValueError, LinearSystem.from_continuous, A, B, h, 'gatto')
self.assertRaises(ValueError, LinearSystem.from_symbolic_continuous, x, u, x_next, h, 'gatto')
def test_controllable(self):
# double integrator (controllable)
A = np.array([[0., 1.],[0., 0.]])
B = np.array([[0.],[1.]])
h = .1
S = LinearSystem.from_continuous(A, B, h)
self.assertTrue(S.controllable)
# make it uncontrollable
B = np.array([[1.],[0.]])
S = LinearSystem.from_continuous(A, B, h)
self.assertFalse(S.controllable)
def test_from_continuous(self):
# test from continuous
for i in range(10):
n = np.random.randint(1, 10)
m = np.random.randint(1, 10)
A = np.random.rand(n, n)
B = np.random.rand(n, m)
# reduce discretization step until the two method are almost equivalent
h = .01
convergence = False
while not convergence:
S_ee = LinearSystem.from_continuous(A, B, h, 'explicit_euler')
S_zoh = LinearSystem.from_continuous(A, B, h,
'zero_order_hold')
convergence = np.allclose(S_ee.A, S_zoh.A) and np.allclose(
S_ee.B, S_zoh.B)
if not convergence:
h /= 10.
self.assertTrue(convergence)
self.assertRaises(ValueError, LinearSystem.from_continuous, A, B, h,
'gatto')
def test_solve_dare_and_simulate_closed_loop(self):
np.random.seed(1)
# uncontrollable system
A = np.array([[0., 1.], [0., 0.]])
B = np.array([[1.], [0.]])
h = .1
S = LinearSystem.from_continuous(A, B, h)
self.assertRaises(ValueError, S.solve_dare, np.eye(2), np.eye(1))
# test lqr on random systems
for i in range(100):
n = np.random.randint(5, 10)
m = np.random.randint(1, n - 1)
controllable = False
while not controllable:
A = np.random.rand(n, n)
B = np.random.rand(n, m)
S = LinearSystem(A, B)
controllable = S.controllable
Q = np.eye(n)
R = np.eye(m)
P, K = S.solve_dare(Q, R)
self.assertTrue(np.min(np.linalg.eig(P)[0]) > 0.)
# simulate in closed-loop and check that x' P x is a Lyapunov function
N = 10
x0 = np.random.rand(n)
x_list = S.simulate_closed_loop(x0, N, K)
V_list = [x.dot(P).dot(x) for x in x_list]
dV_list = [
V_list[i] - V_list[i + 1] for i in range(len(V_list) - 1)
]
self.assertTrue(min(dV_list) > 0.)
# simulate in closed-loop and check that 1/2 x' P x is exactly the cost to go
A_cl = A + B.dot(K)
x0 = np.random.rand(n)
infinite_horizon_V = .5 * x0.dot(P).dot(x0)
finite_horizon_V = 0.
max_iter = 1000
t = 0
while not np.isclose(infinite_horizon_V, finite_horizon_V):
finite_horizon_V += .5 * (x0.dot(Q).dot(x0) +
(K.dot(x0)).dot(R).dot(K).dot(x0))
x0 = A_cl.dot(x0)
t += 1
if t == max_iter:
self.assertTrue(False)
def test_mcais(self):
"""
Tests only if the function macais() il called correctly.
For the tests of mcais() see the class TestMCAIS.
"""
# undamped pendulum linearized around the unstable equilibrium
A = np.array([[0., 1.], [1., 0.]])
B = np.array([[0.], [1.]])
h = .1
S = LinearSystem.from_continuous(A, B, h)
K = S.solve_dare(np.eye(2), np.eye(1))[1]
d_min = - np.ones(3)
d_max = - d_min
D = Polyhedron.from_bounds(d_min, d_max)
O_inf = S.mcais(K, D)
self.assertTrue(O_inf.contains(np.zeros(2)))
from pympc.plot import plot_input_sequence, plot_state_trajectory, plot_state_space_trajectory
# We consider the linear inverted pendulum from the example $\texttt{maximal_constraint_admissible_set.ipynb}$
# parameters
m = 1.
l = 1.
g = 10.
# continuous time-dynamics
A = np.array([[0., 1.], [g / l, 0.]])
B = np.array([[0.], [1. / (m * l**2.)]])
# discrete-time system
h = .1
method = 'zero_order_hold'
S = LinearSystem.from_continuous(A, B, h, method)
# We set the numeric values for the MPC controller
N = 6
Q = np.array([[1., 0.], [0., 1.]])
R = np.array([[1.]])
# As a terminal controller, the obvious chioce is the infinite-horizon LQR, whose cost-to-go can be derived solving the Discrete Algebraic Ricccati Equation (DARE)
# \begin{equation}
# P = (A+BK)^T P (A+BK) + Q + K^T R K
# \end{equation}
# where $K = -(B^T P B + R)^{-1} B^T P A$ is the optimal feedback gain $u = K x$
P, K = S.solve_dare(Q, R)
def test_feedforward_feedback_and_get_mpqp(self):
# numeric parameters
m = 1.
l = 1.
g = 10.
k = 100.
d = .1
h = .01
# discretization method
method = 'explicit_euler'
# dynamics n.1
A1 = np.array([[0., 1.], [g / l, 0.]])
B1 = np.array([[0.], [1 / (m * l**2.)]])
S1 = LinearSystem.from_continuous(A1, B1, h, method)
# dynamics n.2
A2 = np.array([[0., 1.], [g / l - k / m, 0.]])
B2 = B1
c2 = np.array([[0.], [k * d / (m * l)]])
S2 = AffineSystem.from_continuous(A2, B2, c2, h, method)
# list of dynamics
S_list = [S1, S2]
# state domain n.1
x1_min = np.array([[-2. * d / l], [-1.5]])
x1_max = np.array([[d / l], [1.5]])
X1 = Polyhedron.from_bounds(x1_min, x1_max)
# state domain n.2
x2_min = np.array([[d / l], [-1.5]])
x2_max = np.array([[2. * d / l], [1.5]])
X2 = Polyhedron.from_bounds(x2_min, x2_max)
# input domain
u_min = np.array([[-4.]])
u_max = np.array([[4.]])
U = Polyhedron.from_bounds(u_min, u_max)
# domains
D1 = X1.cartesian_product(U)
D2 = X2.cartesian_product(U)
D_list = [D1, D2]
# pwa system
S = PieceWiseAffineSystem(S_list, D_list)
# controller parameters
N = 20
Q = np.eye(S.nx)
R = np.eye(S.nu)
# terminal set and cost
P, K = S1.solve_dare(Q, R)
X_N = S1.mcais(K, D1)
# hybrid MPC controller
controller = HybridModelPredictiveController(S, N, Q, R, P, X_N)
# compare with lqr
x0 = np.array([[.0], [.6]])
self.assertTrue(X_N.contains(x0))
V_lqr = .5 * x0.T.dot(P).dot(x0)[0, 0]
x_lqr = [x0]
u_lqr = []
for t in range(N):
u_lqr.append(K.dot(x_lqr[t]))
x_lqr.append((S1.A + S1.B.dot(K)).dot(x_lqr[t]))
u_hmpc, x_hmpc, ms_hmpc, V_hmpc = controller.feedforward(x0)
np.testing.assert_array_almost_equal(np.vstack((u_lqr)),
np.vstack((u_hmpc)))
np.testing.assert_array_almost_equal(np.vstack((x_lqr)),
np.vstack((x_hmpc)))
self.assertAlmostEqual(V_lqr, V_hmpc)
self.assertTrue(all([m == 0 for m in ms_hmpc]))
np.testing.assert_array_almost_equal(u_hmpc[0],
controller.feedback(x0))
# compare with linear mpc
x0 = np.array([[.0], [.8]])
self.assertFalse(X_N.contains(x0))
linear_controller = ModelPredictiveController(S1, N, Q, R, P, D1, X_N)
u_lmpc, V_lmpc = linear_controller.feedforward(x0)
x_lmpc = S1.simulate(x0, u_lmpc)
u_hmpc, x_hmpc, ms_hmpc, V_hmpc = controller.feedforward(x0)
np.testing.assert_array_almost_equal(np.vstack((u_lmpc)),
np.vstack((u_hmpc)))
np.testing.assert_array_almost_equal(np.vstack((x_lmpc)),
np.vstack((x_hmpc)))
self.assertAlmostEqual(V_lmpc, V_hmpc)
self.assertTrue(all([m == 0 for m in ms_hmpc]))
np.testing.assert_array_almost_equal(u_hmpc[0],
controller.feedback(x0))
# test get mpqp
mpqp = controller.get_mpqp(ms_hmpc)
sol = mpqp.solve(x0)
np.testing.assert_array_almost_equal(np.vstack((u_lmpc)),
sol['argmin'])
self.assertAlmostEqual(V_lmpc, sol['min'])
# with change of the mode sequence
x0 = np.array([[.08], [.8]])
u_hmpc, x_hmpc, ms_hmpc, V_hmpc = controller.feedforward(x0)
self.assertTrue(sum(ms_hmpc) >= 1)
mpqp = controller.get_mpqp(ms_hmpc)
sol = mpqp.solve(x0)
np.testing.assert_array_almost_equal(np.vstack((u_hmpc)),
sol['argmin'])
self.assertAlmostEqual(V_hmpc, sol['min'])