This is a follow-up of another similar issue. Only in my case, I am seeing >2 geometry ids not only for a link, but also its collision element.
In the code below, I am printing out geometry id's that are valid collision pairs:
#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;
}
}
and here is a clip of the output:
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
Here is the corresponding part of the SDF file that contains right_link5 (defined only once):
<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>
Any thoughts on why this could be happening? I also printed out more information, and these geometry bodies seem to be identical:
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