2

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
alvin
  • 137
  • 7
  • Could you share more code for diagnosis? The .STL extension is suspicious because Drake only parse .obj (and .vtk for hydroelastic contact) files for meshed collision geometries. But that probably has nothing to do with the symptom you are seeing. – Xuchen Han Sep 30 '22 at 23:16
  • Ah yes, for the .STL files, there are actually also .OBJ files that I converted using meshlab. I've modified Drake to check for .OBJ and read those instead. Sure, I can provide a full sample. – alvin Oct 01 '22 at 00:51
  • 1
    I see `const auto second_geom_id = pair.first;` Is that a typo while transcribing the code to stack overflow or actual typo in the code? – Xuchen Han Oct 01 '22 at 01:06
  • Ah sigh, no that is my own bug... I'm so sorry. I will close this issue. Thank you for your help. – alvin Oct 01 '22 at 01:21
  • 1
    BTW To help avoid that bug, you can change your for loop to `for (const auto& [first_geom_id, second_geom_id] : collision_pairs) { ... }`. That has multiple advantages over grabbing the `pair` directly. – Sean Curtis Oct 03 '22 at 13:44

0 Answers0