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)
c = np.random.rand(n)
# reduce discretization step until the two method are almost equivalent
h = .01
convergence = False
while not convergence:
S_ee = AffineSystem.from_continuous(A, B, c, h,
'explicit_euler')
S_zoh = AffineSystem.from_continuous(A, B, c, h,
'zero_order_hold')
convergence = np.allclose(S_ee.A, S_zoh.A) and np.allclose(
S_ee.B, S_zoh.B) and np.allclose(S_ee.c, S_zoh.c)
if not convergence:
h /= 10.
self.assertTrue(convergence)
self.assertRaises(ValueError, AffineSystem.from_continuous, A, B, c, h,
'gatto')
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)
c = np.random.rand(n)
# test .from_continuous(), explicit euler vs zoh
convergence = False
while not convergence:
S_ee = AffineSystem.from_continuous(A, B, c, h, 'explicit_euler')
S_zoh = AffineSystem.from_continuous(A, B, c, h, 'zero_order_hold')
convergence = np.allclose(S_ee.A, S_zoh.A) and np.allclose(S_ee.B, S_zoh.B) and np.allclose(S_ee.c, S_zoh.c)
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 + sp.Matrix(c)
S = AffineSystem.from_symbolic(x, u, x_next)
np.testing.assert_array_almost_equal(S.A, A)
np.testing.assert_array_almost_equal(S.B, B)
np.testing.assert_array_almost_equal(S.c, c)
# test .from_symbolic_continuous()
S = AffineSystem.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)
np.testing.assert_array_almost_equal(S.c, S_ee.c)
S = AffineSystem.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)
np.testing.assert_array_almost_equal(S.c, S_zoh.c)
# negative time step
self.assertRaises(ValueError, AffineSystem.from_continuous, A, B, c, -.1, 'explicit_euler')
self.assertRaises(ValueError, AffineSystem.from_symbolic_continuous, x, u, x_next, -.1, 'explicit_euler')
self.assertRaises(ValueError, AffineSystem.from_continuous, A, B, c, -.1, 'zero_order_hold')
self.assertRaises(ValueError, AffineSystem.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, AffineSystem.from_symbolic, x_wrong, u, x_next)
self.assertRaises(TypeError, AffineSystem.from_symbolic, x, u_wrong, x_next)
self.assertRaises(TypeError, AffineSystem.from_symbolic_continuous, x_wrong, u, x_next, h, 'explicit_euler')
self.assertRaises(TypeError, AffineSystem.from_symbolic_continuous, x, u_wrong, x_next, h, 'explicit_euler')
self.assertRaises(TypeError, AffineSystem.from_symbolic_continuous, x_wrong, u, x_next, h, 'zero_order_hold')
self.assertRaises(TypeError, AffineSystem.from_symbolic_continuous, x, u_wrong, x_next, h, 'zero_order_hold')
# from continuous wrong method
self.assertRaises(ValueError, AffineSystem.from_continuous, A, B, c, h, 'gatto')
self.assertRaises(ValueError, AffineSystem.from_symbolic_continuous, x, u, x_next, h, 'gatto')
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 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 pwa_from_RigidBodyPlant(plant, linearization_points, X, U, h, method='zero_order_hold'):
"""
Arguments
----------
plant : RigidBodyPlant
RigidBodyPlant of the robot.
linearization_points : list of numpy.ndarray
Points in the state space where to linearize the dynamics.
X : Polyhedron
Overall bounds of the state space.
U : Polyhedron
Overall bounds of the control space.
h : float
Sampling time for the time discretization of the PWA system.
method : string
Method used to discretize each piece of the PWA dynamics ('explicit_euler' or 'zero_order_hold').
Returns
----------
PWA : PieceWiseAffineSystem
"""
# partition of the state space
X_partition = constrained_voronoi(linearization_points, X)
domains = [Xi.cartesian_product(U) for Xi in X_partition]
# create context
context = plant.CreateDefaultContext()
state = context.get_mutable_continuous_state_vector()
input = BasicVector(np.array([0.]))
context.FixInputPort(0, input)
# affine systems
affine_systems = []
for x in linearization_points:
state.SetFromVector(x)
taylor_approx = FirstOrderTaylorApproximation(plant, context)
affine_system = AffineSystem.from_continuous(
taylor_approx.A(),
taylor_approx.B(),
taylor_approx.f0(),
h,
method
)
affine_systems.append(affine_system)
return PieceWiseAffineSystem(affine_systems, domains)
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_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)
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'])