Drake中Hydroelastic碰撞场景下获取碰撞部件的正确方法

你的核心问题是调用了MultibodyPlant的私有方法FindBodyByGeometryId，Drake提供了公开API来完成从GeometryId到Body的映射，以下是修正后的代码：

simulator->set_monitor([this](const systems::Context<double>& 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();

        // 用公开API从GeometryId获取BodyId，再获取Body对象
        const drake::geometry::GeometryId geometry_id_M = contact_surface.id_M();
        const drake::multibody::BodyId body_id_A = multibody_plant->GetBodyIdFromGeometryIdOrThrow(geometry_id_M);
        const drake::multibody::Body<double>& body_A = multibody_plant->get_body(body_id_A);

        const drake::geometry::GeometryId geometry_id_N = contact_surface.id_N();
        const drake::multibody::BodyId body_id_B = multibody_plant->GetBodyIdFromGeometryIdOrThrow(geometry_id_N);
        const drake::multibody::Body<double>& body_B = multibody_plant->get_body(body_id_B);

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

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

关键修改说明 #

• 替换私有方法FindBodyByGeometryId为公开的GetBodyIdFromGeometryIdOrThrow，该方法会在GeometryId无效时抛出异常，保证安全性；如果不需要异常，也可以用GetBodyIdFromGeometryId返回std::optional<BodyId>。
• 直接通过get_body(BodyId)获取Body对象，省去了中间转FrameId的冗余步骤，代码更简洁。

内容的提问来源于stack exchange，提问作者mqnc