def test_iris_cspace(self):
limits_urdf = """
<robot name="limits">
<link name="movable">
<collision>
<geometry><box size="1 1 1"/></geometry>
</collision>
</link>
<joint name="movable" type="prismatic">
<axis xyz="1 0 0"/>
<limit lower="-2" upper="2"/>
<parent link="world"/>
<child link="movable"/>
</joint>
</robot>"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant).AddModelFromString(limits_urdf, "urdf")
plant.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
options = mut.IrisOptions()
with catch_drake_warnings(expected_count=1):
region = mut.IrisInConfigurationSpace(
plant=plant, context=plant.GetMyContextFromRoot(context),
sample=[0], options=options)
plant.SetPositions(plant.GetMyMutableContextFromRoot(context), [0])
region = mut.IrisInConfigurationSpace(
plant=plant, context=plant.GetMyContextFromRoot(context),
options=options)
self.assertIsInstance(region, mut.ConvexSet)
self.assertEqual(region.ambient_dimension(), 1)
self.assertTrue(region.PointInSet([1.0]))
self.assertFalse(region.PointInSet([3.0]))
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)
