def test_h_polyhedron(self):
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), self.A)
np.testing.assert_array_equal(hpoly.b(), self.b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
self.assertFalse(hpoly.IsBounded())
hpoly.AddPointInSetConstraints(self.prog, self.x)
with self.assertRaisesRegex(
RuntimeError, ".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
h_box = mut.HPolyhedron.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(h_box.IntersectsWith(hpoly))
h_unit_box = mut.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
self.assertIsInstance(
h_box.MaximumVolumeInscribedEllipsoid(),
mut.Hyperellipsoid)
np.testing.assert_array_almost_equal(
h_box.ChebyshevCenter(), [0, 0, 0])
h2 = h_box.CartesianProduct(other=h_unit_box)
self.assertIsInstance(h2, mut.HPolyhedron)
self.assertEqual(h2.ambient_dimension(), 6)
h3 = h_box.CartesianPower(n=3)
self.assertIsInstance(h3, mut.HPolyhedron)
self.assertEqual(h3.ambient_dimension(), 9)
def test_minkowski_sum(self):
point = mut.Point(np.array([11.1, 12.2, 13.3]))
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
sum = mut.MinkowskiSum(setA=point, setB=hpoly)
self.assertEqual(sum.ambient_dimension(), 3)
self.assertEqual(sum.num_terms(), 2)
sum2 = mut.MinkowskiSum(sets=[point, hpoly])
self.assertEqual(sum2.ambient_dimension(), 3)
self.assertEqual(sum2.num_terms(), 2)
self.assertIsInstance(sum2.term(0), mut.Point)
def test_h_polyhedron(self):
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), self.A)
np.testing.assert_array_equal(hpoly.b(), self.b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
self.assertFalse(hpoly.IsBounded())
hpoly.AddPointInSetConstraints(self.prog, self.x)
constraints = hpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x, t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = hpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay,
b=self.by,
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
with self.assertRaisesRegex(RuntimeError,
".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
assert_pickle(self, hpoly, lambda S: np.vstack((S.A(), S.b())))
h_box = mut.HPolyhedron.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(h_box.IntersectsWith(hpoly))
h_unit_box = mut.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
self.assertIsInstance(h_box.MaximumVolumeInscribedEllipsoid(),
mut.Hyperellipsoid)
np.testing.assert_array_almost_equal(h_box.ChebyshevCenter(),
[0, 0, 0])
h2 = h_box.CartesianProduct(other=h_unit_box)
self.assertIsInstance(h2, mut.HPolyhedron)
self.assertEqual(h2.ambient_dimension(), 6)
h3 = h_box.CartesianPower(n=3)
self.assertIsInstance(h3, mut.HPolyhedron)
self.assertEqual(h3.ambient_dimension(), 9)
h4 = h_box.Intersection(other=h_unit_box)
self.assertIsInstance(h4, mut.HPolyhedron)
self.assertEqual(h4.ambient_dimension(), 3)
h5 = h_box.PontryaginDifference(other=h_unit_box)
self.assertIsInstance(h5, mut.HPolyhedron)
np.testing.assert_array_equal(h5.A(), h_box.A())
np.testing.assert_array_equal(h5.b(), np.zeros(6))
def test_make_from_scene_graph_and_iris(self):
"""
Tests the make from scene graph and iris functionality together as
the Iris code makes obstacles from geometries registered in SceneGraph.
"""
scene_graph = SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(
source_id=source_id, frame=GeometryFrame("frame"))
box_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Box(1., 1., 1.),
name="box"))
cylinder_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Cylinder(1., 1.),
name="cylinder"))
sphere_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Sphere(1.), name="sphere"))
capsule_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Capsule(1., 1.0),
name="capsule"))
context = scene_graph.CreateDefaultContext()
pose_vector = FramePoseVector()
pose_vector.set_value(frame_id, RigidTransform())
scene_graph.get_source_pose_port(source_id).FixValue(
context, pose_vector)
query_object = scene_graph.get_query_output_port().Eval(context)
H = mut.HPolyhedron(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(H.ambient_dimension(), 3)
C = mut.CartesianProduct(
query_object=query_object, geometry_id=cylinder_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(C.ambient_dimension(), 3)
E = mut.Hyperellipsoid(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(E.ambient_dimension(), 3)
S = mut.MinkowskiSum(
query_object=query_object, geometry_id=capsule_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(S.ambient_dimension(), 3)
P = mut.Point(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id(),
maximum_allowable_radius=1.5)
self.assertEqual(P.ambient_dimension(), 3)
V = mut.VPolytope(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(V.ambient_dimension(), 3)
obstacles = mut.MakeIrisObstacles(
query_object=query_object,
reference_frame=scene_graph.world_frame_id())
options = mut.IrisOptions()
options.require_sample_point_is_contained = True
options.iteration_limit = 1
options.termination_threshold = 0.1
options.relative_termination_threshold = 0.01
self.assertNotIn("object at 0x", repr(options))
region = mut.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.HPolyhedron)
obstacles = [
mut.HPolyhedron.MakeUnitBox(3),
mut.Hyperellipsoid.MakeUnitBall(3),
mut.Point([0, 0, 0]),
mut.VPolytope.MakeUnitBox(3)]
region = mut.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.HPolyhedron)
def test_h_polyhedron(self):
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), self.A)
np.testing.assert_array_equal(hpoly.b(), self.b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
self.assertFalse(hpoly.IsBounded())
hpoly.AddPointInSetConstraints(self.prog, self.x)
constraints = hpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x, t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = hpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay,
b=self.by,
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
with self.assertRaisesRegex(RuntimeError,
".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
assert_pickle(self, hpoly, lambda S: np.vstack((S.A(), S.b())))
h_box = mut.HPolyhedron.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(h_box.IntersectsWith(hpoly))
h_unit_box = mut.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
A_l1 = np.array([[1, 1, 1], [-1, 1, 1], [1, -1, 1], [-1, -1, 1],
[1, 1, -1], [-1, 1, -1], [1, -1, -1], [-1, -1, -1]])
b_l1 = np.ones(8)
h_l1_ball = mut.HPolyhedron.MakeL1Ball(dim=3)
np.testing.assert_array_equal(A_l1, h_l1_ball.A())
np.testing.assert_array_equal(b_l1, h_l1_ball.b())
self.assertIsInstance(h_box.MaximumVolumeInscribedEllipsoid(),
mut.Hyperellipsoid)
np.testing.assert_array_almost_equal(h_box.ChebyshevCenter(),
[0, 0, 0])
h2 = h_box.CartesianProduct(other=h_unit_box)
self.assertIsInstance(h2, mut.HPolyhedron)
self.assertEqual(h2.ambient_dimension(), 6)
h3 = h_box.CartesianPower(n=3)
self.assertIsInstance(h3, mut.HPolyhedron)
self.assertEqual(h3.ambient_dimension(), 9)
h4 = h_box.Intersection(other=h_unit_box)
self.assertIsInstance(h4, mut.HPolyhedron)
self.assertEqual(h4.ambient_dimension(), 3)
h5 = h_box.PontryaginDifference(other=h_unit_box)
self.assertIsInstance(h5, mut.HPolyhedron)
np.testing.assert_array_equal(h5.A(), h_box.A())
np.testing.assert_array_equal(h5.b(), np.zeros(6))
generator = RandomGenerator()
sample = h_box.UniformSample(generator=generator)
self.assertEqual(sample.shape, (3, ))
self.assertEqual(
h_box.UniformSample(generator=generator,
previous_sample=sample).shape, (3, ))
h_half_box = mut.HPolyhedron.MakeBox(lb=[-0.5, -0.5, -0.5],
ub=[0.5, 0.5, 0.5])
self.assertTrue(h_half_box.ContainedIn(other=h_unit_box))
h_half_box2 = h_half_box.Intersection(other=h_unit_box,
check_for_redundancy=True)
self.assertIsInstance(h_half_box2, mut.HPolyhedron)
self.assertEqual(h_half_box2.ambient_dimension(), 3)
np.testing.assert_array_almost_equal(h_half_box2.A(), h_half_box.A())
np.testing.assert_array_almost_equal(h_half_box2.b(), h_half_box.b())
# Intersection of 1/2*unit_box and unit_box and reducing the redundant
# inequalities should result in the 1/2*unit_box.
h_half_box_intersect_unit_box = h_half_box.Intersection(
other=h_unit_box, check_for_redundancy=False)
# Check that the ReduceInequalities binding works.
h_half_box3 = h_half_box_intersect_unit_box.ReduceInequalities()