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