def _translated_domains(self):
x_eq, u_eq = self.equilibrium_point()
translated_domains = []
for domain in self._domains():
translated_domain = Polytope(domain.A, domain.b - domain.A.dot(np.vstack((x_eq, u_eq))))
translated_domain.assemble()
translated_domains.append(translated_domain)
return translated_domains
def extrude_2d_polytope(p_2d, z_limits):
A = np.vstack((np.hstack((p_2d.lhs_min, np.zeros(
(p_2d.lhs_min.shape[0], 1)))),
np.hstack((np.zeros(
(2, p_2d.lhs_min.shape[1])), np.array([[1.], [-1.]])))))
b = np.vstack((p_2d.rhs_min, np.array([[z_limits[1]], [-z_limits[0]]])))
p_3d = Polytope(A, b)
p_3d.assemble()
return p_3d
def polytope(self, qp):
"""
Stores a polytope that describes the critical region in the parameter space.
"""
# multipliers explicit solution
[G_A, W_A, S_A] = [
qp.G[self.active_set, :], qp.W[self.active_set, :],
qp.S[self.active_set, :]
]
[G_I, W_I, S_I] = [
qp.G[self.inactive_set, :], qp.W[self.inactive_set, :],
qp.S[self.inactive_set, :]
]
H_A = np.linalg.inv(G_A.dot(qp.H_inv.dot(G_A.T)))
self.lambda_A_offset = -H_A.dot(W_A)
self.lambda_A_linear = -H_A.dot(S_A)
# primal variables explicit solution
self.z_offset = -qp.H_inv.dot(G_A.T.dot(self.lambda_A_offset))
self.z_linear = -qp.H_inv.dot(G_A.T.dot(self.lambda_A_linear))
# optimal value function explicit solution: V_star = .5 x' V_quadratic x + V_linear x + V_offset
self.V_quadratic = self.z_linear.T.dot(qp.H).dot(
self.z_linear) + qp.F_xx_q
self.V_linear = self.z_offset.T.dot(qp.H).dot(
self.z_linear) + qp.F_x_q.T
self.V_offset = qp.F_q + .5 * self.z_offset.T.dot(qp.H).dot(
self.z_offset)
# primal original variables explicit solution
self.u_offset = self.z_offset - qp.H_inv.dot(qp.F_u)
self.u_linear = self.z_linear - qp.H_inv.dot(qp.F_xu.T)
# equation (12) (modified: only inactive indices considered)
lhs_type_1 = G_I.dot(self.z_linear) - S_I
rhs_type_1 = -G_I.dot(self.z_offset) + W_I
# equation (13)
lhs_type_2 = -self.lambda_A_linear
rhs_type_2 = self.lambda_A_offset
# gather facets of type 1 and 2 to define the polytope (note the order: the ith facet of the cr is generated by the ith constraint)
lhs = np.zeros((self.n_constraints, self.n_parameters))
rhs = np.zeros((self.n_constraints, 1))
lhs[self.inactive_set + self.active_set, :] = np.vstack(
(lhs_type_1, lhs_type_2))
rhs[self.inactive_set + self.active_set] = np.vstack(
(rhs_type_1, rhs_type_2))
# construct polytope
self.polytope = Polytope(lhs, rhs)
self.polytope.assemble()
return
def test_DTLinearSystem(self):
# constinuous time double integrator
A = np.array([[0., 1.], [0., 0.]])
B = np.array([[0.], [1.]])
t_s = 1.
# discrete time from continuous
sys = DTLinearSystem.from_continuous(A, B, t_s, 'zoh')
A_discrete = np.eye(2) + A * t_s
B_discrete = B * t_s + np.array([[0., t_s**2 / 2.], [0., 0.]]).dot(B)
self.assertTrue(all(np.isclose(sys.A.flatten(), A_discrete.flatten())))
self.assertTrue(all(np.isclose(sys.B.flatten(), B_discrete.flatten())))
# simulation forced dynamics
x0 = np.array([[0.], [1.]])
N = 10
u = np.array([[1.]])
u_list = [u] * N
x_list = sys.simulate(x0, u_list)
real_x_list = [[
x0[0] + x0[1] * i * t_s + u[0, 0] * (i * t_s)**2 / 2.,
x0[1] + u * i * t_s
] for i in range(0, N + 1)]
self.assertTrue(all(np.isclose(x_list, real_x_list).flatten()))
# test maximum output admissible set
Q = np.eye(2)
R = np.eye(1)
P, K = dare(sys.A, sys.B, Q, R)
x_max = np.ones((2, 1))
x_min = -x_max
X = Polytope.from_bounds(x_min, x_max)
X.assemble()
u_max = np.ones((1, 1))
u_min = -u_max
U = Polytope.from_bounds(u_min, u_max)
U.assemble()
moas = moas_closed_loop_from_orthogonal_domains(A, B, K, X, U)
sys_cl = DTLinearSystem(A + B.dot(K), np.zeros((2, 1)))
v0_max = max([v[0] for v in moas.vertices])
v1_max = max([v[1] for v in moas.vertices])
v0_min = min([v[0] for v in moas.vertices])
v1_min = min([v[1] for v in moas.vertices])
for x0 in list(np.linspace(v0_min, v0_max, 100)):
for x1 in list(np.linspace(v1_min, v1_max, 100)):
x_init = np.array([[x0], [x1]])
if moas.applies_to(x_init):
u = np.zeros((1, 1))
u_list = [u] * N
x_list = sys_cl.simulate(x_init, u_list)
for x in x_list:
self.assertTrue(moas.applies_to(x))
def test_DTPWASystem(self):
# PWA dynamics
t_s = .1
A_1 = np.array([[0., 1.], [10., 0.]])
B_1 = np.array([[0.], [1.]])
c_1 = np.array([[0.], [0.]])
sys_1 = DTAffineSystem.from_continuous(A_1, B_1, c_1, t_s, 'zoh')
A_2 = np.array([[0., 1.], [-100., 0.]])
B_2 = B_1
c_2 = np.array([[0.], [10.]])
sys_2 = DTAffineSystem.from_continuous(A_2, B_2, c_2, t_s, 'zoh')
sys = [sys_1, sys_2]
# PWA state domains
x_max_1 = np.array([[.1], [1.5]])
x_max_2 = np.array([[.2], [x_max_1[1, 0]]])
x_min_1 = -x_max_2
x_min_2 = np.array([[x_max_1[0, 0]], [x_min_1[1, 0]]])
X_1 = Polytope.from_bounds(x_min_1, x_max_1)
X_1.assemble()
X_2 = Polytope.from_bounds(x_min_2, x_max_2)
X_2.assemble()
X = [X_1, X_2]
# PWA input domains
u_max = np.array([[4.]])
u_min = -u_max
U_1 = Polytope.from_bounds(u_min, u_max)
U_1.assemble()
U_2 = U_1
U = [U_1, U_2]
sys = DTPWASystem.from_orthogonal_domains(sys, X, U)
# simualate
N = 10
x_0 = np.array([[.0], [.5]])
u_list = [np.ones((1, 1)) for i in range(N)]
x_sim_1, switching_sequence = sys.simulate(x_0, u_list)
x_sim_1 = np.vstack(x_sim_1)
# condense for the simulated switching sequence
A_bar, B_bar, c_bar = sys.condense(switching_sequence)
x_sim_2 = A_bar.dot(x_0) + B_bar.dot(np.vstack(u_list)) + c_bar
# compare the 2 simulations
self.assertTrue(all(np.isclose(x_sim_1.flatten(), x_sim_2.flatten())))
def test_MPCController(self):
# double integrator
A = np.array([[0., 1.], [0., 0.]])
B = np.array([[0.], [1.]])
t_s = 1.
sys = ds.DTLinearSystem.from_continuous(A, B, t_s)
# mpc controller
N = 5
Q = np.eye(A.shape[0])
R = np.eye(B.shape[1])
objective_norm = 'two'
P, K = ds.dare(sys.A, sys.B, Q, R)
u_max = np.array([[1.]])
u_min = -u_max
U = Polytope.from_bounds(u_min, u_max)
U.assemble()
x_max = np.array([[1.], [1.]])
x_min = -x_max
X = Polytope.from_bounds(x_min, x_max)
X.assemble()
X_N = ds.moas_closed_loop_from_orthogonal_domains(
sys.A, sys.B, K, X, U)
controller = MPCController(sys, N, objective_norm, Q, R, P, X, U, X_N)
# explicit vs implicit solution
controller.get_explicit_solution()
n_test = 100
for i in range(n_test):
x0 = np.random.rand(2, 1)
u_explicit, V_explicit = controller.feedforward_explicit(x0)
u_implicit, V_implicit = controller.feedforward(x0)
u_explicit = np.vstack(u_explicit)
u_implicit = np.vstack(u_implicit)
if any(np.isnan(u_explicit)) or any(np.isnan(u_implicit)):
self.assertTrue(all(np.isnan(u_explicit)))
self.assertTrue(all(np.isnan(u_implicit)))
self.assertTrue(np.isnan(V_explicit))
self.assertTrue(np.isnan(V_implicit))
# self.assertFalse(controller.condensed_program.feasible_set.applies_to(x0))
else:
self.assertTrue(
all(
np.isclose(u_explicit, u_implicit,
rtol=1.e-4).flatten()))
self.assertTrue(np.isclose(V_explicit, V_implicit, rtol=1.e-4))
class BoxAtlasKinematicLimits(object):
def __init__(self):
# position bounds
# body
self.q_b_min = np.array([[0.3],[0.3]])
self.q_b_max = np.array([[0.7],[0.7]])
# left foot (limits in the body frame)
self.q_lf_min = np.array([[0.],[-.7]])
self.q_lf_max = np.array([[.4],[-.3]])
# right foot (limits in the body frame)
self.q_rf_min = np.array([[-.4],[-.7]])
self.q_rf_max = np.array([[0.],[-.3]])
# hand (limits in the body frame)
self.q_h_min = np.array([[-.6],[-.1]])
self.q_h_max = np.array([[-.2],[.3]])
# velocity bounds
# body
self.v_b_max = 5 * np.ones((2,1))
self.v_b_min = - self.v_b_max
self.polytope = None # will be generated in self._assemble()
def _assemble(self):
selection_matrix = np.vstack((np.eye(2), -np.eye(2)))
# left foot
lhs = np.hstack((-selection_matrix, selection_matrix, np.zeros((4,6))))
rhs = np.vstack((self.q_lf_max, -self.q_lf_min))
self.polytope = Polytope(lhs, rhs)
# right foot
lhs = np.hstack((-selection_matrix, np.zeros((4,2)), selection_matrix, np.zeros((4,4))))
rhs = np.vstack((self.q_rf_max, -self.q_rf_min))
self.polytope.add_facets(lhs, rhs)
# hand
lhs = np.hstack((-selection_matrix, np.zeros((4,4)), selection_matrix, np.zeros((4,2))))
rhs = np.vstack((self.q_h_max, -self.q_h_min))
self.polytope.add_facets(lhs, rhs)
# body
self.polytope.add_bounds(self.q_b_min, self.q_b_max, [0,1])
self.polytope.add_bounds(self.v_b_min, self.v_b_max, [8,9])
self.polytope.assemble()
return self.polytope
def random_state(self, X=None, controller=None):
"""
Sample a random state within the lower and upper bounds of the piecewise
affine system, and further restrict that state to some polytope X. By
default, X is the polytope defined by the robot's kinematic limits.
If `controller` is not None, use the given controller to check if there
is a feasible input sequence from the given sample before returning it
"""
if X is None:
A = self.kinematic_limits.polytope.A
b = self.kinematic_limits.polytope.b
x_eq, u_eq = self.equilibrium_point()
X = Polytope(A, b - A.dot(x_eq)).assemble()
while True:
x = np.random.rand(self.pwa_system.n_x, 1)
x = np.multiply(x, (self.pwa_system.x_max - self.pwa_system.x_min)) + self.pwa_system.x_min
if X.applies_to(x):
if controller is not None:
u, xtraj, ss, cost = controller.feedforward(x)
if np.any(np.isnan(u[0])):
continue
return x
def _assemble(self):
selection_matrix = np.vstack((np.eye(2), -np.eye(2)))
# left foot
lhs = np.hstack((-selection_matrix, selection_matrix, np.zeros((4,6))))
rhs = np.vstack((self.q_lf_max, -self.q_lf_min))
self.polytope = Polytope(lhs, rhs)
# right foot
lhs = np.hstack((-selection_matrix, np.zeros((4,2)), selection_matrix, np.zeros((4,4))))
rhs = np.vstack((self.q_rf_max, -self.q_rf_min))
self.polytope.add_facets(lhs, rhs)
# hand
lhs = np.hstack((-selection_matrix, np.zeros((4,4)), selection_matrix, np.zeros((4,2))))
rhs = np.vstack((self.q_h_max, -self.q_h_min))
self.polytope.add_facets(lhs, rhs)
# body
self.polytope.add_bounds(self.q_b_min, self.q_b_max, [0,1])
self.polytope.add_bounds(self.v_b_min, self.v_b_max, [8,9])
self.polytope.assemble()
return self.polytope
def _state_constraints(self):
n = len(self.moving_limbs)
selection_matrix = np.vstack((np.eye(2), -np.eye(2)))
X = Polytope(np.zeros((0, 2 * (n + 2))), np.zeros((0, 1)))
for i, limb in enumerate(self.moving_limbs):
lhs = np.hstack((np.zeros(
(4, i * 2)), selection_matrix, np.zeros(
(4, (n - 1 - i) * 2)), -selection_matrix, np.zeros(
(4, 2))))
rhs = np.vstack((self.kinematic_limits[limb]['max'],
-self.kinematic_limits[limb]['min']))
X.add_facets(lhs, rhs)
for limb in self.fixed_limbs:
q_b_min = self.nominal_limb_positions[
limb] - self.kinematic_limits[limb]['max']
q_b_max = self.nominal_limb_positions[
limb] - self.kinematic_limits[limb]['min']
X.add_bounds(q_b_min, q_b_max, [2 * n, 2 * n + 1])
X.add_bounds(self.kinematic_limits['b']['min'],
self.kinematic_limits['b']['max'], [2 * n, 2 * n + 1])
X.add_bounds(self.velocity_limits['b']['min'],
self.velocity_limits['b']['max'], [2 * n + 2, 2 * n + 3])
# X = Polytope(X.A, X.b - X.A.dot(self.x_eq))
return X
def controller(self,
N=10,
Q=10 * np.eye(10),
R=np.eye(9),
objective_norm="two",
X_N=Polytope.from_bounds(-np.ones((10,1)), np.ones((10,1)))):
# terminal set and cost
# terminal_mode = 4
#P, K = dare(translated_affine_systems[terminal_mode].A, translated_affine_systems[terminal_mode].B, Q, R)
#X_N = ds.moas_closed_loop(translated_affine_systems[terminal_mode].A, translated_affine_systems[terminal_mode].B, K, X[1], U[1])
P = Q
if not X_N.assembled:
X_N.assemble()
# hybrid controller
return MPCHybridController(self.pwa_system, N, objective_norm, Q, R, P, X_N)
def test_orthogonal_projection(self):
# test plane generator
points = [
np.array([[1.], [0.], [0.]]),
np.array([[-1.], [0.], [0.]]),
np.array([[0.], [1.], [0.]])
]
a, b = plane_through_points(points)
self.assertTrue(np.allclose(a, np.array([[0.], [0.], [1.]])))
self.assertTrue(np.allclose(b, np.array([[0.]])))
points = [
np.array([[1.], [0.], [0.]]),
np.array([[0.], [1.], [0.]]),
np.array([[0.], [0.], [1.]])
]
a, b = plane_through_points(points)
real_a = np.ones((3, 1)) / np.sqrt(3)
self.assertTrue(np.allclose(a, real_a))
self.assertTrue(np.isclose(b[0, 0], real_a[0, 0]))
# test CHM
n_test = 100
n_ineq = 20
n_var = 5
res = [1, 3, 4]
for i in range(n_test):
everything_ok = False
while not everything_ok:
A = np.random.randn(n_ineq, n_var)
b = np.random.rand(n_ineq, 1)
x_offeset = np.random.rand(n_var, 1)
b += A.dot(x_offeset)
p = Polytope(A, b)
try:
p.assemble()
everything_ok = True
except ValueError:
pass
p_proj_ve = p.orthogonal_projection(res, 'vertex_enumeration')
for method in ['convex_hull']: #, 'block_elimination']:
p_proj = p.orthogonal_projection(res, method)
self.assertTrue(
p_proj.lhs_min.shape[0] == p_proj_ve.lhs_min.shape[0])
# note that sometimes qhull gives the same vertex twice!
for v in p_proj.vertices:
self.assertTrue(
any([
np.allclose(v, v_ve) for v_ve in p_proj_ve.vertices
]))
for v_ve in p_proj_ve.vertices:
self.assertTrue(
any([np.allclose(v, v_ve) for v in p_proj.vertices]))
def _assemble(self, kinematic_limits, friction_coefficient, stiffness):
# force bounds
# left foot
f_lf_x_max = - friction_coefficient * stiffness * (kinematic_limits.q_b_min[1,0]+kinematic_limits.q_lf_min[1,0])
f_lf_x_min = - f_lf_x_max
# right foot
f_rf_x_max = - friction_coefficient * stiffness * (kinematic_limits.q_b_min[1,0]+kinematic_limits.q_rf_min[1,0])
f_rf_x_min = - f_rf_x_max
# hand
f_h_y_max = - friction_coefficient * stiffness * (kinematic_limits.q_b_min[0,0]+kinematic_limits.q_h_min[0,0])
f_h_y_min = - f_h_y_max
input_max = np.vstack((self.v_lf_max, self.v_rf_max, self.v_h_max, f_lf_x_max, f_rf_x_max, f_h_y_max))
input_min = np.vstack((self.v_lf_min, self.v_rf_min, self.v_h_min, f_lf_x_min, f_rf_x_min, f_h_y_min))
self.polytope = Polytope.from_bounds(input_min, input_max).assemble()
return self.polytope
def state_partition(critical_regions,
feasible_set,
active_set=False,
facet_index=False,
**kwargs):
if critical_regions is None:
raise ValueError(
'Explicit solution not computed yet! First run .compute_explicit_solution().'
)
fig, ax = plt.subplots()
for cr in critical_regions:
p = Polytope(cr.polytope.lhs_min, cr.polytope.rhs_min)
p.add_facets(feasible_set.lhs_min, feasible_set.rhs_min)
p.assemble()
try:
pass
p.plot(facecolor=np.random.rand(3), **kwargs)
except AttributeError:
pass
ax.autoscale_view()
return
def _mode_independent_constraints(self, kinematic_limits, input_limits):
kinematic_limits._assemble()
input_limits._assemble(kinematic_limits, self.friction_coefficient, self.stiffness)
lhs = np.vstack((
np.hstack((
kinematic_limits.polytope.A,
np.zeros((
kinematic_limits.polytope.A.shape[0],
input_limits.polytope.A.shape[1]
))
)),
np.hstack((
np.zeros((
input_limits.polytope.A.shape[0],
kinematic_limits.polytope.A.shape[1]
)),
input_limits.polytope.A
))
))
rhs = np.vstack((kinematic_limits.polytope.b, input_limits.polytope.b))
return Polytope(lhs, rhs)
def _contact_mode_constraints(self, contact_mode, X, U):
n = len(self.moving_limbs)
X_mode = copy(X)
# force the limbs to stay inside the polyhedra
for i, limb in enumerate(self.moving_limbs):
moving_limb = self.topology['moving'][limb][contact_mode[limb]]
A = moving_limb.A
b = moving_limb.b
lhs = np.hstack((np.zeros(
(A.shape[0], i * 2)), A, np.zeros(
(A.shape[0], 2 * (n - i) + 2))))
X_mode.add_facets(lhs, b)
# gather state and input constraints
lhs = linalg.block_diag(*[X_mode.A, U.A])
rhs = np.vstack((X_mode.b, U.b))
D = Polytope(lhs, rhs)
# friction constraints
mu = self.parameters['friction_coefficient']
k = self.parameters['stiffness']
c = self.parameters['damping']
for i, limb in enumerate(self.moving_limbs):
moving_limb = self.topology['moving'][limb][contact_mode[limb]]
if moving_limb.contact_surface is not None:
a = moving_limb.A[moving_limb.contact_surface, :]
b = moving_limb.b[moving_limb.contact_surface, 0]
lhs = np.zeros((2, self.n_x + self.n_u))
lhs[:, i * 2:(i + 1) *
2] = mu * k * np.array([[a[0], a[1]], [a[0], a[1]]])
lhs[:, self.n_x + i * 2:self.n_x +
(i + 1) * 2] = c * np.array([[-a[1], a[0]], [a[1], -a[0]]])
rhs = mu * k * b * np.ones((2, 1))
D.add_facets(lhs, rhs)
xu_eq = np.vstack((self.x_eq, self.u_eq))
D = Polytope(D.A, D.b - D.A.dot(xu_eq))
D.assemble()
return D
def _input_constraints(self):
# bounds
u_min = np.zeros((0, 1))
u_max = np.zeros((0, 1))
for limb in self.moving_limbs:
u_min = np.vstack((u_min, self.velocity_limits[limb]['min']))
u_max = np.vstack((u_max, self.velocity_limits[limb]['max']))
for limb in self.fixed_limbs:
u_min = np.vstack((u_min, self.force_limits[limb]['min']))
u_max = np.vstack((u_max, self.force_limits[limb]['max']))
U = Polytope.from_bounds(u_min, u_max)
# friction limits
n_moving = len(self.moving_limbs)
n_fixed = len(self.fixed_limbs)
lhs_friction = np.array(
[[-self.parameters['friction_coefficient'], 1.],
[-self.parameters['friction_coefficient'], -1.]])
lhs = np.hstack((np.zeros((n_fixed * 2, n_moving * 2)),
linalg.block_diag(*[lhs_friction] * n_fixed)))
rhs = np.zeros((n_fixed * 2, 1))
U.add_facets(lhs, rhs)
# U = Polytope(U.A, U.b - U.A.dot(self.u_eq))
return U
def test_Polytope(self):
# empty
A = np.array([[1., 0.], [-1., 0.], [0., 1.]])
b = np.array([[1.], [-2.], [0.]])
p = Polytope(A, b)
p.assemble()
self.assertTrue(p.empty)
# unbounded (easy)
A = np.array([[1., 1.], [1., -1.]])
b = np.array([[0.], [0.]])
p = Polytope(A, b)
with self.assertRaises(ValueError):
p.assemble()
# unbounded (difficult)
A = np.array([[1., 0.], [-1., 0.], [0., 1.]])
b = np.array([[1.], [1.], [0.]])
p = Polytope(A, b)
with self.assertRaises(ValueError):
p.assemble()
# bounded and not empty
A = np.array([[1., 0.], [-1., 0.], [0., 1.], [0., -1.]])
b = np.array([[1.], [1.], [1.], [1.]])
p = Polytope(A, b)
p.assemble()
self.assertFalse(p.empty)
self.assertTrue(p.bounded)
# coincident facets
A = np.array([[1., 0.], [1. - 1.e-10, 1.e-10], [-1., 0.], [0., 1.],
[0., -1.]])
b = np.ones((5, 1))
p = Polytope(A, b)
p.assemble()
true_coincident_facets = [[0, 1], [0, 1], [2], [3], [4]]
self.assertEqual(p.coincident_facets, true_coincident_facets)
self.assertTrue(p.minimal_facets[0] in [0, 1])
self.assertEqual(p.minimal_facets[1:], [2, 3, 4])
# coincident facets, minimal facets, points on facets
A = np.array([[1., 1.], [-1., 1.], [0., -1.], [0., 1.], [0., 1.],
[1. - 1.e-10, 1. - 1.e-10]])
b = np.array([[1.], [1.], [1.], [1.], [2.], [1.]])
p = Polytope(A, b)
p.assemble()
true_coincident_facets = [[0, 5], [1], [2], [3], [4], [0, 5]]
true_minimal_facets = [1, 2, 5]
true_facet_centers = [[[-1.], [0.]], [[0.], [-1.]], [[1.], [0.]]]
self.assertEqual(p.coincident_facets, true_coincident_facets)
self.assertEqual(true_minimal_facets, p.minimal_facets)
for i in range(0, len(true_facet_centers)):
self.assertTrue(
all(np.isclose(true_facet_centers[i], p.facet_centers(i))))
# from_ and add_ methods
x_max = np.ones((2, 1))
x_min = -x_max
p = Polytope.from_bounds(x_min, x_max)
p.add_bounds(x_min * 2., x_max / 2.)
A = np.array([[-1., 0.], [0., -1.]])
b = np.array([[.2], [2.]])
p.add_facets(A, b)
p.assemble()
self.assertFalse(p.empty)
self.assertTrue(p.bounded)
true_vertices = [
np.array([[.5], [.5]]),
np.array([[-.2], [.5]]),
np.array([[-.2], [-1.]]),
np.array([[.5], [-1.]])
]
self.assertTrue(
all([
any(np.all(np.isclose(vertex, true_vertices), axis=1))
for vertex in p.vertices
]))
# intersection and inclusion
x_max = np.ones((2, 1))
x_min = -x_max
p1 = Polytope.from_bounds(x_min, x_max)
p1.assemble()
x_max = np.ones((2, 1)) * 2.
x_min = -x_max
p2 = Polytope.from_bounds(x_min, x_max)
p2.assemble()
x_min = np.zeros((2, 1))
p3 = Polytope.from_bounds(x_min, x_max)
p3.assemble()
x_max = np.ones((2, 1)) * 5.
x_min = np.ones((2, 1)) * 4.
p4 = Polytope.from_bounds(x_min, x_max)
p4.assemble()
# intersection
self.assertTrue(p1.intersect_with(p2))
self.assertTrue(p1.intersect_with(p3))
self.assertFalse(p1.intersect_with(p4))
# inclusion
self.assertTrue(p1.included_in(p2))
self.assertFalse(p1.included_in(p3))
self.assertFalse(p1.included_in(p4))
class CriticalRegion:
"""
Implements the algorithm from Tondel et al. "An algorithm for multi-parametric quadratic programming and explicit MPC solutions"
VARIABLES:
n_constraints: number of contraints in the qp
n_parameters: number of parameters of the qp
active_set: active set inside the critical region
inactive_set: list of indices of non active contraints inside the critical region
polytope: polytope describing the ceritical region in the parameter space
weakly_active_constraints: list of indices of constraints that are weakly active iside the entire critical region
candidate_active_sets: list of lists of active sets, its ith element collects the set of all the possible
active sets that can be found crossing the ith minimal facet of the polyhedron
z_linear: linear term in the piecewise affine primal solution z_opt = z_linear*x + z_offset
z_offset: offset term in the piecewise affine primal solution z_opt = z_linear*x + z_offset
u_linear: linear term in the piecewise affine primal solution u_opt = u_linear*x + u_offset
u_offset: offset term in the piecewise affine primal solution u_opt = u_linear*x + u_offset
lambda_A_linear: linear term in the piecewise affine dual solution (only active multipliers) lambda_A = lambda_A_linear*x + lambda_A_offset
lambda_A_offset: offset term in the piecewise affine dual solution (only active multipliers) lambda_A = lambda_A_linear*x + lambda_A_offset
"""
def __init__(self, active_set, qp):
# store active set
print 'Computing critical region for the active set ' + str(active_set)
[self.n_constraints, self.n_parameters] = qp.S.shape
self.active_set = active_set
self.inactive_set = sorted(
list(set(range(self.n_constraints)) - set(active_set)))
# find the polytope
self.polytope(qp)
if self.polytope.empty:
return
# find candidate active sets for the neighboiring regions
minimal_coincident_facets = [
self.polytope.coincident_facets[i]
for i in self.polytope.minimal_facets
]
self.candidate_active_sets = self.candidate_active_sets(
active_set, minimal_coincident_facets)
# find weakly active constraints
self.find_weakly_active_constraints()
# expand the candidates if there are weakly active constraints
if self.weakly_active_constraints:
self.candidate_active_set = self.expand_candidate_active_sets(
self.candidate_active_set, self.weakly_active_constraints)
return
def polytope(self, qp):
"""
Stores a polytope that describes the critical region in the parameter space.
"""
# multipliers explicit solution
[G_A, W_A, S_A] = [
qp.G[self.active_set, :], qp.W[self.active_set, :],
qp.S[self.active_set, :]
]
[G_I, W_I, S_I] = [
qp.G[self.inactive_set, :], qp.W[self.inactive_set, :],
qp.S[self.inactive_set, :]
]
H_A = np.linalg.inv(G_A.dot(qp.H_inv.dot(G_A.T)))
self.lambda_A_offset = -H_A.dot(W_A)
self.lambda_A_linear = -H_A.dot(S_A)
# primal variables explicit solution
self.z_offset = -qp.H_inv.dot(G_A.T.dot(self.lambda_A_offset))
self.z_linear = -qp.H_inv.dot(G_A.T.dot(self.lambda_A_linear))
# optimal value function explicit solution: V_star = .5 x' V_quadratic x + V_linear x + V_offset
self.V_quadratic = self.z_linear.T.dot(qp.H).dot(
self.z_linear) + qp.F_xx_q
self.V_linear = self.z_offset.T.dot(qp.H).dot(
self.z_linear) + qp.F_x_q.T
self.V_offset = qp.F_q + .5 * self.z_offset.T.dot(qp.H).dot(
self.z_offset)
# primal original variables explicit solution
self.u_offset = self.z_offset - qp.H_inv.dot(qp.F_u)
self.u_linear = self.z_linear - qp.H_inv.dot(qp.F_xu.T)
# equation (12) (modified: only inactive indices considered)
lhs_type_1 = G_I.dot(self.z_linear) - S_I
rhs_type_1 = -G_I.dot(self.z_offset) + W_I
# equation (13)
lhs_type_2 = -self.lambda_A_linear
rhs_type_2 = self.lambda_A_offset
# gather facets of type 1 and 2 to define the polytope (note the order: the ith facet of the cr is generated by the ith constraint)
lhs = np.zeros((self.n_constraints, self.n_parameters))
rhs = np.zeros((self.n_constraints, 1))
lhs[self.inactive_set + self.active_set, :] = np.vstack(
(lhs_type_1, lhs_type_2))
rhs[self.inactive_set + self.active_set] = np.vstack(
(rhs_type_1, rhs_type_2))
# construct polytope
self.polytope = Polytope(lhs, rhs)
self.polytope.assemble()
return
def find_weakly_active_constraints(self, toll=1e-8):
"""
Stores the list of constraints that are weakly active in the whole critical region
enumerated in the as in the equation G z <= W + S x ("original enumeration")
(by convention weakly active constraints are included among the active set,
so that only constraints of type 2 are anlyzed)
"""
# equation (13), again...
lhs_type_2 = -self.lambda_A_linear
rhs_type_2 = self.lambda_A_offset
# weakly active constraints are included in the active set
self.weakly_active_constraints = []
for i in range(0, len(self.active_set)):
# to be weakly active in the whole region they can only be in the form 0^T x <= 0
if np.linalg.norm(lhs_type_2[i, :]) + np.absolute(
rhs_type_2[i, :]) < toll:
print('Weakly active constraint detected!')
self.weakly_active_constraints.append(self.active_set[i])
return
@staticmethod
def candidate_active_sets(active_set, minimal_coincident_facets):
"""
Computes one candidate active set for each non-redundant facet of a critical region
(Theorem 2 and Corollary 1).
INPUTS:
active_set: active set of the parent critical region
minimal_coincident_facets: list of facets coincident to the minimal facets
(i.e.: [coincident_facets[i] for i in minimal_facets])
OUTPUTS:
candidate_active_sets: list of the candidate active sets for each minimal facet
"""
# initialize list of condidate active sets
candidate_active_sets = []
# cross each non-redundant facet of the parent CR
for coincident_facets in minimal_coincident_facets:
# add or remove each constraint crossed to the active set of the parent CR
candidate_active_set = set(active_set).symmetric_difference(
set(coincident_facets))
candidate_active_sets.append([sorted(list(candidate_active_set))])
return candidate_active_sets
@staticmethod
def expand_candidate_active_sets(candidate_active_sets,
weakly_active_constraints):
"""
Expands the candidate active sets if there are some weakly active contraints (Theorem 5).
INPUTS:
candidate_active_sets: list of the candidate active sets for each minimal facet
weakly_active_constraints: list of weakly active constraints (in the "original enumeration")
OUTPUTS:
candidate_active_sets: list of the candidate active sets for each minimal facet
"""
# determine every possible combination of the weakly active contraints
wac_combinations = []
for n in range(1, len(weakly_active_constraints) + 1):
wac_combinations_n = itertools.combinations(
weakly_active_constraints, n)
wac_combinations += [list(c) for c in wac_combinations_n]
# for each minimal facet of the CR add or remove each combination of wakly active constraints
for i in range(0, len(candidate_active_sets)):
active_set = candidate_active_sets[i][0]
for combination in wac_combinations:
further_active_set = set(active_set).symmetric_difference(
combination)
candidate_active_sets[i].append(
sorted(list(further_active_set)))
return candidate_active_sets
def z_optimal(self, x):
"""
Returns the explicit solution of the mpQP as a function of the parameter.
INPUTS:
x: value of the parameter
OUTPUTS:
z_optimal: solution of the QP
"""
z_optimal = self.z_offset + self.z_linear.dot(x).reshape(
self.z_offset.shape)
return z_optimal
def lambda_optimal(self, x):
"""
Returns the explicit value of the multipliers of the mpQP as a function of the parameter.
INPUTS:
x: value of the parameter
OUTPUTS:
lambda_optimal: optimal multipliers
"""
lambda_A_optimal = self.lambda_A_offset + self.lambda_A_linear.dot(x)
lambda_optimal = np.zeros(len(self.active_set + self.inactive_set))
for i in range(0, len(self.active_set)):
lambda_optimal[self.active_set[i]] = lambda_A_optimal[i]
return lambda_optimal
def applies_to(self, x):
"""
Determines is a given point belongs to the critical region.
INPUTS:
x: value of the parameter
OUTPUTS:
is_inside: flag (True if x is in the CR, False otherwise)
"""
# check if x is inside the polytope
is_inside = self.polytope.applies_to(x)
return is_inside
def reorder_coordinates_visualizer(p):
T = np.array([[0., 1., 0.], [0., 0., 1.], [1., 0., 0.]])
A = p.lhs_min.dot(T)
p_rotated = Polytope(A, p.rhs_min)
p_rotated.assemble()
return p_rotated
def test_MPCHybridController(self):
# PWA dynamics
t_s = .1
A_1 = np.array([[0., 1.], [1., 0.]])
B_1 = np.array([[0.], [1.]])
c_1 = np.array([[0.], [0.]])
sys_1 = ds.DTAffineSystem.from_continuous(A_1, B_1, c_1, t_s)
A_2 = np.array([[0., 1.], [-1., 0.]])
B_2 = B_1
c_2 = np.array([[0.], [1.]])
sys_2 = ds.DTAffineSystem.from_continuous(A_2, B_2, c_2, t_s)
sys = [sys_1, sys_2]
# PWA state domains
x_max_1 = np.array([[1.], [1.5]])
x_max_2 = np.array([[2.], [x_max_1[1, 0]]])
x_min_1 = -x_max_2
x_min_2 = np.array([[x_max_1[0, 0]], [x_min_1[1, 0]]])
X_1 = Polytope.from_bounds(x_min_1, x_max_1)
X_1.assemble()
X_2 = Polytope.from_bounds(x_min_2, x_max_2)
X_2.assemble()
X = [X_1, X_2]
# PWA input domains
u_max = np.array([[1.]])
u_min = -u_max
U_1 = Polytope.from_bounds(u_min, u_max)
U_1.assemble()
U_2 = U_1
U = [U_1, U_2]
# PWA system
pwa_sys = ds.DTPWASystem.from_orthogonal_domains(sys, X, U)
# hybrid controller
N = 10
Q = np.eye(A_1.shape[0])
R = np.eye(B_1.shape[1])
P = Q
X_N = X_1
for objective_norm in ['one', 'two']:
controller = MPCHybridController(pwa_sys, N, objective_norm, Q, R,
P, X_N)
# compare the cost of the MIP and the condensed program
n_test = 100
for i in range(n_test):
x0 = np.random.rand(A_1.shape[0], 1)
u_mip, _, ss, V_mip = controller.feedforward(x0)
if not np.isnan(V_mip):
prog = controller.condense_program(ss)
u_condensed, V_condensed = prog.solve(
x0, u_length=B_1.shape[1])
argmin_error = max([
np.linalg.norm(u_mip[i] - u_condensed[i])
for i in range(len(u_mip))
])
# if not argmin_error < 1.e-5:
# print argmin_error, objective_norm
self.assertTrue(np.isclose(V_mip, V_condensed))
if objective_norm == 'two':
self.assertTrue(argmin_error < 1.e-4)
import numpy as np
from pympc.geometry.polytope import Polytope
import matplotlib.pyplot as plt
from pympc.geometry.convex_hull import PolytopeProjectionInnerApproximation
n_var = 5
n_cons = 100
# A = np.random.randn(n_cons, n_var)
# b = np.random.rand(n_cons, 1)
# np.save('A_random', A)
# np.save('b_random', b)
A = np.load('A_random.npy')
b = np.load('b_random.npy')
p0 = Polytope(A, b)
p0.assemble()
app = PolytopeProjectionInnerApproximation(A, b, [0, 1])
p1 = Polytope(app.A, app.b)
p1.assemble()
p0.plot()
p1.plot()
plt.show()
for point in [np.array([[0.], [.1]]), np.array([[-.13], [.0]])]:
print 'aaa'
p0.plot()
app.include_point(point)
def visualize_environment(self, vis):
z_lim = [-.3, .3]
environment_limits = np.ones((2, 1)) * .6
for limb in self.moving_limbs:
for i, moving_limb in enumerate(self.topology['moving'][limb]):
if moving_limb.contact_surface is not None:
contact_domain = Polytope(moving_limb.A, moving_limb.b)
contact_domain.add_bounds(-environment_limits,
environment_limits)
contact_domain.assemble()
contact_domain = extrude_2d_polytope(contact_domain, z_lim)
vis = visualize_3d_polytope(contact_domain,
'w_' + limb + '_' + str(i),
vis)
for limb in self.fixed_limbs:
fixed_limb = self.topology['fixed'][limb]
b = fixed_limb.normal.T.dot(fixed_limb.position)
contact_domain = Polytope(fixed_limb.normal.T, b)
contact_domain.add_bounds(-environment_limits, environment_limits)
contact_domain.assemble()
contact_domain = extrude_2d_polytope(contact_domain, z_lim)
vis = visualize_3d_polytope(contact_domain, 'w_' + limb, vis)
return vis
