def test_contact_applet_hydroelastic(self):
"""Check that _ContactApplet doesn't crash when receiving hydroelastic
messages.
"""
# Create the device under test.
dut = mut.Meldis()
meshcat = dut.meshcat
lcm = dut._lcm
# Enqueue a hydroelastic contact message.
sdf_file = FindResourceOrThrow(
"drake/multibody/meshcat/test/hydroelastic.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001)
parser = Parser(plant=plant)
parser.AddModelFromFile(sdf_file)
body1 = plant.GetBodyByName("body1")
body2 = plant.GetBodyByName("body2")
plant.AddJoint(
PrismaticJoint(name="body1",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body1.body_frame(),
axis=[0, 0, 1]))
plant.AddJoint(
PrismaticJoint(name="body2",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body2.body_frame(),
axis=[1, 0, 0]))
plant.Finalize()
ConnectContactResultsToDrakeVisualizer(builder=builder,
plant=plant,
scene_graph=scene_graph,
lcm=lcm)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant.SetPositions(plant.GetMyMutableContextFromRoot(context),
[0.1, 0.3])
diagram.Publish(context)
# The geometry isn't registered until the load is processed.
hydro_path = "/CONTACT_RESULTS/hydroelastic/" + \
"body1.two_bodies::body1_collision+body2"
hydro_path2 = "/CONTACT_RESULTS/hydroelastic/" + \
"body1.two_bodies::body1_collision2+body2"
self.assertEqual(meshcat.HasPath(hydro_path), False)
self.assertEqual(meshcat.HasPath(hydro_path2), False)
# Process the load + draw; contact results should now exist.
lcm.HandleSubscriptions(timeout_millis=1)
dut._invoke_subscriptions()
self.assertEqual(meshcat.HasPath("/CONTACT_RESULTS/hydroelastic"),
True)
self.assertEqual(meshcat.HasPath(hydro_path), True)
self.assertEqual(meshcat.HasPath(hydro_path2), True)
def test_contact_applet_point_pair(self):
"""Check that _ContactApplet doesn't crash when receiving point
contact messages.
"""
# Create the device under test.
dut = mut.Meldis()
meshcat = dut.meshcat
lcm = dut._lcm
# Enqueue a point contact result message.
sdf_file = FindResourceOrThrow(
"drake/examples/manipulation_station/models/sphere.sdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001)
parser = Parser(plant=plant)
sphere1_model = parser.AddModelFromFile(sdf_file, "sphere1")
sphere2_model = parser.AddModelFromFile(sdf_file, "sphere2")
body1 = plant.GetBodyByName("base_link", sphere1_model)
body2 = plant.GetBodyByName("base_link", sphere2_model)
plant.AddJoint(
PrismaticJoint(name="sphere1_x",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body1.body_frame(),
axis=[1, 0, 0]))
plant.AddJoint(
PrismaticJoint(name="sphere2_x",
frame_on_parent=plant.world_body().body_frame(),
frame_on_child=body2.body_frame(),
axis=[1, 0, 0]))
plant.Finalize()
ConnectContactResultsToDrakeVisualizer(builder=builder,
plant=plant,
scene_graph=scene_graph,
lcm=lcm)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant.SetPositions(plant.GetMyMutableContextFromRoot(context),
[-0.03, 0.03])
diagram.Publish(context)
# The geometry isn't registered until the load is processed.
pair_path = "/CONTACT_RESULTS/point/base_link(2)+base_link(3)"
self.assertEqual(meshcat.HasPath(pair_path), False)
# Process the load + draw; make sure the geometry exists now.
lcm.HandleSubscriptions(timeout_millis=0)
dut._invoke_subscriptions()
self.assertEqual(meshcat.HasPath(pair_path), True)
def make_mbp(builder):
mbp, sg = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
robot_body = make_box(mbp, "robot")
planar_joint = mbp.AddJoint(
PlanarJoint(name="robot_planar_joint",
frame_on_parent=mbp.world_frame(),
frame_on_child=robot_body.body_frame(),
damping=[1, 1, 0.9]))
blocker1 = make_box(mbp, "blocker1")
mbp.WeldFrames(A=mbp.world_frame(),
B=blocker1.body_frame(),
X_AB=RigidTransform([1, 0.5, 0]))
return mbp, sg
