0

I'm trying to remove the proximity role from certain geometries. However, I'm getting an error that my plant source id is not registered:

Referenced geometry source 0 is not registered.

I have checked that the plant is indeed registered, since calling

plant->RegisterAsSourceForSceneGraph(scene_graph);

fails with !geometry_source_is_registered().

I've also created geometries (by adding a sdf model), so source_id should be valid. Here's how I'm attempting this:

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);

drake::multibody::Parser parser(plant, scene_graph);

const auto robot_model_index = parser.AddModelFromFile(sdf_filepath, "robot");

plant->Finalize();
const auto& source_id = plant->get_source_id().value();

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

auto all_geom_ids = inspector.GetAllGeometryIds();
for (auto& geom_id : all_geom_ids) {
    const auto frame_id = inspector.GetFrameId(geom_id);
    const auto geom_model_index = inspector.GetFrameGroup(frame_id);
    // https://stackoverflow.com/questions/68225198/does-a-modelinstance-in-a-multibodyplan-have-a-unique-source-id-for-determining
    if (geom_model_index == robot_model_index) {

        // This fails: Referenced geometry source 0 is not registered.
        scene_graph->RemoveRole(source_id, geom_id, drake::geometry::Role::kProximity);
    }
}
alvin
  • 137
  • 7
  • Your sample code doesn't show where `source_id` came from. – jwnimmer-tri Sep 30 '22 at 22:06
  • Sorry, updated just now. Currently, I resolved this by adding a new function: `template const SourceId SceneGraphInspector::GetOwningSourceId( GeometryId geometry_id) const { DRAKE_DEMAND(state_ != nullptr); return state_->GetOwningSourceId(geometry_id); }` Is there a reason why GeometryState::get_source_id() is private? – alvin Sep 30 '22 at 22:10
  • Like Jeremy pointed out below, `plant.get_source_id()` is the recommended way to obtain the source id of the plant. `GeometryState` is supposed to be an internal class. A more complete (or even better, reproducible) snippet of code may help with the diagnosis. – Xuchen Han Oct 01 '22 at 01:15
  • Two thoughts: 1) looking at your code, it should work. Do you have buildable code that another user can build and run? 2) SceneGraph will never reveal SourceId because the "source id" is about ownership. The "owener" of that id can certainly give it up (hence `plant.get_source_id()`, but SceneGraph can't. – Sean Curtis Oct 03 '22 at 13:42
  • I'll make one suggestion: change: `const auto& source_id = plant->get_source_id().value();` to `const auto source_id = plant->get_source_id().value();` (Remove the reference). `MultibodyPlant::get_source_id()` returns an `optional` *by value*. You are grabbing a reference to the internals of a temporary variable. It's most likely getting reset. Whereas, if you simply copy it into your own local `const SourceId`, your value won't get corrupted. – Sean Curtis Oct 03 '22 at 13:51
  • EDIT: Never mind; it can't be this. The const l-value ref *should* keep it alive.... – Sean Curtis Oct 03 '22 at 14:11
  • If you print out the source id immediately after acquiring it (`cout << source_id << "\n";`) does it print "0"? If not, somewhere between you're acquiring it and using it, it's being replaced with a default-constructed identifier. – Sean Curtis Oct 03 '22 at 14:18
  • It's a bit strange, but plant.get_source_id() works in another executable when built using standard cmake. This error occurred when building in a ros workspace. I will give an update on this if it happens again. – alvin Oct 05 '22 at 19:27

1 Answers1

1

If you want to change geometry associated with the a MultibodyPlant, the source_id to use is the plant.get_source_id().

jwnimmer-tri
  • 1,994
  • 2
  • 5
  • 6
  • Right, I am doing that above with ```const auto& source_id = plant->get_source_id().value();```, but that is returning 0 when the correct id should be 15. – alvin Sep 30 '22 at 22:29