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