添加HalfSpace后Drake中SDF模型碰撞检测失效问题咨询

在Ubuntu 24.04系统中通过apt安装了Drake 1.44.0版本，编写代码用于检测两个完全重合的相同SDF模型之间的碰撞。原本QueryObject可以正常检测到碰撞，但当向MultibodyPlant添加无碰撞的HalfSpace后，碰撞不再被报告。不确定这是Bug还是预期行为，希望得知原因。已搜索相关Issue和Stack Overflow帖子，未找到类似问题。

运行日志 #

$ ./drake_collision_check
Collision Candidates:
  TwoLinkRobot::base_link_collision vs TwoLinkRobot_1::upper_link_collision
  TwoLinkRobot::upper_link_collision vs TwoLinkRobot_1::base_link_collision
  TwoLinkRobot::upper_link_collision vs TwoLinkRobot_1::upper_link_collision
Collision

$ ./drake_collision_check --add-half-space
[2025-09-09 23:33:39.657] [console] [warning] Rigid (non-deformable) half spaces are not currently supported for deformable contact; registration is allowed, but no contact data will be reported.
Collision Candidates:
  TwoLinkRobot::base_link_collision vs TwoLinkRobot_1::upper_link_collision
  TwoLinkRobot::upper_link_collision vs TwoLinkRobot_1::base_link_collision
  TwoLinkRobot::upper_link_collision vs TwoLinkRobot_1::upper_link_collision
  TwoLinkRobot::upper_link_collision vs halfspace
  TwoLinkRobot_1::upper_link_collision vs halfspace
No Collision

测试代码 #

可通过以下命令编译：

mkdir build; cd build; cmake ..; make

drake_collision_check.cpp #

#include <drake/multibody/parsing/parser.h>

constexpr char kModel[] = "../TwoLinkRobot.sdf";

void PrintCollisionCandidates(
    const drake::geometry::SceneGraph<double> *scene_graph) {
  const auto &inspector = scene_graph->model_inspector();
  const auto collision_candidates = inspector.GetCollisionCandidates();
  fprintf(stdout, "Collision Candidates:\n");
  for (const auto &cc : collision_candidates) {
    fprintf(stdout, "  %s vs %s\n", inspector.GetName(cc.first).c_str(),
            inspector.GetName(cc.second).c_str());
  }
}

int main(int argc, char **argv) {
  bool add_half_space = false;
  if (argc > 1 && std::string(argv[1]) == "--add-half-space") {
    add_half_space = true;
  }

  constexpr double kSimTimeStep = 1e-2;
  drake::systems::DiagramBuilder<double> builder;
  auto [plant, scene_graph] =
      drake::multibody::AddMultibodyPlantSceneGraph<double>(&builder,
                                                            kSimTimeStep);

  drake::multibody::Parser model_parser(&plant, &scene_graph);
  model_parser.SetAutoRenaming(true);

  auto models = model_parser.AddModels(kModel);
  if (models.size() < 1) {
    fprintf(stderr, "Failed to load model '%s'\n", kModel);
    return 1;
  }
  auto robot = models[0];
  plant.WeldFrames(plant.world_frame(),
                   plant.GetFrameByName("base_link", robot));

  // load exactly same model to cause collision
  auto models2 = model_parser.AddModels(kModel);
  if (models2.size() < 1) {
    fprintf(stderr, "Failed to load model2 '%s'\n", kModel);
    return 1;
  }
  auto robot2 = models2[0];
  plant.WeldFrames(plant.world_frame(),
                   plant.GetFrameByName("base_link", robot2),
                   drake::math::RigidTransformd(Eigen::Vector3d(0, 0, 0)));

  if (add_half_space) {
    plant.RegisterCollisionGeometry(
        plant.world_body(),
        drake::geometry::HalfSpace::MakePose({-1, 0, 0}, {10, 0, 0}),
        drake::geometry::HalfSpace(), "halfspace",
        drake::geometry::ProximityProperties());
  }

  // finalize
  plant.Finalize();
  auto diagram = builder.Build();
  auto diagram_context = diagram->CreateDefaultContext();

  // debug
  PrintCollisionCandidates(&scene_graph);

  // check collision
  const auto &scene_graph_context =
      diagram->GetSubsystemContext(scene_graph, *diagram_context);
  const auto &query_object =
      scene_graph.get_query_output_port()
          .Eval<drake::geometry::QueryObject<double>>(scene_graph_context);
  if (query_object.HasCollisions()) {
    printf("Collision\n");
  } else {
    printf("No Collision\n");
  }
  return 0;
}

CMakeLists.txt #

cmake_minimum_required(VERSION 3.22)
project(drake_collision_check)

set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(CMAKE_CXX_STANDARD 23)
add_compile_options(-Wall -Wextra -Wpedantic)

find_package(drake CONFIG REQUIRED PATHS /opt/drake)

add_executable(drake_collision_check
  drake_collision_check.cpp
)
target_link_libraries(drake_collision_check
  drake::drake
)

TwoLinkRobot.sdf #

<?xml version="1.0"?>
<sdf version="1.7">
  <model name="TwoLinkRobot">
    <link name="base_link">
      <collision name="base_link_collision">
        <geometry>
          <box>
            <size>0.2 0.2 0.2</size>
          </box>
        </geometry>
      </collision>
    </link>
    <link name="upper_link">
      <pose>0 0.15 0 0 0 0</pose>
      <collision name="upper_link_collision">
        <pose>0 0 -0.5 0 0 0</pose>
        <geometry>
          <cylinder>
            <length>1.1</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
      </collision>
    </link>
    <joint name="shoulder" type="revolute">
      <child>upper_link</child>
      <parent>base_link</parent>
      <axis>
        <xyz expressed_in="__model__">0 1 0</xyz>
        <limit>
          <effort>0</effort>
        </limit>
      </axis>
    </joint>
  </model>
</sdf>

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