Report bodies that are involved in a collision in Drake

I run a robot simulation in Drake. The robot is loaded from URDF and I want to use hydroelastic collision simulation. Upon collision, I want to report which parts collide. Digging through source, docs and examples, this is what I thought how this is done:

simulator->set_monitor([this](const systems::Context<double>& root_context) {
    (void) root_context;

    systems::Context<double>& plant_context =
        this->diagram->GetMutableSubsystemContext(*(this->multibody_plant), &(this->simulator)->get_mutable_context());

    const auto& contact_results =
        this->multibody_plant->get_contact_results_output_port()
            .Eval<drake::multibody::ContactResults<double>>(plant_context);

    std::cout << contact_results.num_hydroelastic_contacts() << " contacts" << std::endl;

    for (int i = 0; i < contact_results.num_hydroelastic_contacts(); ++i) {
        const auto& contact_info = contact_results.hydroelastic_contact_info(i);
        const auto& contact_surface = contact_info.contact_surface();

        const auto& geometry_id_M = contact_surface.id_M();
        const auto& body_id_A = multibody_plant->FindBodyByGeometryId(geometry_id_M);
        const auto& frame_id_A = multibody_plant->GetBodyFrameIdOrThrow(body_id_A);
        const auto& body_A = multibody_plant->GetBodyFromFrameId(frame_id_A);

        const auto& geometry_id_N = contact_surface.id_N();
        const auto& body_id_B = multibody_plant->FindBodyByGeometryId(geometry_id_N);
        const auto& frame_id_B = multibody_plant->GetBodyFrameIdOrThrow(body_id_B);
        const auto& body_B = multibody_plant->GetBodyFromFrameId(frame_id_B);

        std::cout << "bodies: " << body_A->name() << " + " << body_B->name() << std::endl;
    }

    return systems::EventStatus::Succeeded();
});

However, FindBodyByGeometryId is a private method. What is the intended way of doing this?

You've got the right idea, you just need to redirect, ever so slightly.

In addition to the MultibodyPlant, you also need access to the SceneGraphInspector and you'll find a slightly different path. The logical path is this:

geometry id -> geometry frame id -> body

In your code, it would look something like this:

simulator->set_monitor([this](const systems::Context<double>& root_context) {
    (void) root_context;

    systems::Context<double>& plant_context =
        this->diagram->GetMutableSubsystemContext(*(this->multibody_plant), &(this->simulator)->get_mutable_context());

    const auto& contact_results =
        this->multibody_plant->get_contact_results_output_port()
            .Eval<drake::multibody::ContactResults<double>>(plant_context);
    const auto& inspector =
        this->multibody_plant->get_geometry_query_input_port()
            .Eval<drake::geometry::QueryObject<double>>(plant_context).inspector();

    auto body_for_geometry = [&inspector, &plant = this->multibody_plant]
                             (geometry::GeometryId id) {
        const auto geo_frame_id = inspector.GetFrameId(id);
        const auto* body = plant->GetBodyFromFrameId(geo_frame_id);
        DRAKE_DEMAND(body != nullptr);
        return body;
    };

    std::cout << contact_results.num_hydroelastic_contacts() << " contacts" << std::endl;

    for (int i = 0; i < contact_results.num_hydroelastic_contacts(); ++i) {
        const auto& contact_info = contact_results.hydroelastic_contact_info(i);
        const auto& contact_surface = contact_info.contact_surface();

        const auto* body_A = body_for_geometry(contact_surface.id_M());
        const auto* body_B = body_for_geometry(contact_surface.id_N());

        std::cout << "bodies: " << body_A->name() << " + " << body_B->name() << std::endl;
    }

    return systems::EventStatus::Succeeded();
});