def test_scene_graph_register_geometry(self, T):
SceneGraph = mut.SceneGraph_[T]
scene_graph = SceneGraph()
global_source = scene_graph.RegisterSource("anchored")
global_frame = scene_graph.world_frame_id()
context = scene_graph.CreateDefaultContext()
model_inspector = scene_graph.model_inspector()
query_object = scene_graph.get_query_output_port().Eval(context)
context_inspector = query_object.inspector()
self.assertEqual(model_inspector.num_geometries(), 0)
self.assertEqual(context_inspector.num_geometries(), 0)
# Register with context
geometry = mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere1")
global_geometry = scene_graph.RegisterGeometry(
context=context, source_id=global_source, frame_id=global_frame,
geometry=geometry)
self.assertEqual(model_inspector.num_geometries(), 0)
self.assertEqual(context_inspector.num_geometries(), 1)
# Now register the geometry in scene_graph with a new geometry
new_geometry = mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere1")
scene_graph.RegisterGeometry(
source_id=global_source, frame_id=global_frame,
geometry=new_geometry)
self.assertEqual(model_inspector.num_geometries(), 1)
self.assertEqual(context_inspector.num_geometries(), 1)
def test_shapes(self):
RigidTransform = RigidTransform_[float]
sphere = mut.Sphere(radius=1.0)
self.assertEqual(sphere.radius(), 1.0)
cylinder = mut.Cylinder(radius=1.0, length=2.0)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
box = mut.Box(width=1.0, depth=2.0, height=3.0)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
capsule = mut.Capsule(radius=1.0, length=2.0)
self.assertEqual(capsule.radius(), 1.0)
self.assertEqual(capsule.length(), 2.0)
ellipsoid = mut.Ellipsoid(a=1.0, b=2.0, c=3.0)
self.assertEqual(ellipsoid.a(), 1.0)
self.assertEqual(ellipsoid.b(), 2.0)
self.assertEqual(ellipsoid.c(), 3.0)
X_FH = mut.HalfSpace.MakePose(Hz_dir_F=[0, 1, 0], p_FB=[1, 1, 1])
self.assertIsInstance(X_FH, RigidTransform)
box_mesh_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/meshes/box.obj")
mesh = mut.Mesh(absolute_filename=box_mesh_path, scale=1.0)
self.assertEqual(mesh.filename(), box_mesh_path)
self.assertEqual(mesh.scale(), 1.0)
convex = mut.Convex(absolute_filename=box_mesh_path, scale=1.0)
self.assertEqual(convex.filename(), box_mesh_path)
self.assertEqual(convex.scale(), 1.0)
def test_geometry_instance_api(self):
RigidTransform = RigidTransform_[float]
geometry = mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Sphere(1.),
name="sphere")
self.assertIsInstance(geometry.id(), mut.GeometryId)
geometry.set_pose(RigidTransform([1, 0, 0]))
self.assertIsInstance(geometry.pose(), RigidTransform)
self.assertIsInstance(geometry.shape(), mut.Shape)
self.assertIsInstance(geometry.release_shape(), mut.Shape)
self.assertEqual(geometry.name(), "sphere")
geometry.set_proximity_properties(mut.ProximityProperties())
geometry.set_illustration_properties(mut.IllustrationProperties())
geometry.set_perception_properties(mut.PerceptionProperties())
self.assertIsInstance(geometry.mutable_proximity_properties(),
mut.ProximityProperties)
self.assertIsInstance(geometry.proximity_properties(),
mut.ProximityProperties)
self.assertIsInstance(geometry.mutable_illustration_properties(),
mut.IllustrationProperties)
self.assertIsInstance(geometry.illustration_properties(),
mut.IllustrationProperties)
self.assertIsInstance(geometry.mutable_perception_properties(),
mut.PerceptionProperties)
self.assertIsInstance(geometry.perception_properties(),
mut.PerceptionProperties)
def test_scene_graph_api(self, T):
SceneGraph = mut.SceneGraph_[T]
InputPort = InputPort_[T]
OutputPort = OutputPort_[T]
scene_graph = SceneGraph()
global_source = scene_graph.RegisterSource("anchored")
global_frame = scene_graph.RegisterFrame(
source_id=global_source, frame=mut.GeometryFrame("anchored_frame"))
global_frame_2 = scene_graph.RegisterFrame(
source_id=global_source,
parent_id=global_frame,
frame=mut.GeometryFrame("anchored_frame"))
global_geometry = scene_graph.RegisterGeometry(
source_id=global_source,
frame_id=global_frame,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere"))
global_geometry_2 = scene_graph.RegisterGeometry(
source_id=global_source,
geometry_id=global_geometry,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere"))
anchored_geometry = scene_graph.RegisterAnchoredGeometry(
source_id=global_source,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere"))
self.assertIsInstance(scene_graph.get_source_pose_port(global_source),
InputPort)
self.assertIsInstance(scene_graph.get_pose_bundle_output_port(),
OutputPort)
self.assertIsInstance(scene_graph.get_query_output_port(), OutputPort)
# Test limited rendering API.
scene_graph.AddRenderer(
"test_renderer",
mut.render.MakeRenderEngineVtk(mut.render.RenderEngineVtkParams()))
# Test SceneGraphInspector API
inspector = scene_graph.model_inspector()
self.assertEqual(inspector.num_frames(), 3)
self.assertEqual(inspector.num_sources(), 2)
self.assertEqual(inspector.num_geometries(), 3)
def test_geometry_instance_api(self):
RigidTransform = RigidTransform_[float]
geometry = mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Sphere(1.),
name="sphere")
self.assertIsInstance(geometry.id(), mut.GeometryId)
geometry.set_pose(RigidTransform([1, 0, 0]))
self.assertIsInstance(geometry.pose(), RigidTransform)
self.assertIsInstance(geometry.shape(), mut.Shape)
self.assertIsInstance(geometry.release_shape(), mut.Shape)
self.assertEqual(geometry.name(), "sphere")
def test_shapes(self):
sphere = mut.Sphere(radius=1.0)
self.assertEqual(sphere.radius(), 1.0)
cylinder = mut.Cylinder(radius=1.0, length=2.0)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
box = mut.Box(width=1.0, depth=2.0, height=3.0)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
def test_shape_constructors(self):
box_mesh_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/meshes/box.obj")
shapes = [
mut.Sphere(radius=1.0),
mut.Cylinder(radius=1.0, length=2.0),
mut.Box(width=1.0, depth=2.0, height=3.0),
mut.HalfSpace(),
mut.Mesh(absolute_filename=box_mesh_path, scale=1.0),
mut.Convex(absolute_filename=box_mesh_path, scale=1.0)
]
for shape in shapes:
self.assertIsInstance(shape, mut.Shape)
def test_shapes(self):
sphere = mut.Sphere(radius=1.0)
self.assertEqual(sphere.radius(), 1.0)
cylinder = mut.Cylinder(radius=1.0, length=2.0)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
box = mut.Box(width=1.0, depth=2.0, height=3.0)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
# Test for existence of deprecated accessors.
with catch_drake_warnings(expected_count=3):
cylinder.get_radius()
cylinder.get_length()
sphere.get_radius()
def test_shape_constructors(self):
shapes = [
mut.Sphere(radius=1.0),
mut.Cylinder(radius=1.0, length=2.0),
mut.Box(width=1.0, depth=2.0, height=3.0),
mut.Capsule(radius=1.0, length=2.0),
mut.Ellipsoid(a=1.0, b=2.0, c=3.0),
mut.HalfSpace(),
mut.Mesh(absolute_filename="arbitrary/path", scale=1.0),
mut.Convex(absolute_filename="arbitrary/path", scale=1.0),
mut.MeshcatCone(height=1.23, a=3.45, b=6.78)
]
for shape in shapes:
self.assertIsInstance(shape, mut.Shape)
shape_cls = type(shape)
shape_copy = shape.Clone()
self.assertIsInstance(shape_copy, shape_cls)
self.assertIsNot(shape, shape_copy)
def test_shapes(self):
sphere = mut.Sphere(radius=1.0)
self.assertEqual(sphere.radius(), 1.0)
cylinder = mut.Cylinder(radius=1.0, length=2.0)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
box = mut.Box(width=1.0, depth=2.0, height=3.0)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
box_mesh_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/meshes/box.obj")
mesh = mut.Mesh(absolute_filename=box_mesh_path, scale=1.0)
self.assertEqual(mesh.filename(), box_mesh_path)
self.assertEqual(mesh.scale(), 1.0)
convex = mut.Convex(absolute_filename=box_mesh_path, scale=1.0)
self.assertEqual(convex.filename(), box_mesh_path)
self.assertEqual(convex.scale(), 1.0)
def test_scene_graph_api(self, T):
SceneGraph = mut.SceneGraph_[T]
InputPort = InputPort_[T]
OutputPort = OutputPort_[T]
scene_graph = SceneGraph()
global_source = scene_graph.RegisterSource("anchored")
global_frame = scene_graph.RegisterFrame(
source_id=global_source, frame=mut.GeometryFrame("anchored_frame"))
global_frame_2 = scene_graph.RegisterFrame(
source_id=global_source,
parent_id=global_frame,
frame=mut.GeometryFrame("anchored_frame"))
global_geometry = scene_graph.RegisterGeometry(
source_id=global_source,
frame_id=global_frame,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere"))
global_geometry_2 = scene_graph.RegisterGeometry(
source_id=global_source,
geometry_id=global_geometry,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere"))
anchored_geometry = scene_graph.RegisterAnchoredGeometry(
source_id=global_source,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere"))
self.assertIsInstance(scene_graph.get_source_pose_port(global_source),
InputPort)
self.assertIsInstance(scene_graph.get_pose_bundle_output_port(),
OutputPort)
self.assertIsInstance(scene_graph.get_query_output_port(), OutputPort)
# Test limited rendering API.
scene_graph.AddRenderer(
"test_renderer",
mut.render.MakeRenderEngineVtk(mut.render.RenderEngineVtkParams()))
self.assertTrue(scene_graph.HasRenderer("test_renderer"))
self.assertEqual(scene_graph.RendererCount(), 1)
# Test SceneGraphInspector API
inspector = scene_graph.model_inspector()
self.assertEqual(inspector.num_frames(), 3)
self.assertEqual(inspector.num_sources(), 2)
self.assertEqual(inspector.num_geometries(), 3)
# Check AssignRole bits.
proximity = mut.ProximityProperties()
perception = mut.PerceptionProperties()
perception.AddProperty("label", "id", mut.render.RenderLabel(0))
illustration = mut.IllustrationProperties()
props = [
proximity,
perception,
illustration,
]
context = scene_graph.CreateDefaultContext()
for prop in props:
# Check SceneGraph mutating variant.
scene_graph.AssignRole(source_id=global_source,
geometry_id=global_geometry,
properties=prop,
assign=mut.RoleAssign.kNew)
# Check Context mutating variant.
scene_graph.AssignRole(context=context,
source_id=global_source,
geometry_id=global_geometry,
properties=prop,
assign=mut.RoleAssign.kNew)
# Check property accessors.
self.assertIsInstance(
inspector.GetProximityProperties(geometry_id=global_geometry),
mut.ProximityProperties)
self.assertIsInstance(
inspector.GetIllustrationProperties(geometry_id=global_geometry),
mut.IllustrationProperties)
self.assertIsInstance(
inspector.GetPerceptionProperties(geometry_id=global_geometry),
mut.PerceptionProperties)
def test_scene_graph_api(self, T):
SceneGraph = mut.SceneGraph_[T]
InputPort = InputPort_[T]
OutputPort = OutputPort_[T]
scene_graph = SceneGraph()
global_source = scene_graph.RegisterSource("anchored")
global_frame = scene_graph.RegisterFrame(
source_id=global_source, frame=mut.GeometryFrame("anchored_frame"))
scene_graph.RegisterFrame(
source_id=global_source, parent_id=global_frame,
frame=mut.GeometryFrame("anchored_frame"))
global_geometry = scene_graph.RegisterGeometry(
source_id=global_source, frame_id=global_frame,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere1"))
scene_graph.RegisterGeometry(
source_id=global_source, geometry_id=global_geometry,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere2"))
scene_graph.RegisterAnchoredGeometry(
source_id=global_source,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere3"))
self.assertIsInstance(
scene_graph.get_source_pose_port(global_source), InputPort)
self.assertIsInstance(
scene_graph.get_pose_bundle_output_port(), OutputPort)
self.assertIsInstance(
scene_graph.get_query_output_port(), OutputPort)
# Test limited rendering API.
scene_graph.AddRenderer("test_renderer",
mut.render.MakeRenderEngineVtk(
mut.render.RenderEngineVtkParams()))
self.assertTrue(scene_graph.HasRenderer("test_renderer"))
self.assertEqual(scene_graph.RendererCount(), 1)
# Test SceneGraphInspector API
inspector = scene_graph.model_inspector()
self.assertEqual(inspector.num_sources(), 2)
self.assertEqual(inspector.num_frames(), 3)
self.assertEqual(inspector.num_geometries(), 3)
self.assertEqual(len(inspector.GetAllGeometryIds()), 3)
self.assertEqual(
inspector.NumGeometriesWithRole(role=mut.Role.kUnassigned), 3)
self.assertEqual(inspector.NumDynamicGeometries(), 2)
self.assertEqual(inspector.NumAnchoredGeometries(), 1)
self.assertTrue(inspector.SourceIsRegistered(id=global_source))
self.assertEqual(inspector.GetSourceName(id=global_source), "anchored")
self.assertEqual(inspector.GetFrameId(global_geometry), global_frame)
self.assertEqual(len(inspector.GetGeometries(frame_id=global_frame)),
2)
self.assertTrue(
global_geometry in inspector.GetGeometries(frame_id=global_frame))
self.assertEqual(
len(inspector.GetGeometries(frame_id=global_frame,
role=mut.Role.kProximity)),
0)
self.assertEqual(
inspector.GetGeometryIdByName(frame_id=global_frame,
role=mut.Role.kUnassigned,
name="sphere1"),
global_geometry)
self.assertEqual(
inspector.GetName(frame_id=global_frame), "anchored_frame")
self.assertEqual(
inspector.GetName(geometry_id=global_geometry), "sphere1")
self.assertIsInstance(
inspector.GetPoseInParent(geometry_id=global_geometry),
RigidTransform_[float])
self.assertIsInstance(
inspector.GetPoseInFrame(geometry_id=global_geometry),
RigidTransform_[float])
self.assertIsInstance(inspector.geometry_version(),
mut.GeometryVersion)
# Check AssignRole bits.
proximity = mut.ProximityProperties()
perception = mut.PerceptionProperties()
perception.AddProperty("label", "id", mut.render.RenderLabel(0))
illustration = mut.IllustrationProperties()
props = [
proximity,
perception,
illustration,
]
context = scene_graph.CreateDefaultContext()
for prop in props:
# Check SceneGraph mutating variant.
scene_graph.AssignRole(
source_id=global_source, geometry_id=global_geometry,
properties=prop, assign=mut.RoleAssign.kNew)
# Check Context mutating variant.
scene_graph.AssignRole(
context=context, source_id=global_source,
geometry_id=global_geometry, properties=prop,
assign=mut.RoleAssign.kNew)
# Check property accessors.
self.assertIsInstance(
inspector.GetProximityProperties(geometry_id=global_geometry),
mut.ProximityProperties)
self.assertIsInstance(
inspector.GetIllustrationProperties(geometry_id=global_geometry),
mut.IllustrationProperties)
self.assertIsInstance(
inspector.GetPerceptionProperties(geometry_id=global_geometry),
mut.PerceptionProperties)
self.assertIsInstance(
inspector.CloneGeometryInstance(geometry_id=global_geometry),
mut.GeometryInstance)
roles = [
mut.Role.kProximity,
mut.Role.kPerception,
mut.Role.kIllustration,
]
for role in roles:
self.assertEqual(
scene_graph.RemoveRole(
source_id=global_source, geometry_id=global_geometry,
role=role),
1)
def build_mbp(seed=0, verts_geom=False, convex_collision_geom=True):
# Make some random lumpy objects
trimeshes = []
np.random.seed(42)
for k in range(3):
# Make a small random number of triangles and chull it
# to get a lumpy object
mesh = trimesh.creation.random_soup(5)
mesh = trimesh.convex.convex_hull(mesh)
trimeshes.append(mesh)
# Create Drake geometry from those objects by adding a small
# sphere at each vertex
sphere_rad = 0.05
cmap = plt.cm.get_cmap('jet')
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.001))
# Add ground
friction = CoulombFriction(0.9, 0.8)
g = pydrake_geom.Box(100., 100., 0.5)
tf = RigidTransform(p=[0., 0., -0.25])
mbp.RegisterVisualGeometry(body=mbp.world_body(),
X_BG=tf,
shape=g,
name="ground",
diffuse_color=[1.0, 1.0, 1.0, 1.0])
mbp.RegisterCollisionGeometry(body=mbp.world_body(),
X_BG=tf,
shape=g,
name="ground",
coulomb_friction=friction)
for i, mesh in enumerate(trimeshes):
inertia = SpatialInertia(mass=1.0,
p_PScm_E=np.zeros(3),
G_SP_E=UnitInertia(0.01, 0.01, 0.01))
body = mbp.AddRigidBody(name="body_%d" % i, M_BBo_B=inertia)
color = cmap(np.random.random())
if verts_geom:
for j, vert in enumerate(mesh.vertices):
g = pydrake_geom.Sphere(radius=sphere_rad)
tf = RigidTransform(p=vert)
mbp.RegisterVisualGeometry(body=body,
X_BG=tf,
shape=g,
name="body_%d_color_%d" % (i, j),
diffuse_color=color)
mbp.RegisterCollisionGeometry(body=body,
X_BG=tf,
shape=g,
name="body_%d_collision_%d" %
(i, j),
coulomb_friction=friction)
# And add mesh itself for vis
path = "/tmp/part_%d.obj" % i
trimesh.exchange.export.export_mesh(mesh, path)
g = pydrake_geom.Convex(path)
mbp.RegisterVisualGeometry(body=body,
X_BG=RigidTransform(),
shape=g,
name="body_%d_base" % i,
diffuse_color=color)
if convex_collision_geom:
mbp.RegisterCollisionGeometry(body=body,
X_BG=RigidTransform(),
shape=g,
name="body_%d_base_col" % i,
coulomb_friction=friction)
mbp.SetDefaultFreeBodyPose(body, RigidTransform(p=[i % 3, i / 3., 1.]))
mbp.Finalize()
return builder, mbp, scene_graph
def test_optimization(self):
"""Tests geometry::optimization bindings"""
A = np.eye(3)
b = [1.0, 1.0, 1.0]
prog = MathematicalProgram()
x = prog.NewContinuousVariables(3, "x")
t = prog.NewContinuousVariables(1, "t")
# Test Point.
p = np.array([11.1, 12.2, 13.3])
point = mut.optimization.Point(p)
self.assertEqual(point.ambient_dimension(), 3)
np.testing.assert_array_equal(point.x(), p)
point.set_x(x=2*p)
np.testing.assert_array_equal(point.x(), 2*p)
point.set_x(x=p)
# Test HPolyhedron.
hpoly = mut.optimization.HPolyhedron(A=A, b=b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), A)
np.testing.assert_array_equal(hpoly.b(), b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
hpoly.AddPointInSetConstraints(prog, x)
with self.assertRaisesRegex(
RuntimeError, ".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
h_box = mut.optimization.HPolyhedron.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
h_unit_box = mut.optimization.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.optimization.Hyperellipsoid)
np.testing.assert_array_almost_equal(
h_box.ChebyshevCenter(), [0, 0, 0])
# Test Hyperellipsoid.
ellipsoid = mut.optimization.Hyperellipsoid(A=A, center=b)
self.assertEqual(ellipsoid.ambient_dimension(), 3)
np.testing.assert_array_equal(ellipsoid.A(), A)
np.testing.assert_array_equal(ellipsoid.center(), b)
self.assertTrue(ellipsoid.PointInSet(x=b, tol=0.0))
ellipsoid.AddPointInSetConstraints(prog, x)
shape, pose = ellipsoid.ToShapeWithPose()
self.assertIsInstance(shape, mut.Ellipsoid)
self.assertIsInstance(pose, RigidTransform)
scale, witness = ellipsoid.MinimumUniformScalingToTouch(point)
self.assertTrue(scale > 0.0)
np.testing.assert_array_almost_equal(witness, p)
e_ball = mut.optimization.Hyperellipsoid.MakeAxisAligned(
radius=[1, 1, 1], center=b)
np.testing.assert_array_equal(e_ball.A(), A)
np.testing.assert_array_equal(e_ball.center(), b)
e_ball2 = mut.optimization.Hyperellipsoid.MakeHypersphere(
radius=1, center=b)
np.testing.assert_array_equal(e_ball2.A(), A)
np.testing.assert_array_equal(e_ball2.center(), b)
e_ball3 = mut.optimization.Hyperellipsoid.MakeUnitBall(dim=3)
np.testing.assert_array_equal(e_ball3.A(), A)
np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])
# Test VPolytope.
vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]])
vpoly = mut.optimization.VPolytope(vertices=vertices)
self.assertEqual(vpoly.ambient_dimension(), 2)
np.testing.assert_array_equal(vpoly.vertices(), vertices)
self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8))
vpoly.AddPointInSetConstraints(prog, x[0:2])
v_box = mut.optimization.VPolytope.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(v_box.PointInSet([0, 0, 0]))
v_unit_box = mut.optimization.VPolytope.MakeUnitBox(dim=3)
self.assertTrue(v_unit_box.PointInSet([0, 0, 0]))
# Test remaining ConvexSet methods using these instances.
self.assertIsInstance(hpoly.Clone(), mut.optimization.HPolyhedron)
self.assertTrue(ellipsoid.IsBounded())
hpoly.AddPointInNonnegativeScalingConstraints(prog=prog, x=x, t=t[0])
# Test MakeFromSceneGraph methods.
scene_graph = mut.SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(
source_id=source_id, frame=mut.GeometryFrame("frame"))
box_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Box(1., 1., 1.),
name="sphere"))
sphere_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Sphere(1.), name="sphere"))
context = scene_graph.CreateDefaultContext()
pose_vector = mut.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.optimization.HPolyhedron(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(H.ambient_dimension(), 3)
E = mut.optimization.Hyperellipsoid(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(E.ambient_dimension(), 3)
P = mut.optimization.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.optimization.VPolytope(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(V.ambient_dimension(), 3)
# Test Iris.
obstacles = mut.optimization.MakeIrisObstacles(
query_object=query_object,
reference_frame=scene_graph.world_frame_id())
options = mut.optimization.IrisOptions()
options.require_sample_point_is_contained = True
options.iteration_limit = 1
options.termination_threshold = 0.1
region = mut.optimization.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.optimization.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.optimization.HPolyhedron)
obstacles = [
mut.optimization.HPolyhedron.MakeUnitBox(3),
mut.optimization.Hyperellipsoid.MakeUnitBall(3),
mut.optimization.Point([0, 0, 0]),
mut.optimization.VPolytope.MakeUnitBox(3)]
region = mut.optimization.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.optimization.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.optimization.HPolyhedron)
def test_shapes(self):
# We'll test some invariants on all shapes as inherited from the Shape
# API.
def assert_shape_api(shape):
self.assertIsInstance(shape, mut.Shape)
shape_cls = type(shape)
shape_copy = shape.Clone()
self.assertIsInstance(shape_copy, shape_cls)
self.assertIsNot(shape, shape_copy)
# Note: these are ordered alphabetical order and not in the declared
# order in shape_specification.h
box = mut.Box(width=1.0, depth=2.0, height=3.0)
assert_shape_api(box)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
assert_pickle(
self, box,
lambda shape: [shape.width(
), shape.depth(), shape.height()])
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
self.assertAlmostEqual(mut.CalcVolume(box), 6.0, 1e-14)
capsule = mut.Capsule(radius=1.0, length=2.0)
assert_shape_api(capsule)
self.assertEqual(capsule.radius(), 1.0)
self.assertEqual(capsule.length(), 2.0)
assert_pickle(
self, capsule,
lambda shape: [shape.radius(), shape.length()])
junk_path = "arbitrary/path"
convex = mut.Convex(absolute_filename=junk_path, scale=1.0)
assert_shape_api(convex)
self.assertEqual(convex.filename(), junk_path)
self.assertEqual(convex.scale(), 1.0)
assert_pickle(
self, convex,
lambda shape: [shape.filename(), shape.scale()])
cylinder = mut.Cylinder(radius=1.0, length=2.0)
assert_shape_api(cylinder)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
assert_pickle(
self, cylinder,
lambda shape: [shape.radius(), shape.length()])
ellipsoid = mut.Ellipsoid(a=1.0, b=2.0, c=3.0)
assert_shape_api(ellipsoid)
self.assertEqual(ellipsoid.a(), 1.0)
self.assertEqual(ellipsoid.b(), 2.0)
self.assertEqual(ellipsoid.c(), 3.0)
assert_pickle(self, ellipsoid, lambda shape:
[shape.a(), shape.b(), shape.c()])
X_FH = mut.HalfSpace.MakePose(Hz_dir_F=[0, 1, 0], p_FB=[1, 1, 1])
self.assertIsInstance(X_FH, RigidTransform)
mesh = mut.Mesh(absolute_filename=junk_path, scale=1.0)
assert_shape_api(mesh)
self.assertEqual(mesh.filename(), junk_path)
self.assertEqual(mesh.scale(), 1.0)
assert_pickle(
self, mesh,
lambda shape: [shape.filename(), shape.scale()])
sphere = mut.Sphere(radius=1.0)
assert_shape_api(sphere)
self.assertEqual(sphere.radius(), 1.0)
assert_pickle(self, sphere, mut.Sphere.radius)
cone = mut.MeshcatCone(height=1.2, a=3.4, b=5.6)
assert_shape_api(cone)
self.assertEqual(cone.height(), 1.2)
self.assertEqual(cone.a(), 3.4)
self.assertEqual(cone.b(), 5.6)
assert_pickle(self, cone,
lambda shape: [shape.height(
), shape.a(), shape.b()])
def test_query_object(self, T):
RigidTransform = RigidTransform_[float]
SceneGraph = mut.SceneGraph_[T]
QueryObject = mut.QueryObject_[T]
SceneGraphInspector = mut.SceneGraphInspector_[T]
FramePoseVector = mut.FramePoseVector_[T]
# First, ensure we can default-construct it.
model = QueryObject()
self.assertIsInstance(model, QueryObject)
scene_graph = SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(
source_id=source_id, frame=mut.GeometryFrame("frame"))
geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Sphere(1.), name="sphere"))
render_params = mut.render.RenderEngineVtkParams()
renderer_name = "test_renderer"
scene_graph.AddRenderer(renderer_name,
mut.render.MakeRenderEngineVtk(render_params))
context = scene_graph.CreateDefaultContext()
pose_vector = FramePoseVector()
pose_vector.set_value(frame_id, RigidTransform_[T]())
scene_graph.get_source_pose_port(source_id).FixValue(
context, pose_vector)
query_object = scene_graph.get_query_output_port().Eval(context)
self.assertIsInstance(query_object.inspector(), SceneGraphInspector)
self.assertIsInstance(
query_object.GetPoseInWorld(frame_id=frame_id), RigidTransform_[T])
self.assertIsInstance(
query_object.GetPoseInParent(frame_id=frame_id),
RigidTransform_[T])
self.assertIsInstance(
query_object.GetPoseInWorld(geometry_id=geometry_id),
RigidTransform_[T])
# Proximity queries -- all of these will produce empty results.
results = query_object.ComputeSignedDistancePairwiseClosestPoints()
self.assertEqual(len(results), 0)
results = query_object.ComputePointPairPenetration()
self.assertEqual(len(results), 0)
results = query_object.ComputeSignedDistanceToPoint(p_WQ=(1, 2, 3))
self.assertEqual(len(results), 0)
results = query_object.FindCollisionCandidates()
self.assertEqual(len(results), 0)
self.assertFalse(query_object.HasCollisions())
# ComputeSignedDistancePairClosestPoints() requires two valid geometry
# ids. There are none in this SceneGraph instance. Rather than
# populating the SceneGraph, we look for the exception thrown in
# response to invalid ids as evidence of correct binding.
with self.assertRaisesRegex(
RuntimeError,
r"The geometry given by id \d+ does not reference a geometry"
+ " that can be used in a signed distance query"):
query_object.ComputeSignedDistancePairClosestPoints(
geometry_id_A=mut.GeometryId.get_new_id(),
geometry_id_B=mut.GeometryId.get_new_id())
# Confirm rendering API returns images of appropriate type.
camera_core = mut.render.RenderCameraCore(
renderer_name=renderer_name,
intrinsics=CameraInfo(width=10, height=10, fov_y=pi/6),
clipping=mut.render.ClippingRange(0.1, 10.0),
X_BS=RigidTransform())
color_camera = mut.render.ColorRenderCamera(
core=camera_core, show_window=False)
depth_camera = mut.render.DepthRenderCamera(
core=camera_core, depth_range=mut.render.DepthRange(0.1, 5.0))
image = query_object.RenderColorImage(
camera=color_camera, parent_frame=SceneGraph.world_frame_id(),
X_PC=RigidTransform())
self.assertIsInstance(image, ImageRgba8U)
image = query_object.RenderDepthImage(
camera=depth_camera, parent_frame=SceneGraph.world_frame_id(),
X_PC=RigidTransform())
self.assertIsInstance(image, ImageDepth32F)
image = query_object.RenderLabelImage(
camera=color_camera, parent_frame=SceneGraph.world_frame_id(),
X_PC=RigidTransform())
self.assertIsInstance(image, ImageLabel16I)
def test_scene_graph_api(self, T):
SceneGraph = mut.SceneGraph_[T]
InputPort = InputPort_[T]
OutputPort = OutputPort_[T]
scene_graph = SceneGraph()
global_source = scene_graph.RegisterSource("anchored")
global_frame = scene_graph.RegisterFrame(
source_id=global_source, frame=mut.GeometryFrame("anchored_frame"))
scene_graph.RegisterFrame(source_id=global_source,
parent_id=global_frame,
frame=mut.GeometryFrame("anchored_frame"))
global_geometry = scene_graph.RegisterGeometry(
source_id=global_source,
frame_id=global_frame,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere1"))
scene_graph.RegisterGeometry(source_id=global_source,
geometry_id=global_geometry,
geometry=mut.GeometryInstance(
X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere2"))
scene_graph.RegisterAnchoredGeometry(source_id=global_source,
geometry=mut.GeometryInstance(
X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere3"))
self.assertIsInstance(scene_graph.get_source_pose_port(global_source),
InputPort)
self.assertIsInstance(scene_graph.get_pose_bundle_output_port(),
OutputPort)
self.assertIsInstance(scene_graph.get_query_output_port(), OutputPort)
# Test limited rendering API.
scene_graph.AddRenderer(
"test_renderer",
mut.render.MakeRenderEngineVtk(mut.render.RenderEngineVtkParams()))
self.assertTrue(scene_graph.HasRenderer("test_renderer"))
self.assertEqual(scene_graph.RendererCount(), 1)
# Test SceneGraphInspector API
inspector = scene_graph.model_inspector()
self.assertEqual(inspector.num_sources(), 2)
self.assertEqual(inspector.num_frames(), 3)
self.assertEqual(len(inspector.all_frame_ids()), 3)
self.assertTrue(
inspector.world_frame_id() in inspector.all_frame_ids())
self.assertTrue(global_frame in inspector.all_frame_ids())
self.assertIsInstance(inspector.world_frame_id(), mut.FrameId)
self.assertEqual(inspector.num_geometries(), 3)
self.assertEqual(len(inspector.GetAllGeometryIds()), 3)
self.assertEqual(
inspector.NumGeometriesWithRole(role=mut.Role.kUnassigned), 3)
self.assertEqual(inspector.NumDynamicGeometries(), 2)
self.assertEqual(inspector.NumAnchoredGeometries(), 1)
self.assertEqual(len(inspector.GetCollisionCandidates()), 0)
self.assertTrue(inspector.SourceIsRegistered(source_id=global_source))
# TODO(SeanCurtis-TRI) Remove this call at the same time as deprecating
# the subsequent deprecation tests; it is only here to show that the
# non-keyword call invokes the non-deprecated overload.
self.assertTrue(inspector.SourceIsRegistered(global_source))
with catch_drake_warnings(expected_count=2):
self.assertTrue(inspector.SourceIsRegistered(id=global_source))
self.assertEqual(inspector.GetSourceName(source_id=global_source),
"anchored")
self.assertEqual(inspector.NumFramesForSource(source_id=global_source),
2)
self.assertTrue(global_frame in inspector.FramesForSource(
source_id=global_source))
self.assertTrue(
inspector.BelongsToSource(frame_id=global_frame,
source_id=global_source))
self.assertEqual(inspector.GetOwningSourceName(frame_id=global_frame),
"anchored")
self.assertEqual(inspector.GetName(frame_id=global_frame),
"anchored_frame")
self.assertEqual(inspector.GetFrameGroup(frame_id=global_frame), 0)
self.assertEqual(
inspector.NumGeometriesForFrame(frame_id=global_frame), 2)
self.assertEqual(
inspector.NumGeometriesForFrameWithRole(frame_id=global_frame,
role=mut.Role.kProximity),
0)
self.assertEqual(len(inspector.GetGeometries(frame_id=global_frame)),
2)
self.assertTrue(global_geometry in inspector.GetGeometries(
frame_id=global_frame))
self.assertEqual(
len(
inspector.GetGeometries(frame_id=global_frame,
role=mut.Role.kProximity)), 0)
self.assertEqual(
inspector.GetGeometryIdByName(frame_id=global_frame,
role=mut.Role.kUnassigned,
name="sphere1"), global_geometry)
self.assertTrue(
inspector.BelongsToSource(geometry_id=global_geometry,
source_id=global_source))
self.assertEqual(
inspector.GetOwningSourceName(geometry_id=global_geometry),
"anchored")
self.assertEqual(inspector.GetFrameId(global_geometry), global_frame)
self.assertEqual(inspector.GetName(geometry_id=global_geometry),
"sphere1")
self.assertIsInstance(inspector.GetShape(geometry_id=global_geometry),
mut.Sphere)
self.assertIsInstance(
inspector.GetPoseInParent(geometry_id=global_geometry),
RigidTransform_[float])
self.assertIsInstance(
inspector.GetPoseInFrame(geometry_id=global_geometry),
RigidTransform_[float])
self.assertIsInstance(inspector.geometry_version(),
mut.GeometryVersion)
# Check AssignRole bits.
proximity = mut.ProximityProperties()
perception = mut.PerceptionProperties()
perception.AddProperty("label", "id", mut.render.RenderLabel(0))
illustration = mut.IllustrationProperties()
props = [
proximity,
perception,
illustration,
]
context = scene_graph.CreateDefaultContext()
for prop in props:
# Check SceneGraph mutating variant.
scene_graph.AssignRole(source_id=global_source,
geometry_id=global_geometry,
properties=prop,
assign=mut.RoleAssign.kNew)
# Check Context mutating variant.
scene_graph.AssignRole(context=context,
source_id=global_source,
geometry_id=global_geometry,
properties=prop,
assign=mut.RoleAssign.kNew)
# Check property accessors.
self.assertIsInstance(
inspector.GetProximityProperties(geometry_id=global_geometry),
mut.ProximityProperties)
self.assertIsInstance(
inspector.GetProperties(geometry_id=global_geometry,
role=mut.Role.kProximity),
mut.ProximityProperties)
self.assertIsInstance(
inspector.GetIllustrationProperties(geometry_id=global_geometry),
mut.IllustrationProperties)
self.assertIsInstance(
inspector.GetProperties(geometry_id=global_geometry,
role=mut.Role.kIllustration),
mut.IllustrationProperties)
self.assertIsInstance(
inspector.GetPerceptionProperties(geometry_id=global_geometry),
mut.PerceptionProperties)
self.assertIsInstance(
inspector.GetProperties(geometry_id=global_geometry,
role=mut.Role.kPerception),
mut.PerceptionProperties)
self.assertIsInstance(
inspector.CloneGeometryInstance(geometry_id=global_geometry),
mut.GeometryInstance)
self.assertTrue(
inspector.CollisionFiltered(geometry_id1=global_geometry,
geometry_id2=global_geometry))
roles = [
mut.Role.kProximity,
mut.Role.kPerception,
mut.Role.kIllustration,
]
for role in roles:
self.assertEqual(
scene_graph.RemoveRole(source_id=global_source,
geometry_id=global_geometry,
role=role), 1)
def test_scene_graph_api(self, T):
SceneGraph = mut.SceneGraph_[T]
InputPort = InputPort_[T]
OutputPort = OutputPort_[T]
scene_graph = SceneGraph()
global_source = scene_graph.RegisterSource("anchored")
global_frame = scene_graph.RegisterFrame(
source_id=global_source, frame=mut.GeometryFrame("anchored_frame"))
scene_graph.RegisterFrame(
source_id=global_source, parent_id=global_frame,
frame=mut.GeometryFrame("anchored_frame"))
global_geometry = scene_graph.RegisterGeometry(
source_id=global_source, frame_id=global_frame,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere1"))
scene_graph.RegisterGeometry(
source_id=global_source, geometry_id=global_geometry,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere2"))
scene_graph.RegisterAnchoredGeometry(
source_id=global_source,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere3"))
self.assertIsInstance(
scene_graph.get_source_pose_port(global_source), InputPort)
self.assertIsInstance(
scene_graph.get_pose_bundle_output_port(), OutputPort)
self.assertIsInstance(
scene_graph.get_query_output_port(), OutputPort)
# Test limited rendering API.
scene_graph.AddRenderer("test_renderer",
mut.render.MakeRenderEngineVtk(
mut.render.RenderEngineVtkParams()))
self.assertTrue(scene_graph.HasRenderer("test_renderer"))
self.assertEqual(scene_graph.RendererCount(), 1)
# Test SceneGraphInspector API
inspector = scene_graph.model_inspector()
self.assertEqual(inspector.num_sources(), 2)
self.assertEqual(inspector.num_frames(), 3)
self.assertEqual(len(inspector.all_frame_ids()), 3)
self.assertTrue(inspector.world_frame_id()
in inspector.all_frame_ids())
self.assertTrue(global_frame in inspector.all_frame_ids())
self.assertIsInstance(inspector.world_frame_id(), mut.FrameId)
self.assertEqual(inspector.num_geometries(), 3)
self.assertEqual(len(inspector.GetAllGeometryIds()), 3)
# Test both GeometrySet API as well as SceneGraphInspector's
# GeometrySet API.
empty_set = mut.GeometrySet()
self.assertEqual(
len(inspector.GetGeometryIds(empty_set)),
0)
self.assertEqual(
len(inspector.GetGeometryIds(empty_set, mut.Role.kProximity)),
0)
# Cases 1.a: Explicit frame, constructor
# N.B. Only in this case (1.a), do we test for non-kwarg usages of
# functions. In other tests,
frame_set_options = [
# Frame scalar.
mut.GeometrySet(frame_id=global_frame),
# Frame list.
mut.GeometrySet(frame_ids=[global_frame]),
# Frame list, no kwargs.
mut.GeometrySet([global_frame]),
# Frame list w/ (empty) geometry list.
mut.GeometrySet(geometry_ids=[], frame_ids=[global_frame]),
# Frame list w/ (empty) geometry list, no kwargs.
mut.GeometrySet([], [global_frame]),
]
# Case 1.b: Explicit frame, via Add().
# - Frame scalar.
cur = mut.GeometrySet()
cur.Add(frame_id=global_frame)
frame_set_options.append(cur)
# - Frame list.
cur = mut.GeometrySet()
cur.Add(frame_ids=[global_frame])
frame_set_options.append(cur)
# - Frame list w/ (empty) geometry list.
cur = mut.GeometrySet()
cur.Add(geometry_ids=[], frame_ids=[global_frame])
frame_set_options.append(cur)
# Cases 1.*: Test 'em all.
for frame_set in frame_set_options:
ids = inspector.GetGeometryIds(frame_set)
# N.B. Per above, we have 2 geometries that have been affixed to
# global frame ("sphere1" and "sphere2").
self.assertEqual(len(ids), 2)
# Cases 2.a: Explicit geometry, constructor (with non-kwarg check).
geometry_set_options = [
# Geometry scalar.
mut.GeometrySet(geometry_id=global_geometry),
# Geometry list.
mut.GeometrySet(geometry_ids=[global_geometry]),
# Geometry list, no kwargs.
mut.GeometrySet([global_geometry]),
# Geometry list w/ (empty) frame list.
mut.GeometrySet(geometry_ids=[global_geometry], frame_ids=[]),
# Geometry list w/ (empty) frame list, no kwargs.
mut.GeometrySet([global_geometry], []),
]
# Cases 2.b: Explicit geometry, via Add().
# - Geometry scalar.
cur = mut.GeometrySet()
cur.Add(geometry_id=global_geometry)
geometry_set_options.append(cur)
# - Geometry list.
cur = mut.GeometrySet()
cur.Add(geometry_ids=[global_geometry])
geometry_set_options.append(cur)
# - Geometry list w/ (empty) frame list.
cur = mut.GeometrySet()
cur.Add(geometry_ids=[global_geometry], frame_ids=[])
geometry_set_options.append(cur)
# Cases 1.*: Test 'em all.
for geometry_set in geometry_set_options:
ids = inspector.GetGeometryIds(geometry_set)
self.assertEqual(len(ids), 1)
self.assertEqual(
inspector.NumGeometriesWithRole(role=mut.Role.kUnassigned), 3)
self.assertEqual(inspector.NumDynamicGeometries(), 2)
self.assertEqual(inspector.NumAnchoredGeometries(), 1)
self.assertEqual(len(inspector.GetCollisionCandidates()), 0)
self.assertTrue(inspector.SourceIsRegistered(source_id=global_source))
# TODO(SeanCurtis-TRI) Remove this call at the same time as deprecating
# the subsequent deprecation tests; it is only here to show that the
# non-keyword call invokes the non-deprecated overload.
self.assertTrue(inspector.SourceIsRegistered(global_source))
self.assertEqual(inspector.NumFramesForSource(source_id=global_source),
2)
self.assertTrue(global_frame in inspector.FramesForSource(
source_id=global_source))
self.assertTrue(inspector.BelongsToSource(
frame_id=global_frame, source_id=global_source))
self.assertEqual(inspector.GetOwningSourceName(frame_id=global_frame),
"anchored")
self.assertEqual(
inspector.GetName(frame_id=global_frame), "anchored_frame")
self.assertEqual(inspector.GetFrameGroup(frame_id=global_frame), 0)
self.assertEqual(
inspector.NumGeometriesForFrame(frame_id=global_frame), 2)
self.assertEqual(inspector.NumGeometriesForFrameWithRole(
frame_id=global_frame, role=mut.Role.kProximity), 0)
self.assertEqual(len(inspector.GetGeometries(frame_id=global_frame)),
2)
self.assertTrue(
global_geometry in inspector.GetGeometries(frame_id=global_frame))
self.assertEqual(
len(inspector.GetGeometries(frame_id=global_frame,
role=mut.Role.kProximity)),
0)
self.assertEqual(
inspector.GetGeometryIdByName(frame_id=global_frame,
role=mut.Role.kUnassigned,
name="sphere1"),
global_geometry)
self.assertTrue(inspector.BelongsToSource(
geometry_id=global_geometry, source_id=global_source))
self.assertEqual(
inspector.GetOwningSourceName(geometry_id=global_geometry),
"anchored")
self.assertEqual(inspector.GetFrameId(global_geometry), global_frame)
self.assertEqual(
inspector.GetName(geometry_id=global_geometry), "sphere1")
self.assertIsInstance(inspector.GetShape(geometry_id=global_geometry),
mut.Sphere)
self.assertIsInstance(
inspector.GetPoseInParent(geometry_id=global_geometry),
RigidTransform_[float])
self.assertIsInstance(
inspector.GetPoseInFrame(geometry_id=global_geometry),
RigidTransform_[float])
self.assertIsInstance(inspector.geometry_version(),
mut.GeometryVersion)
# Check AssignRole bits.
proximity = mut.ProximityProperties()
perception = mut.PerceptionProperties()
perception.AddProperty("label", "id", mut.render.RenderLabel(0))
illustration = mut.IllustrationProperties()
props = [
proximity,
perception,
illustration,
]
context = scene_graph.CreateDefaultContext()
for prop in props:
# Check SceneGraph mutating variant.
scene_graph.AssignRole(
source_id=global_source, geometry_id=global_geometry,
properties=prop, assign=mut.RoleAssign.kNew)
# Check Context mutating variant.
scene_graph.AssignRole(
context=context, source_id=global_source,
geometry_id=global_geometry, properties=prop,
assign=mut.RoleAssign.kNew)
# Check property accessors.
self.assertIsInstance(
inspector.GetProximityProperties(geometry_id=global_geometry),
mut.ProximityProperties)
self.assertIsInstance(
inspector.GetProperties(geometry_id=global_geometry,
role=mut.Role.kProximity),
mut.ProximityProperties)
self.assertIsInstance(
inspector.GetIllustrationProperties(geometry_id=global_geometry),
mut.IllustrationProperties)
self.assertIsInstance(
inspector.GetProperties(geometry_id=global_geometry,
role=mut.Role.kIllustration),
mut.IllustrationProperties)
self.assertIsInstance(
inspector.GetPerceptionProperties(geometry_id=global_geometry),
mut.PerceptionProperties)
self.assertIsInstance(
inspector.GetProperties(geometry_id=global_geometry,
role=mut.Role.kPerception),
mut.PerceptionProperties)
self.assertIsInstance(
inspector.CloneGeometryInstance(geometry_id=global_geometry),
mut.GeometryInstance)
self.assertTrue(inspector.CollisionFiltered(
geometry_id1=global_geometry, geometry_id2=global_geometry))
roles = [
mut.Role.kProximity,
mut.Role.kPerception,
mut.Role.kIllustration,
]
for role in roles:
self.assertEqual(
scene_graph.RemoveRole(
source_id=global_source, geometry_id=global_geometry,
role=role),
1)
def test_contact_surface(self, T):
# We can't construct a ContactSurface directly. So, we need to evaluate
# hydroelastic contact in order to get a result that we can assess.
RigidTransform = RigidTransform_[T]
RigidTransformd = RigidTransform_[float]
SceneGraph = mut.SceneGraph_[T]
FramePoseVector = mut.FramePoseVector_[T]
scene_graph = SceneGraph()
s_id = scene_graph.RegisterSource("source")
# Add a compliant "moving" ball.
f_id = scene_graph.RegisterFrame(
source_id=s_id, frame=mut.GeometryFrame("frame"))
g_id0 = scene_graph.RegisterGeometry(
source_id=s_id, frame_id=f_id,
geometry=mut.GeometryInstance(X_PG=RigidTransformd(),
shape=mut.Sphere(1.), name="sphere"))
props = mut.ProximityProperties()
mut.AddCompliantHydroelasticProperties(
resolution_hint=1.0, hydroelastic_modulus=1e5, properties=props)
scene_graph.AssignRole(s_id, g_id0, props)
# Add a rigd half space.
g_id1 = scene_graph.RegisterAnchoredGeometry(
source_id=s_id,
geometry=mut.GeometryInstance(X_PG=RigidTransformd(),
shape=mut.HalfSpace(), name="plane"))
props = mut.ProximityProperties()
mut.AddRigidHydroelasticProperties(properties=props)
scene_graph.AssignRole(s_id, g_id1, props)
context = scene_graph.CreateDefaultContext()
# Provide poses so we can evaluate the query object. The poses should
# lead to a single collision.
poses = FramePoseVector()
poses.set_value(id=f_id, value=RigidTransform([0, 0, 0.5]))
scene_graph.get_source_pose_port(s_id).FixValue(context, poses)
query_object = scene_graph.get_query_output_port().Eval(context)
# Test both mesh representations.
for rep in (mut.HydroelasticContactRepresentation.kTriangle,
mut.HydroelasticContactRepresentation.kPolygon):
expect_triangles = (
rep == mut.HydroelasticContactRepresentation.kTriangle)
results = query_object.ComputeContactSurfaces(rep)
self.assertEqual(len(results), 1)
contact_surface = results[0]
self.assertLess(g_id0, g_id1) # confirm M = 0 and N = 1.
self.assertEqual(contact_surface.id_M(), g_id0)
self.assertEqual(contact_surface.id_N(), g_id1)
self.assertGreater(contact_surface.num_faces(), 0)
self.assertGreater(contact_surface.num_vertices(), 0)
self.assertGreater(contact_surface.area(face_index=0), 0)
self.assertGreater(contact_surface.total_area(), 0)
contact_surface.face_normal(face_index=0)
contact_surface.centroid(face_index=0)
contact_surface.centroid()
self.assertEqual(contact_surface.is_triangle(), expect_triangles)
self.assertEqual(contact_surface.representation(), rep)
if expect_triangles:
# Details of mesh are tested in geometry_hydro_test.py
contact_surface.tri_mesh_W()
field = contact_surface.tri_e_MN()
# Only triangle mesh fields can evaluate barycentric coords.
field.Evaluate(e=0, b=(0.25, 0.5, 0.25))
else:
# Details of mesh are tested in geometry_hydro_test.py
contact_surface.poly_mesh_W()
field = contact_surface.poly_e_MN()
# Only the Polygonal mesh has gradients pre-computed.
field.EvaluateGradient(e=0)
# APIs available to both Triangle and Polygon-based fields.
field.EvaluateAtVertex(v=0)
field.EvaluateCartesian(e=0, p_MQ=(0.25, 0.25, 0))
def test_query_object(self, T):
RigidTransform = RigidTransform_[float]
SceneGraph = mut.SceneGraph_[T]
QueryObject = mut.QueryObject_[T]
SceneGraphInspector = mut.SceneGraphInspector_[T]
FramePoseVector = mut.FramePoseVector_[T]
# First, ensure we can default-construct it.
model = QueryObject()
self.assertIsInstance(model, QueryObject)
scene_graph = SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(
source_id=source_id, frame=mut.GeometryFrame("frame"))
geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Sphere(1.), name="sphere"))
render_params = mut.render.RenderEngineVtkParams()
renderer_name = "test_renderer"
scene_graph.AddRenderer(renderer_name,
mut.render.MakeRenderEngineVtk(render_params))
context = scene_graph.CreateDefaultContext()
pose_vector = FramePoseVector()
pose_vector.set_value(frame_id, RigidTransform_[T]())
scene_graph.get_source_pose_port(source_id).FixValue(
context, pose_vector)
query_object = scene_graph.get_query_output_port().Eval(context)
self.assertIsInstance(query_object.inspector(), SceneGraphInspector)
with catch_drake_warnings(expected_count=3):
self.assertIsInstance(
query_object.X_WF(id=frame_id), RigidTransform_[T])
self.assertIsInstance(
query_object.X_PF(id=frame_id), RigidTransform_[T])
self.assertIsInstance(
query_object.X_WG(id=geometry_id), RigidTransform_[T])
self.assertIsInstance(
query_object.GetPoseInWorld(frame_id=frame_id), RigidTransform_[T])
self.assertIsInstance(
query_object.GetPoseInParent(frame_id=frame_id),
RigidTransform_[T])
self.assertIsInstance(
query_object.GetPoseInWorld(geometry_id=geometry_id),
RigidTransform_[T])
# Proximity queries -- all of these will produce empty results.
results = query_object.ComputeSignedDistancePairwiseClosestPoints()
self.assertEqual(len(results), 0)
results = query_object.ComputePointPairPenetration()
self.assertEqual(len(results), 0)
results = query_object.ComputeSignedDistanceToPoint(p_WQ=(1, 2, 3))
self.assertEqual(len(results), 0)
results = query_object.FindCollisionCandidates()
self.assertEqual(len(results), 0)
self.assertFalse(query_object.HasCollisions())
# ComputeSignedDistancePairClosestPoints() requires two valid geometry
# ids. There are none in this SceneGraph instance. Rather than
# populating the SceneGraph, we look for the exception thrown in
# response to invalid ids as evidence of correct binding.
with self.assertRaisesRegex(
RuntimeError,
r"The geometry given by id \d+ does not reference a geometry"
+ " that can be used in a signed distance query"):
query_object.ComputeSignedDistancePairClosestPoints(
geometry_id_A=mut.GeometryId.get_new_id(),
geometry_id_B=mut.GeometryId.get_new_id())
# TODO(SeanCurtis-TRI) Remove this call at the same time as deprecating
# the subsequent deprecation test; it is only here to show that the
# non-keyword call invokes the non-deprecated overload.
self.assertRaisesRegex(
RuntimeError,
r"The geometry given by id \d+ does not reference a geometry"
+ " that can be used in a signed distance query",
query_object.ComputeSignedDistancePairClosestPoints,
mut.GeometryId.get_new_id(), mut.GeometryId.get_new_id())
with catch_drake_warnings(expected_count=1):
with self.assertRaisesRegex(
RuntimeError,
r"The geometry given by id \d+ does not reference a geometry"
+ " that can be used in a signed distance query"):
query_object.ComputeSignedDistancePairClosestPoints(
id_A=mut.GeometryId.get_new_id(),
id_B=mut.GeometryId.get_new_id())
# Confirm rendering API returns images of appropriate type.
d_camera = mut.render.DepthCameraProperties(
width=320, height=240, fov_y=pi/6, renderer_name=renderer_name,
z_near=0.1, z_far=5.0)
image = query_object.RenderColorImage(
camera=d_camera, parent_frame=SceneGraph.world_frame_id(),
X_PC=RigidTransform())
self.assertIsInstance(image, ImageRgba8U)
image = query_object.RenderDepthImage(
camera=d_camera, parent_frame=SceneGraph.world_frame_id(),
X_PC=RigidTransform())
self.assertIsInstance(image, ImageDepth32F)
image = query_object.RenderLabelImage(
camera=d_camera, parent_frame=SceneGraph.world_frame_id(),
X_PC=RigidTransform())
self.assertIsInstance(image, ImageLabel16I)