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 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 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'])