def test_connect_meshcat_visualizer(self):
"""Cart-Pole with simple geometry. This should produce two cart-poles,
with identical geometry, one under the prefix `vis` and another under
the prefix `vis2`. You should confirm in the visualizer that both
exist (by unchecking one at a time in the MeshCat controls menu)"""
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
cart_pole, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.Finalize()
vis = ConnectMeshcatVisualizer(builder=builder,
prefix="vis",
scene_graph=scene_graph,
zmq_url=ZMQ_URL,
open_browser=False)
self.assertIsInstance(vis, MeshcatVisualizer)
vis2 = ConnectMeshcatVisualizer(
builder=builder,
prefix="vis2",
output_port=scene_graph.get_query_output_port(),
zmq_url=ZMQ_URL,
open_browser=False)
vis2.set_name("vis2")
self.assertIsInstance(vis2, MeshcatVisualizer)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
vis.load(vis.GetMyContextFromRoot(context))
vis2.load(vis2.GetMyContextFromRoot(context))
diagram.Publish(context)
def test_warnings_and_errors(self):
builder = DiagramBuilder()
sg = builder.AddSystem(SceneGraph())
v2 = builder.AddSystem(MeshcatVisualizer(scene_graph=sg))
builder.Connect(sg.get_query_output_port(),
v2.get_geometry_query_input_port())
v2.set_name("v2")
v4 = builder.AddSystem(MeshcatVisualizer(scene_graph=None))
builder.Connect(sg.get_query_output_port(),
v4.get_geometry_query_input_port())
v4.set_name("v4")
v5 = ConnectMeshcatVisualizer(builder, scene_graph=sg)
v5.set_name("v5")
v7 = ConnectMeshcatVisualizer(builder,
scene_graph=sg,
output_port=sg.get_query_output_port())
v7.set_name("v7")
with self.assertRaises(AssertionError):
v8 = ConnectMeshcatVisualizer(builder,
scene_graph=None,
output_port=None)
v8.set_name("v8")
v10 = ConnectMeshcatVisualizer(builder,
scene_graph=None,
output_port=sg.get_query_output_port())
v10.set_name("v10")
diagram = builder.Build()
context = diagram.CreateDefaultContext()
def PublishWithNoWarnings(v):
with warnings.catch_warnings(record=True) as w:
v.Publish(v.GetMyContextFromRoot(context))
self.assertEqual(len(w), 0, [x.message for x in w])
# Use geometry_query API
v2.load(v2.GetMyContextFromRoot(context))
PublishWithNoWarnings(v2)
v2.load()
PublishWithNoWarnings(v2)
# Use geometry_query API
v4.load(v4.GetMyContextFromRoot(context))
PublishWithNoWarnings(v4)
with self.assertRaises(RuntimeError):
v4.load() # Can't work without scene_graph nor context.
# Use geometry_query API
v5.load(v5.GetMyContextFromRoot(context))
PublishWithNoWarnings(v5)
v5.load()
PublishWithNoWarnings(v5)
# Use geometry_query API
v7.load(v7.GetMyContextFromRoot(context))
PublishWithNoWarnings(v7)
v7.load()
PublishWithNoWarnings(v7)
# Use geometry_query API
v10.load(v10.GetMyContextFromRoot(context))
PublishWithNoWarnings(v10)
with self.assertRaises(RuntimeError):
v10.load() # Can't work without scene_graph nor context.
def test_warnings_and_errors(self):
builder = DiagramBuilder()
sg = builder.AddSystem(SceneGraph())
v1 = builder.AddSystem(MeshcatVisualizer(scene_graph=sg))
builder.Connect(sg.get_pose_bundle_output_port(), v1.get_input_port(0))
v1.set_name("v1")
v2 = builder.AddSystem(MeshcatVisualizer(scene_graph=sg))
builder.Connect(sg.get_query_output_port(),
v2.get_geometry_query_input_port())
v2.set_name("v2")
v3 = builder.AddSystem(MeshcatVisualizer(scene_graph=None))
builder.Connect(sg.get_pose_bundle_output_port(), v3.get_input_port(0))
v3.set_name("v3")
v4 = builder.AddSystem(MeshcatVisualizer(scene_graph=None))
builder.Connect(sg.get_query_output_port(),
v4.get_geometry_query_input_port())
v4.set_name("v4")
v5 = ConnectMeshcatVisualizer(builder, scene_graph=sg)
v5.set_name("v5")
v6 = ConnectMeshcatVisualizer(
builder,
scene_graph=sg,
output_port=sg.get_pose_bundle_output_port())
v6.set_name("v6")
v7 = ConnectMeshcatVisualizer(builder,
scene_graph=sg,
output_port=sg.get_query_output_port())
v7.set_name("v7")
with self.assertRaises(AssertionError):
v8 = ConnectMeshcatVisualizer(builder,
scene_graph=None,
output_port=None)
v8.set_name("v8")
v9 = ConnectMeshcatVisualizer(
builder,
scene_graph=None,
output_port=sg.get_pose_bundle_output_port())
v9.set_name("v9")
v10 = ConnectMeshcatVisualizer(builder,
scene_graph=None,
output_port=sg.get_query_output_port())
v10.set_name("v10")
diagram = builder.Build()
context = diagram.CreateDefaultContext()
def PublishWithNoWarnings(v):
with warnings.catch_warnings(record=True) as w:
v.Publish(v.GetMyContextFromRoot(context))
self.assertEqual(len(w), 0, [x.message for x in w])
# Use pose_bundle API, give warning
v1.load(v1.GetMyContextFromRoot(context))
with catch_drake_warnings(expected_count=1):
v1.Publish(v1.GetMyContextFromRoot(context))
v1._warned_pose_bundle_input_port_connected = False
v1.load()
with catch_drake_warnings(expected_count=1):
v1.Publish(v1.GetMyContextFromRoot(context))
# Use geometry_query API
v2.load(v2.GetMyContextFromRoot(context))
PublishWithNoWarnings(v2)
v2.load()
PublishWithNoWarnings(v2)
# Can't use pose_bundle API without passing a scene graph.
with self.assertRaises(RuntimeError):
v3.load(v3.GetMyContextFromRoot(context))
with self.assertRaises(RuntimeError):
v3.load()
# Use geometry_query API
v4.load(v4.GetMyContextFromRoot(context))
PublishWithNoWarnings(v4)
with self.assertRaises(RuntimeError):
v4.load() # Can't work without scene_graph nor context.
# Use geometry_query API
v5.load(v5.GetMyContextFromRoot(context))
PublishWithNoWarnings(v5)
v5.load()
PublishWithNoWarnings(v5)
# Use pose_bundle API, give warning
v6.load(v6.GetMyContextFromRoot(context))
with catch_drake_warnings(expected_count=1):
v6.Publish(v6.GetMyContextFromRoot(context))
v6._warned_pose_bundle_input_port_connected = False
v6.load()
with catch_drake_warnings(expected_count=1):
v6.Publish(v6.GetMyContextFromRoot(context))
# Use geometry_query API
v7.load(v7.GetMyContextFromRoot(context))
PublishWithNoWarnings(v7)
v7.load()
PublishWithNoWarnings(v7)
# Can't use pose_bundle API without passing a scene graph.
with self.assertRaises(RuntimeError):
v9.load(v9.GetMyContextFromRoot(context))
with self.assertRaises(RuntimeError):
v9.load()
# Use geometry_query API
v10.load(v10.GetMyContextFromRoot(context))
PublishWithNoWarnings(v10)
with self.assertRaises(RuntimeError):
v10.load() # Can't work without scene_graph nor context.
builder = DiagramBuilder()
# Adds both MultibodyPlant and SceneGraph and wires them together
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
# Note that we parse into both the plant and the scene_graph here
Parser(plant, scene_graph).AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf"
))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
# Adds the MeshcatVisualizer and wires it to the Scene Graph
meshcat = ConnectMeshcatVisualizer(builder, scene_graph, open_browser=True)
plant.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
meshcat.load()
diagram.Publish(context)
plant_context = plant.GetMyMutableContextFromRoot(context)
plant.SetPositions(plant_context, [-1.57, 0.1, 0, -1.2, 0, 1.6, 0])
plant.get_actuation_input_port().FixValue(plant_context, np.zeros(7))
simulator = Simulator(diagram, context)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(5)