同一碰撞/视觉体对应多个GeometryId的问题排查

这是之前一个相似问题的跟进场景，不仅同一Link存在多个GeometryId，其碰撞元素也出现了该情况。

打印碰撞对GeometryId的代码 #

#include "drake/multibody/inverse_kinematics/inverse_kinematics.h"
#include "drake/math/rotation_matrix.h"
#include "drake/solvers/create_constraint.h"
#include "drake/solvers/solve.h"
#include "drake/multibody/parsing/parser.h"

// #include "drake/solvers/clp_solver.h"
// #include "drake/solvers/csdp_solver.h"
#include "drake/solvers/gurobi_solver.h"
#include "drake/solvers/ipopt_solver.h"
#include "drake/solvers/mosek_solver.h"
#include "drake/solvers/snopt_solver.h"
#include "drake/solvers/solver_options.h"
#include "drake/solvers/nlopt_solver.h"

#include "drake/geometry/proximity_properties.h"
#include "drake/geometry/scene_graph_inspector.h"
#include "drake/geometry/shape_specification.h"
// #include "drake/multibody/tree/multibody_tree_indexes.h"

#include "drake/systems/analysis/simulator.h"
#include "drake/visualization/visualization_config_functions.h"

#include <iostream>
#include <boost/filesystem.hpp>
#include <fstream>
#include <chrono>
#include <numeric>
#include <bits/stdc++.h>

// using namespace boost::filesystem;    

using namespace std;


int main() {
    string sdf_filepath = "path_to_sdf.sdf";

    drake::systems::DiagramBuilder<double> builder;
    drake::multibody::MultibodyPlant<double>* plant{};
    drake::geometry::SceneGraph<double>* scene_graph{};
    std::tie(plant, scene_graph) = drake::multibody::AddMultibodyPlantSceneGraph(&builder, timestep);
    plant->set_name("plant");
    scene_graph->set_name("scene_graph");
    drake::multibody::Parser parser(plant, scene_graph);

    const auto robot_model_index = parser.AddModelFromFile(sdf_filepath, "robot");
    plant->Finalize();
    
    auto diagram = builder.Build();
    auto diagram_context= diagram->CreateDefaultContext();
    auto plant_context = &(diagram->GetMutableSubsystemContext(*plant,
    diagram_context.get()));

    const auto& inspector = scene_graph->model_inspector();

    auto collision_pairs = inspector.GetCollisionCandidates();
    for (auto& pair : collision_pairs) {
        cout << pair.first << ", " << pair.second << endl;
        const auto first_geom_id = pair.first;
        const auto second_geom_id = pair.first;
        const auto first_name = inspector.GetName(first_geom_id); 
        const auto second_name = inspector.GetName(second_geom_id); 
        cout << first_name << ", " << second_name << endl;

        cout << "Left: " << endl;
        const auto first_frame_id = inspector.GetFrameId(first_geom_id);
        const auto first_geom_model_index = inspector.GetFrameGroup(first_frame_id);
        const auto first_source_name = inspector.GetOwningSourceName(first_frame_id);

        // NOTE: CUSTOM MODIFIED FUNCTION, essentially making GeometryState::get_solver_id() public
        const auto first_source_id = inspector.GetOwningSourceId(first_geom_id);
        cout << "frame_id: " << first_frame_id << endl;
        cout << "geom_model_index: " << first_geom_model_index << endl;
        cout << "robot_model_index: " << robot_model_index << endl;
        cout << "source_name: " << first_source_name << endl;
        cout << "source_id: " << first_source_id << endl;
        cout << endl;

        cout << "Right: " << endl;
        const auto second_frame_id = inspector.GetFrameId(second_geom_id);
        const auto second_geom_model_index = inspector.GetFrameGroup(second_frame_id);
        const auto second_source_name = inspector.GetOwningSourceName(second_frame_id);
        const auto second_source_id = inspector.GetOwningSourceId(second_geom_id);
        cout << "frame_id: " << second_frame_id << endl;
        cout << "geom_model_index: " << second_geom_model_index << endl;
        cout << "robot_model_index: " << robot_model_index << endl;
        cout << "source_name: " << second_source_name << endl;
        cout << "source_id: " << second_source_id << endl;
        cout << endl;
    }
}

输出片段 #

129, 150
robot::right_link5_collision, robot::right_link5_collision
129, 152
robot::right_link5_collision, robot::right_link5_collision
129, 154
robot::right_link5_collision, robot::right_link5_collision
129, 156
robot::right_link5_collision, robot::right_link5_collision
129, 162
robot::right_link5_collision, robot::right_link5_collision
129, 168
robot::right_link5_collision, robot::right_link5_collision

SDF中right_link5的定义（仅出现一次） #

<link name='right_link5'>
      <pose relative_to='right_joint5'>0 0 0 0 -0 0</pose>
      <inertial>
        <pose>-0.000234 0.036705 -0.080064 0 -0 0</pose>
        <mass>0.18554</mass>
        <inertia>
          <ixx>0.010000</ixx>
          <ixy>8.9957e-07</ixy>
          <ixz>8.5285e-07</ixz>
          <iyy>0.010000</iyy>
          <iyz>-0.00025682</iyz>
          <izz>0.010000</izz>
        </inertia>
      </inertial>
      <collision name='right_link5_collision'>
        <pose>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>file:///home/user/.../collision/link5.STL</uri>
          </mesh>
        </geometry>
      </collision>
      <visual name='right_link5_visual'>
        <pose>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>file:///home/user/.../visual/link5.STL</uri>
          </mesh>
        </geometry>
      </visual>
    </link>

额外打印信息（几何体属性完全一致） #

129, 162
robot::right_link5_collision, robot::right_link5_collision
Left: 
frame_id: 125
geom_model_index: 2
robot_model_index: 2
source_name: plant
source_id: 15

Right: 
frame_id: 125
geom_model_index: 2
robot_model_index: 2
source_name: plant
source_id: 15

原因分析 #

这种现象是Drake对复杂网格自动进行凸分解导致的：

• 当导入的STL网格为非凸几何体时，Drake会自动将其分解为多个凸子几何体。凸分解是实现高效碰撞检测的必要手段——非凸几何体的碰撞检测计算成本极高，而凸几何体的碰撞检测算法成熟且速度快。
• 每个分解后的凸子几何体都会被分配一个独立的GeometryId，但它们都隶属于同一个Link，因此所有关联属性（frame_id、source_id、所属模型索引等）完全一致。
• 你看到的多个GeometryId，本质上是同一个碰撞网格分解出来的多个凸块，它们共同构成原Link的碰撞形状。

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