SceneGraphCollisionChecker refers to removed joint index after RemoveJoint() function has been called on the plant

[agarwal-abhinav]

What happened?

When working with MultibodyPlant, after calling plant.RemoveJoint() method, the joint index is removed and not replaced (as expected). Thus, there would be a skip of integer number at the point where the removed joint's index was numerically assigned. When this plant (via its RobotDiagram) is then passed to the SceneGraphCollisionChecker, the following error is given: RuntimeError: The JointIndex(26) has been removed. Essentially, SceneGraphCollisionChecker is not working on a plant for which joints have been removed and new joints/ welds have been added at the end, creating a discontinuity in the integer index of joints.

Version

1.32.0

What operating system are you using?

Ubuntu 22.04

What installation option are you using?

pip install drake

Relevant log output

No response

[cohnt]

Per our in-person discussion, looks like the bug is caused by how the joints are iterated over in linear_distance_and_interpolation_provider.cc.

Abhinav and I will fix this.