def test_cartesian_product(self): point = mut.Point(np.array([11.1, 12.2, 13.3])) h_box = mut.HPolyhedron.MakeBox( lb=[-1, -1, -1], ub=[1, 1, 1]) sum = mut.CartesianProduct(setA=point, setB=h_box) self.assertEqual(sum.ambient_dimension(), 6) self.assertEqual(sum.num_factors(), 2) sum2 = mut.CartesianProduct(sets=[point, h_box]) self.assertEqual(sum2.ambient_dimension(), 6) self.assertEqual(sum2.num_factors(), 2) self.assertIsInstance(sum2.factor(0), mut.Point) sum2 = mut.CartesianProduct( sets=[point, h_box], A=np.eye(6, 3), b=[0, 1, 2, 3, 4, 5]) self.assertEqual(sum2.ambient_dimension(), 3) self.assertEqual(sum2.num_factors(), 2) self.assertIsInstance(sum2.factor(1), mut.HPolyhedron)
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)