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