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 test_cartesian_product(self):
# simple case
n = 2
x_min = -np.ones((n, 1))
x_max = -x_min
p1 = Polyhedron.from_bounds(x_min, x_max)
C = np.ones((1, n))
d = np.zeros((1, 1))
p1.add_equality(C, d)
p2 = p1.cartesian_product(p1)
# derive the cartesian product
x_min = -np.ones((2 * n, 1))
x_max = -x_min
p3 = Polyhedron.from_bounds(x_min, x_max)
C = block_diag(*[np.ones((1, n))] * 2)
d = np.zeros((2, 1))
p3.add_equality(C, d)
# compare results
self.assertTrue(
same_rows(np.hstack((p2.A, p2.b)), np.hstack((p3.A, p3.b))))
self.assertTrue(
same_rows(np.hstack((p2.C, p2.d)), np.hstack((p3.C, p3.d))))
def test_intersection(self):
# first polyhedron
x1_min = -np.ones((2, 1))
x1_max = -x1_min
p1 = Polyhedron.from_bounds(x1_min, x1_max)
# second polyhedron
x2_min = np.zeros((2, 1))
x2_max = 2. * np.ones((2, 1))
p2 = Polyhedron.from_bounds(x2_min, x2_max)
# intersection
p3 = p1.intersection(p2)
p3.remove_redundant_inequalities()
p4 = Polyhedron.from_bounds(x2_min, x1_max)
self.assertTrue(
same_rows(np.hstack((p3.A, p3.b)), np.hstack((p4.A, p4.b))))
# add equalities
C1 = np.array([[1., 0.]])
d1 = np.array([-.5])
p1.add_equality(C1, d1)
p3 = p1.intersection(p2)
self.assertTrue(p3.empty)
def verify_controller(A, B, K, x_min, x_max, u_min, u_max, dimensions=[0, 1]):
"""
x_min = np.array([[-1.],[-1.]])
x_max = np.array([[ 1.],[ 1.]])
u_min = np.array([[-15.]])
u_max = np.array([[ 15.]])
"""
S = LinearSystem(A, B)
X = Polyhedron.from_bounds(x_min, x_max)
U = Polyhedron.from_bounds(u_min, u_max)
D = X.cartesian_product(U)
start = time.time()
# I added this try-catch -Greg
try:
O_inf = S.mcais(K, D)
except ValueError:
# K is not stable for this environment - therefore the safe space is
# empty, so we return an empty polyhedron (Added -Greg)
O_inf = Polyhedron.from_bounds(x_max, x_min)
end = time.time()
print("mcais execution time: {} secs".format(end - start))
#if (len(dimensions) >= 2):
# D.plot(dimensions, label=r'$D$', facecolor='b')
# O_inf.plot(dimensions, label=r'$\mathcal{O}_{\infty}$', facecolor='r')
# plt.legend()
# plt.show()
return O_inf
def test_vertices(self):
# basic eample
A = np.array([[-1, 0.],[0., -1.],[2., 1.],[-0.5, 1.]])
b = np.array([0.,0.,4.,2.])
p = Polyhedron(A,b)
u_list = [
np.array([0.,0.]),
np.array([2.,0.]),
np.array([0.,2.]),
np.array([.8,2.4])
]
self.assertTrue(same_vectors(p.vertices, u_list))
# 1d example
A = np.array([[-1],[1.]])
b = np.array([1.,1.])
p = Polyhedron(A,b)
u_list = [
np.array([-1.]),
np.array([1.])
]
self.assertTrue(same_vectors(p.vertices, u_list))
# unbounded
x_min = np.zeros(2)
p = Polyhedron.from_lower_bound(x_min)
self.assertTrue(p.vertices is None)
# lower dimensional (because of the inequalities)
x_max = np.array([1.,0.])
p.add_upper_bound(x_max)
self.assertTrue(p.vertices is None)
# empty
x_max = - np.ones(2)
p.add_upper_bound(x_max)
self.assertTrue(p.vertices is None)
# 3d case
x_min = - np.ones(3)
x_max = - x_min
p = Polyhedron.from_bounds(x_min, x_max)
u_list = [np.array(v) for v in product([1., -1.], repeat=3)]
self.assertTrue(same_vectors(p.vertices, u_list))
# 3d case with equalities
x_min = np.array([-1.,-2.])
x_max = - x_min
p = Polyhedron.from_bounds(x_min, x_max, [0,1], 3)
C = np.array([[1., 0., -1.]])
d = np.zeros(1)
p.add_equality(C, d)
u_list = [
np.array([1.,2.,1.]),
np.array([-1.,2.,-1.]),
np.array([1.,-2.,1.]),
np.array([-1.,-2.,-1.])
]
self.assertTrue(same_vectors(p.vertices, u_list))
def test_bounded(self):
# bounded (empty), easy (ker(A) empty)
x_min = np.ones((2, 1))
x_max = -np.ones((2, 1))
p = Polyhedron.from_bounds(x_min, x_max)
self.assertTrue(p.bounded)
# bounded (empty), tricky (ker(A) not empty)
x0_min = np.array([[1.]])
x0_max = np.array([[-1.]])
p = Polyhedron.from_bounds(x0_min, x0_max, [0], 2)
self.assertTrue(p.bounded)
# bounded easy
x_min = 1. * np.ones((2, 1))
x_max = 2. * np.ones((2, 1))
p = Polyhedron.from_bounds(x_min, x_max)
self.assertTrue(p.bounded)
# unbounded, halfspace
x0_min = np.array([[0.]])
p = Polyhedron.from_lower_bound(x0_min, [0], 2)
self.assertFalse(p.bounded)
# unbounded, positive orthant
x1_min = x0_min
p.add_lower_bound(x1_min, [1])
self.assertFalse(p.bounded)
# unbounded: parallel inequalities, slice of positive orthant
x0_max = np.array([[1.]])
p.add_upper_bound(x0_max, [0])
self.assertFalse(p.bounded)
# unbounded lower dimensional, line in the positive orthant
x0_min = x0_max
p.add_lower_bound(x0_min, [0])
self.assertFalse(p.bounded)
# bounded lower dimensional, segment in the positive orthant
x1_max = np.array([[1000.]])
p.add_upper_bound(x1_max, [1])
self.assertTrue(p.bounded)
# with equalities, 3d case
x_min = np.array([[-1.], [-2.]])
x_max = -x_min
p = Polyhedron.from_bounds(x_min, x_max, [0, 1], 3)
C = np.array([[1., 0., -1.]])
d = np.zeros((1, 1))
p.add_equality(C, d)
self.assertTrue(p.bounded)
def test_chebyshev(self):
# simple 2d problem
x_min = np.zeros((2, 1))
x_max = 2. * np.ones((2, 1))
p = Polyhedron.from_bounds(x_min, x_max)
self.assertAlmostEqual(p.radius, 1.)
np.testing.assert_array_almost_equal(p.center, np.ones((2, 1)))
# add nasty inequality
A = np.zeros((1, 2))
b = np.ones((1, 1))
p.add_inequality(A, b)
self.assertAlmostEqual(p.radius, 1.)
np.testing.assert_array_almost_equal(p.center, np.ones((2, 1)))
# add equality
C = np.ones((1, 2))
d = np.array([[3.]])
p.add_equality(C, d)
self.assertAlmostEqual(p.radius, np.sqrt(2.) / 2.)
np.testing.assert_array_almost_equal(p.center, 1.5 * np.ones((2, 1)))
# negative radius
x_min = np.ones((2, 1))
x_max = -np.ones((2, 1))
p = Polyhedron.from_bounds(x_min, x_max)
self.assertAlmostEqual(p.radius, -1.)
np.testing.assert_array_almost_equal(p.center, np.zeros((2, 1)))
# unbounded
p = Polyhedron.from_lower_bound(x_min)
self.assertTrue(p.radius is None)
self.assertTrue(p.center is None)
# bounded very difficult
x0_max = np.array([[2.]])
p.add_upper_bound(x0_max, [1])
self.assertAlmostEqual(p.radius, .5)
self.assertAlmostEqual(p.center[0, 0], 1.5)
# 3d case
x_min = np.array([[-1.], [-2.]])
x_max = -x_min
p = Polyhedron.from_bounds(x_min, x_max, [0, 1], 3)
C = np.array([[1., 0., -1.]])
d = np.zeros((1, 1))
p.add_equality(C, d)
self.assertAlmostEqual(p.radius, np.sqrt(2.))
self.assertAlmostEqual(p.center[0, 0], 0.)
self.assertAlmostEqual(p.center[2, 0], 0.)
self.assertTrue(np.abs(p.center[1, 0]) < 2. - p.radius + 1.e-6)
def test_is_included_in(self):
# inner polyhdron
x_min = 1. * np.ones((2, 1))
x_max = 2. * np.ones((2, 1))
p1 = Polyhedron.from_bounds(x_min, x_max)
# outer polyhdron
x_min = .5 * np.ones((2, 1))
x_max = 2.5 * np.ones((2, 1))
p2 = Polyhedron.from_bounds(x_min, x_max)
# check inclusions
self.assertTrue(p1.is_included_in(p1))
self.assertTrue(p1.is_included_in(p2))
self.assertFalse(p2.is_included_in(p1))
# polytope that intesects p1
x_min = .5 * np.ones((2, 1))
x_max = 1.5 * np.ones((2, 1))
p2 = Polyhedron.from_bounds(x_min, x_max)
self.assertFalse(p1.is_included_in(p2))
self.assertFalse(p2.is_included_in(p1))
# polytope that includes p1, with two equal facets
x_min = .5 * np.ones((2, 1))
x_max = 2. * np.ones((2, 1))
p2 = Polyhedron.from_bounds(x_min, x_max)
self.assertTrue(p1.is_included_in(p2))
self.assertFalse(p2.is_included_in(p1))
# polytope that does not include p1, with two equal facets
x_min = 1.5 * np.ones((2, 1))
p2.add_lower_bound(x_min)
self.assertFalse(p1.is_included_in(p2))
self.assertTrue(p2.is_included_in(p1))
# with equalities
C = np.ones((1, 2))
d = np.array([[3.5]])
p1.add_equality(C, d)
self.assertTrue(p1.is_included_in(p2))
self.assertFalse(p2.is_included_in(p1))
p2.add_equality(C, d)
self.assertTrue(p1.is_included_in(p2))
self.assertTrue(p2.is_included_in(p1))
x1_max = np.array([[1.75]])
p2.add_upper_bound(x1_max, [1])
self.assertFalse(p1.is_included_in(p2))
self.assertTrue(p2.is_included_in(p1))
def test_mcais(self):
np.random.seed(1)
# domain
nx = 2
x_min = - np.ones(nx)
x_max = - x_min
X = Polyhedron.from_bounds(x_min, x_max)
# stable dynamics
for i in range(10):
stable = False
while not stable:
A = np.random.rand(nx, nx)
stable = np.max(np.absolute(np.linalg.eig(A)[0])) < 1.
# get mcais
O_inf = mcais(A, X)
# generate random initial conditions
for j in range(100):
x = 3.*np.random.rand(nx) - 1.5
# if inside stays inside X, if outside sooner or later will leave X
if O_inf.contains(x):
while np.linalg.norm(x) > 0.001:
x = A.dot(x)
self.assertTrue(X.contains(x))
else:
while X.contains(x):
x = A.dot(x)
if np.linalg.norm(x) < 0.0001:
self.assertTrue(False)
pass
def test_orthogonal_projection(self):
# (more tests on projections are in geometry/test_orthogonal_projection.py)
# unbounded polyhedron
x_min = -np.ones((3, 1))
p = Polyhedron.from_lower_bound(x_min)
self.assertRaises(ValueError, p.project_to, range(2))
# simple 3d onto 2d
p.add_upper_bound(-x_min)
E = np.vstack((np.eye(2), -np.eye(2)))
f = np.ones((4, 1))
vertices = [
np.array(v).reshape(2, 1) for v in product([1., -1.], repeat=2)
]
proj = p.project_to(range(2))
proj.remove_redundant_inequalities()
self.assertTrue(
same_rows(np.hstack((E, f)), np.hstack((proj.A, proj.b))))
self.assertTrue(same_vectors(vertices, proj.vertices))
# lower dimensional
C = np.array([[1., 1., 0.]])
d = np.zeros((1, 1))
p.add_equality(C, d)
self.assertRaises(ValueError, p.project_to, range(2))
# lower dimensional
x_min = -np.ones((3, 1))
x_max = 2. * x_min
p = Polyhedron.from_bounds(x_min, x_max)
self.assertRaises(ValueError, p.project_to, range(2))
def verify_controller_via_discretization(Acl, h, x_min, x_max):
#discretize the system for efficient verification
X = Polyhedron.from_bounds(x_min, x_max)
O_inf = mcais(la.expm(Acl * h), X, verbose=False)
# dimensions=[0,2]
# X.plot(dimensions, label=r'$D$', facecolor='b')
# O_inf.plot(dimensions, label=r'$\mathcal{O}_{\infty}$', facecolor='r')
# plt.legend()
# plt.show()
return O_inf
def test_convex_hull_method(self):
np.random.seed(3)
# cube from n-dimensions to n-1-dimensions
for n in range(2, 6):
x_min = - np.ones(n)
x_max = - x_min
p = Polyhedron.from_bounds(x_min, x_max)
E = np.vstack((
np.eye(n-1),
- np.eye(n-1)
))
f = np.ones(2*(n-1))
vertices = [np.array(v) for v in product([1., -1.], repeat=n-1)]
E_chm, f_chm, vertices_chm = convex_hull_method(p.A, p.b, range(n-1))
p_chm = Polyhedron(E_chm, f_chm)
p_chm.remove_redundant_inequalities()
self.assertTrue(same_rows(
np.column_stack((E, f)),
np.column_stack((p_chm.A, p_chm.b))
))
self.assertTrue(same_vectors(vertices, vertices_chm))
# test with random polytopes wiith m facets
m = 10
# dimension of the higher dimensional polytope
for n in range(3, 6):
# dimension of the lower dimensional polytope
for n_proj in range(2, n):
# higher dimensional polytope
A = np.random.randn(m, n)
b = np.random.rand(m)
p = Polyhedron(A, b)
# if not empty or unbounded
if p.vertices is not None:
points = [v[:n_proj] for v in p.vertices]
p = Polyhedron.from_convex_hull(points)
# check half spaces
p.remove_redundant_inequalities()
E_chm, f_chm, vertices_chm = convex_hull_method(A, b, range(n_proj))
p_chm = Polyhedron(E_chm, f_chm)
p_chm.remove_redundant_inequalities()
self.assertTrue(same_rows(
np.column_stack((p.A, p.b)),
np.column_stack((p_chm.A, p_chm.b))
))
# check vertices
self.assertTrue(same_vectors(p.vertices, vertices_chm))
def test_delete_attributes(self):
# initialize polyhedron
x_min = - np.ones(2)
x_max = - x_min
p = Polyhedron.from_bounds(x_min, x_max)
# compute alle the property attributes
self.assertFalse(p.empty)
self.assertTrue(p.bounded)
self.assertAlmostEqual(p.radius, 1.)
np.testing.assert_array_almost_equal(
p.center,
np.zeros(2)
)
self.assertTrue(same_vectors(
p.vertices,
[np.array(v) for v in product([1., -1.], repeat=2)]
))
# add inequality
A = np.eye(2)
b = .8*np.ones(2)
p.add_inequality(A, b)
self.assertFalse(p.empty)
self.assertTrue(p.bounded)
self.assertAlmostEqual(p.radius, .9)
np.testing.assert_array_almost_equal(
p.center,
-.1*np.ones(2)
)
self.assertTrue(same_vectors(
p.vertices,
[np.array(v) for v in product([.8, -1.], repeat=2)]
))
# add equality
C = np.array([[1., 1.]])
d = np.zeros(1)
p.add_equality(C, d)
self.assertFalse(p.empty)
self.assertTrue(p.bounded)
self.assertAlmostEqual(p.radius, .8*np.sqrt(2.))
np.testing.assert_array_almost_equal(
p.center,
np.zeros(2)
)
self.assertTrue(same_vectors(
p.vertices,
[np.array([-.8,.8]), np.array([.8,-.8])]
))
def verify_controller(A, B, K, x_min, x_max, u_min, u_max, dimensions=[0, 1]):
"""
x_min = np.array([[-1.],[-1.]])
x_max = np.array([[ 1.],[ 1.]])
u_min = np.array([[-15.]])
u_max = np.array([[ 15.]])
"""
S = LinearSystem(A, B)
X = Polyhedron.from_bounds(x_min, x_max)
U = Polyhedron.from_bounds(u_min, u_max)
D = X.cartesian_product(U)
start = time.time()
O_inf = S.mcais(K, D)
end = time.time()
print("mcais execution time: {} secs".format(end - start))
#if (len(dimensions) >= 2):
# D.plot(dimensions, label=r'$D$', facecolor='b')
# O_inf.plot(dimensions, label=r'$\mathcal{O}_{\infty}$', facecolor='r')
# plt.legend()
# plt.show()
return O_inf
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)))
def test_empty(self):
# full dimensional
x_min = 1. * np.ones((2, 1))
x_max = 2. * np.ones((2, 1))
p = Polyhedron.from_bounds(x_min, x_max)
self.assertFalse(p.empty)
# lower dimensional, but not empy
C = np.ones((1, 2))
d = np.array([[3.]])
p.add_equality(C, d)
self.assertFalse(p.empty)
# lower dimensional and empty
x_0_max = np.array([[.5]])
p.add_upper_bound(x_0_max, [0])
self.assertTrue(p.empty)
def test_remove_equalities(self):
# contruct polyhedron
x_min = - np.ones(2)
x_max = - x_min
p = Polyhedron.from_bounds(x_min, x_max)
C = np.ones((1, 2))
d = np.ones(1)
p.add_equality(C, d)
E, f, N, R = p._remove_equalities()
# check result
self.assertAlmostEqual(N[0,0]/N[1,0], -1)
self.assertAlmostEqual(R[0,0]/R[1,0], 1)
intersections = [
np.sqrt(2.)/2,
3.*np.sqrt(2.)/2,
- np.sqrt(2.)/2,
- 3.*np.sqrt(2.)/2
]
for n in intersections:
residual = E.dot(np.array([[n]])) - f
self.assertAlmostEqual(np.min(np.abs(residual)), 0.)
# add another feasible equality (raises 'equality constraints C x = d do not have a nullspace.')
C = np.array([[1., -1.]])
d = np.zeros(1)
p1 = copy(p)
p1.add_equality(C, d)
self.assertRaises(ValueError, p1._remove_equalities)
# add another unfeasible equality (raises 'equality constraints C x = d do not have a nullspace.')
C = np.array([[0., 1.]])
d = np.array([5.])
p1.add_equality(C, d)
self.assertRaises(ValueError, p1._remove_equalities)
# add a linearly dependent equality (raises 'equality constraints C x = d are linearly dependent.')
C = 2*np.ones((1, 2))
d = np.ones(1)
p2 = copy(p)
p2.add_equality(C, d)
self.assertRaises(ValueError, p2._remove_equalities)
def test_contains(self):
# some points
x1 = 2. * np.ones((2, 1))
x2 = 2.5 * np.ones((2, 1))
x3 = 3.5 * np.ones((2, 1))
# full dimensional
x_min = 1. * np.ones((2, 1))
x_max = 3. * np.ones((2, 1))
p = Polyhedron.from_bounds(x_min, x_max)
self.assertTrue(p.contains(x1))
self.assertTrue(p.contains(x2))
self.assertFalse(p.contains(x3))
# lower dimensional, but not empty
C = np.ones((1, 2))
d = np.array([[4.]])
p.add_equality(C, d)
self.assertTrue(p.contains(x1))
self.assertFalse(p.contains(x2))
self.assertFalse(p.contains(x3))
def test_explicit_solution(self):
np.random.seed(1)
# random linear system
for i in range(10):
# ensure controllability
n = np.random.randint(2, 3)
m = np.random.randint(1, 2)
controllable = False
while not controllable:
A = np.random.rand(n, n)
B = np.random.rand(n, m)
S = LinearSystem(A, B)
controllable = S.controllable
# stage constraints
x_min = -np.random.rand(n, 1)
x_max = np.random.rand(n, 1)
u_min = -np.random.rand(m, 1)
u_max = np.random.rand(m, 1)
X = Polyhedron.from_bounds(x_min, x_max)
U = Polyhedron.from_bounds(u_min, u_max)
D = X.cartesian_product(U)
# assemble controller
N = 10
Q = np.eye(n)
R = np.eye(m)
P, K = S.solve_dare(Q, R)
X_N = S.mcais(K, D)
controller = ModelPredictiveController(S, N, Q, R, P, D, X_N)
# store explicit solution
controller.store_explicit_solution()
# simulate
for j in range(10):
# intial state
x = np.multiply(np.random.rand(n, 1), x_max - x_min) + x_min
# implicit vs explicit mpc
u_implicit, V_implicit = controller.feedforward(x)
u_explicit, V_explicit = controller.feedforward_explicit(x)
# if feasible
if V_implicit is not None:
self.assertAlmostEqual(V_implicit, V_explicit)
for t in range(N):
np.testing.assert_array_almost_equal(
u_implicit[t], u_explicit[t])
np.testing.assert_array_almost_equal(
controller.feedback(x),
controller.feedback_explicit(x))
# if unfeasible
else:
self.assertTrue(V_explicit is None)
self.assertTrue(u_explicit is None)
self.assertTrue(controller.feedback_explicit(x) is None)
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'])
def test_from_functions(self):
# row vector input
x = np.ones((1, 5))
self.assertRaises(ValueError, Polyhedron.from_lower_bound, x)
# from lower bound
x = np.ones((2, 1))
p = Polyhedron.from_lower_bound(x)
A_lb = np.array([[-1., 0.], [0., -1.]])
b_lb = np.array([[-1.], [-1.]])
self.assertTrue(
same_rows(np.hstack((A_lb, b_lb)), np.hstack((p.A, p.b))))
# from upper bound
p = Polyhedron.from_upper_bound(x)
A_ub = np.array([[1., 0.], [0., 1.]])
b_ub = np.array([[1.], [1.]])
self.assertTrue(
same_rows(np.hstack((A_ub, b_ub)), np.hstack((p.A, p.b))))
# from upper and lower bounds
p = Polyhedron.from_bounds(x, x)
A = np.vstack((A_lb, A_ub))
b = np.vstack((b_lb, b_ub))
self.assertTrue(same_rows(np.hstack((A, b)), np.hstack((p.A, p.b))))
# different size lower and upper bound
y = np.ones((3, 1))
self.assertRaises(ValueError, Polyhedron.from_bounds, x, y)
# from lower bound of not all the variables
indices = [1, 2]
n = 3
p = Polyhedron.from_lower_bound(x, indices, n)
A_lb = np.array([[0., -1., 0.], [0., 0., -1.]])
b_lb = np.array([[-1.], [-1.]])
self.assertTrue(
same_rows(np.hstack((A_lb, b_lb)), np.hstack((p.A, p.b))))
# from lower bound of not all the variables
indices = [0, 2]
n = 3
p = Polyhedron.from_upper_bound(x, indices, n)
A_ub = np.array([[1., 0., 0.], [0., 0., 1.]])
b_ub = np.array([[1.], [1.]])
self.assertTrue(
same_rows(np.hstack((A_ub, b_ub)), np.hstack((p.A, p.b))))
# from upper and lower bounds of not all the variables
indices = [0, 1]
n = 3
p = Polyhedron.from_bounds(x, x, indices, n)
A = np.array([[-1., 0., 0.], [0., -1., 0.], [1., 0., 0.], [0., 1.,
0.]])
b = np.array([[-1.], [-1.], [1.], [1.]])
self.assertTrue(same_rows(np.hstack((A, b)), np.hstack((p.A, p.b))))
# too many indices
indices = [1, 2, 3]
n = 5
self.assertRaises(ValueError, Polyhedron.from_lower_bound, x, indices,
n)
self.assertRaises(ValueError, Polyhedron.from_upper_bound, x, indices,
n)
self.assertRaises(ValueError, Polyhedron.from_bounds, x, x, indices, n)
# 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)
# We then consider then the polytopic constraints
# \begin{align}
# x(t) \in \mathcal X, \quad
# u(t) \in \mathcal U \quad
# \text{for} \quad
# t=0,\ldots, N-1,
# \end{align}
# and define $\mathcal D := \mathcal X \times \mathcal U$
u_min = np.array([-6.])
u_max = np.array([6.])
U = Polyhedron.from_bounds(u_min, u_max)
x_min = np.array([-.5, -.5])
x_max = np.array([.5, .5])
X = Polyhedron.from_bounds(x_min, x_max)
D = X.cartesian_product(U)
# Regarding the terminal set, the theory of Maximal Constraint-Admissible Invariant Sets (MCAIS) allows us
# to derive the biggest set for which the desired property holds
X_N = S.mcais(K, D)
# Now we have all the ingredients to build the MPC controller
controller = ModelPredictiveController(S, N, Q, R, P, D, X_N)
# We can simulate the closed loop system for $N_{\mathrm{sim}}$ steps starting from the initial condition $x_0$
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)
theta_min = -np.pi * .3
theta_max = np.pi * .3
n_linearizations = 3
# linearization points for the entire state x = (q_cart, q_pole, v_cart, v_pole)
linearization_points = [np.zeros(4) for i in range(n_linearizations)]
for i, theta in enumerate(np.linspace(theta_min, theta_max, n_linearizations)):
linearization_points[i][1] = theta
#%%
# boundaries for the state (despite of the mode the state has to lie in X)
theta_step = (theta_max - theta_min) / (n_linearizations - 1)
x_min = np.array([-3., theta_min - theta_step, -10., -10.])
x_max = np.array([3., theta_max + theta_step, 10., 10.])
X = Polyhedron.from_bounds(x_min, x_max)
#%%
# boundaries for the input --force on the cart-- (despite of the mode the inpu has to lie in U)
u_min = np.array([-100.])
u_max = np.array([100.])
U = Polyhedron.from_bounds(u_min, u_max)
#%%
# parse RigidBodyPlant from urdf
tree = RigidBodyTree("cart_pole.urdf", FloatingBaseType.kFixed)
plant = RigidBodyPlant(tree)
#%%
def test_from_functions(self):
# row vector input
x = np.ones((1,5))
self.assertRaises(ValueError, Polyhedron.from_lower_bound, x)
# from lower bound
x = np.ones(2)
p = Polyhedron.from_lower_bound(x)
A_lb = np.array([[-1., 0.], [0., -1.]])
b_lb = np.array([-1.,-1.])
self.assertTrue(same_rows(
np.column_stack((A_lb, b_lb)),
np.column_stack((p.A, p.b))
))
# from upper bound
p = Polyhedron.from_upper_bound(x)
A_ub = np.array([[1., 0.], [0., 1.]])
b_ub = np.array([1.,1.])
self.assertTrue(same_rows(
np.column_stack((A_ub, b_ub)),
np.column_stack((p.A, p.b))
))
# from upper and lower bounds
p = Polyhedron.from_bounds(x, x)
A = np.vstack((A_lb, A_ub))
b = np.concatenate((b_lb, b_ub))
self.assertTrue(same_rows(
np.column_stack((A, b)),
np.column_stack((p.A, p.b))
))
# different size lower and upper bound
y = np.ones((3,1))
self.assertRaises(ValueError, Polyhedron.from_bounds, x, y)
# from lower bound of not all the variables
indices = [1, 2]
n = 3
p = Polyhedron.from_lower_bound(x, indices, n)
A_lb = np.array([[0., -1., 0.], [0., 0., -1.]])
b_lb = np.array([-1.,-1.])
self.assertTrue(same_rows(
np.column_stack((A_lb, b_lb)),
np.column_stack((p.A, p.b))
))
# from lower bound of not all the variables
indices = [0, 2]
n = 3
p = Polyhedron.from_upper_bound(x, indices, n)
A_ub = np.array([[1., 0., 0.], [0., 0., 1.]])
b_ub = np.array([1.,1.])
self.assertTrue(same_rows(
np.column_stack((A_ub, b_ub)),
np.column_stack((p.A, p.b))
))
# from upper and lower bounds of not all the variables
indices = [0, 1]
n = 3
p = Polyhedron.from_bounds(x, x, indices, n)
A = np.array([[-1., 0., 0.], [0., -1., 0.], [1., 0., 0.], [0., 1., 0.]])
b = np.array([-1.,-1.,1.,1.])
self.assertTrue(same_rows(
np.column_stack((A, b)),
np.column_stack((p.A, p.b))
))
# too many indices
indices = [1, 2, 3]
n = 5
self.assertRaises(ValueError, Polyhedron.from_lower_bound, x, indices, n)
self.assertRaises(ValueError, Polyhedron.from_upper_bound, x, indices, n)
self.assertRaises(ValueError, Polyhedron.from_bounds, x, x, indices, n)
# from symbolic
n = 5
m = 3
x = sp.Matrix([sp.Symbol('x'+str(i), real=True) for i in range(n)])
A = np.random.rand(m, n)
b = np.random.rand(m)
ineq = sp.Matrix(A)*x - sp.Matrix(b)
P = Polyhedron.from_symbolic(x, ineq)
np.testing.assert_array_almost_equal(P.A, A)
np.testing.assert_array_almost_equal(P.b, b)
C = np.random.rand(m, n)
d = np.random.rand(m)
eq = sp.Matrix(C)*x - sp.Matrix(d)
P = Polyhedron.from_symbolic(x, ineq, eq)
np.testing.assert_array_almost_equal(P.A, A)
np.testing.assert_array_almost_equal(P.b, b)
np.testing.assert_array_almost_equal(P.C, C)
np.testing.assert_array_almost_equal(P.d, d)
def test_remove_redundant_inequalities(self):
# minimal facets only inequalities
A = np.array([[1., 1.], [-1., 1.], [0., -1.], [0., 1.], [0., 1.],
[2., 2.]])
b = np.array([[1.], [1.], [1.], [1.], [2.], [2.]])
p = Polyhedron(A, b)
mf = set(p.minimal_facets())
self.assertTrue(mf == set([1, 2, 0]) or mf == set([1, 2, 5]))
# add nasty redundant inequality
A = np.zeros((1, 2))
b = np.ones((1, 1))
p.add_inequality(A, b)
mf = set(p.minimal_facets())
self.assertTrue(mf == set([1, 2, 0]) or mf == set([1, 2, 5]))
# remove redundant facets
p.remove_redundant_inequalities()
A_min = np.array([[-1., 1.], [0., -1.], [1., 1.]])
b_min = np.array([[1.], [1.], [1.]])
self.assertTrue(
same_rows(np.hstack((A_min, b_min)), np.hstack((p.A, p.b))))
# both inequalities and equalities
x_min = -np.ones((2, 1))
x_max = -x_min
p = Polyhedron.from_bounds(x_min, x_max)
C = np.ones((1, 2))
d = np.ones((1, 1))
p.add_equality(C, d)
self.assertEqual([2, 3], sorted(p.minimal_facets()))
p.remove_redundant_inequalities()
A_min = np.array([[1., 0.], [0., 1.]])
b_min = np.array([[1.], [1.]])
self.assertTrue(
same_rows(np.hstack((A_min, b_min)), np.hstack((p.A, p.b))))
# add (redundant) inequality coincident with the equality
p.add_inequality(C, d)
p.remove_redundant_inequalities()
self.assertTrue(
same_rows(np.hstack((A_min, b_min)), np.hstack((p.A, p.b))))
# add (redundant) inequality
p.add_inequality(np.array([[-1., 1.]]), np.array([[1.1]]))
p.remove_redundant_inequalities()
self.assertTrue(
same_rows(np.hstack((A_min, b_min)), np.hstack((p.A, p.b))))
# add unfeasible equality (raises: 'empty polyhedron, cannot remove redundant inequalities.')
C = np.ones((1, 2))
d = -np.ones((1, 1))
p.add_equality(C, d)
self.assertRaises(ValueError, p.remove_redundant_inequalities)
# empty polyhderon (raises: 'empty polyhedron, cannot remove redundant inequalities.')
x_min = np.ones((2, 1))
x_max = np.zeros((2, 1))
p = Polyhedron.from_bounds(x_min, x_max)
self.assertRaises(ValueError, p.remove_redundant_inequalities)
def test_implicit_solution(self):
np.random.seed(1)
# random linear system
for i in range(100):
# ensure controllability
n = np.random.randint(2, 5)
m = np.random.randint(1, n)
controllable = False
while not controllable:
A = np.random.rand(n, n) / 10.
B = np.random.rand(n, m) / 10.
S = LinearSystem(A, B)
controllable = S.controllable
# stage constraints
x_min = -np.random.rand(n, 1)
x_max = np.random.rand(n, 1)
u_min = -np.random.rand(m, 1)
u_max = np.random.rand(m, 1)
X = Polyhedron.from_bounds(x_min, x_max)
U = Polyhedron.from_bounds(u_min, u_max)
D = X.cartesian_product(U)
# assemble controller
N = np.random.randint(5, 10)
Q = np.eye(n)
R = np.eye(m)
P, K = S.solve_dare(Q, R)
X_N = S.mcais(K, D)
controller = ModelPredictiveController(S, N, Q, R, P, D, X_N)
# simulate
for j in range(10):
# intial state
x = np.multiply(np.random.rand(n, 1), x_max - x_min) + x_min
# mpc
u_mpc, V_mpc = controller.feedforward(x)
# lqr
V_lqr = (.5 * x.T.dot(P).dot(x))[0, 0]
u_lqr = []
x_next = x
for t in range(N):
u_lqr.append(K.dot(x_next))
x_next = (A + B.dot(K)).dot(x_next)
# constraint set (not condensed)
constraints = copy(D)
C = np.hstack((np.eye(n), np.zeros((n, m))))
constraints.add_equality(C, x)
for t in range(N - 1):
constraints = constraints.cartesian_product(D)
C = np.zeros((n, constraints.A.shape[1]))
C[:, -2 * (n + m):] = np.hstack(
(A, B, -np.eye(n), np.zeros((n, m))))
d = np.zeros((n, 1))
constraints.add_equality(C, d)
constraints = constraints.cartesian_product(X_N)
# if x is inside mcais lqr = mpc
if X_N.contains(x):
self.assertAlmostEqual(V_mpc, V_lqr)
for t in range(N):
np.testing.assert_array_almost_equal(
u_mpc[t], u_lqr[t])
# if x is outside mcais lqr > mpc
elif V_mpc is not None:
self.assertTrue(V_mpc > V_lqr)
np.testing.assert_array_almost_equal(
u_mpc[0], controller.feedback(x))
# simulate the open loop control
x_mpc = S.simulate(x, u_mpc)
for t in range(N):
self.assertTrue(X.contains(x_mpc[t]))
self.assertTrue(U.contains(u_mpc[t]))
self.assertTrue(X_N.contains(x_mpc[N]))
# check feasibility
xu_mpc = np.vstack(
[np.vstack((x_mpc[t], u_mpc[t])) for t in range(N)])
xu_mpc = np.vstack((xu_mpc, x_mpc[N]))
self.assertTrue(constraints.contains(xu_mpc))
# else certify empyness of the constraint set
else:
self.assertTrue(controller.feedback(x) is None)
self.assertTrue(constraints.empty)
