Collision between environment bodies should already be filtered

Question

I am trying to use Drake to check for collisions in my scene. My setup consists of a table, a robot arm, and a part on top of the table.

However, when I use the CalcContextRobotClearance on my instance of SceneGraphCollisionChecker, it is outputting that there is a negative distance (i.e., a collision) between the table and the part. I want to ignore this collision, since it's expected for the part to be touching the table. I see the code print out:

1723081728.8459249: <RigidBody name='ingress_table_link' index=11 model_instance=3> is in collision with <RigidBody name='part1_link' index=12 model_instance=4>

When I try to add a collision filter between the part and the table, I get a drake internal error:

RuntimeError: Drake internal error at planning/scene_graph_collision_checker.cc:217 in DoCalcContextRobotClearance(): Collision between bodies [ingress_table::ingress_table_link] and [part1::part1_link] should already be filtered

Here is my code below. I use the parameter set_collision_filter to toggle between setting the collision filter or not.

from pathlib import Path
from time import time

from pydrake.all import (
    CollisionCheckerContext,
    Meshcat,
    RigidTransform,
    RobotDiagramBuilder,
    RollPitchYaw,
    SceneGraphCollisionChecker,
)
from pydrake.systems.analysis import Simulator
from pydrake.visualization import AddDefaultVisualization

ROOT_PATH = Path(__file__).parent.parent.parent.as_posix()
ROBOT_URDF_PATH = ROOT_PATH + "/component_configs/abb/urdf/irb1200_7_70.urdf"
PART1_URDF_PATH = ROOT_PATH + "/resources/test_objects/part1.urdf"
TABLE_URDF_PATH = ROOT_PATH + "/component_configs/ingress_table/urdf/ingress_table.urdf"


def launch_abb_simulator(
    time_step: float,
    realtime_rate: float,
    set_collision_filter: bool = False,
) -> None:
    robot_diagram_builder = RobotDiagramBuilder(time_step=0)
    builder = robot_diagram_builder.builder()
    plant = robot_diagram_builder.plant()
    parser = robot_diagram_builder.parser()

    # Add robot
    robot_model = parser.AddModels(ROBOT_URDF_PATH)
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base_link"))

    # Add table
    table_model = parser.AddModels(TABLE_URDF_PATH)
    translation = [0, 0.5, 0.325]
    rotation = RollPitchYaw(0, 0, 0).ToQuaternion()
    table_transform = RigidTransform(rotation, translation)
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ingress_table_link"), table_transform)

    # Add the part object
    part_model = parser.AddModels(PART1_URDF_PATH)

    # Finalize the scene and add visualization
    plant.Finalize()
    meshcat = Meshcat()
    AddDefaultVisualization(builder, meshcat)
    diagram = robot_diagram_builder.Build()
    simulator = Simulator(diagram)

    # Set context
    collision_context = CollisionCheckerContext(diagram)
    # plant_context = collision_context.plant_context()
    simulator_context = simulator.get_mutable_context()
    plant_sim_context = plant.GetMyContextFromRoot(simulator_context)

    # Set the initial positions, with the part on the table
    robot_joint_positions = [0, 0, 0, 0, 0, 0]
    part_quaternion = [1, 0, 0, 0]
    part_translation = [0, 0.5, 0.4]
    positions = robot_joint_positions + part_quaternion + part_translation

    plant.SetPositions(plant_sim_context, positions)

    # Make a collision checker
    collision_checker = SceneGraphCollisionChecker(
        model=diagram,
        robot_model_instances=robot_model + table_model + part_model,
        edge_step_size=0.01,
        env_collision_padding=0.0,
        self_collision_padding=0.0,
    )

    if set_collision_filter:
        collision_checker.SetCollisionFilteredBetween(
            plant.GetBodyByName("ingress_table_link"),
            plant.GetBodyByName("part1_link"),
            filter_collision=True,
        )

    simulator.Initialize()
    simulator.set_publish_every_time_step(False)
    simulator.set_target_realtime_rate(realtime_rate)

    while True:
        meshcat.PublishRecording()
        simulator.AdvanceTo(simulator_context.get_time() + time_step)

        clearance = collision_checker.CalcContextRobotClearance(
            collision_context,
            plant.GetPositions(plant_sim_context),
            100,
        )
        for i, dist in enumerate(clearance.distances()):
            if dist < 0:
                robot = clearance.robot_indices()[i]
                other = clearance.other_indices()[i]
                print(f"{time()}: {plant.get_body(robot)} is in collision with {plant.get_body(other)}")


if __name__ == "__main__":
    launch_abb_simulator(
        time_step=3e-3,
        realtime_rate=1.0,
        set_collision_filter=False,
    )

Answer 1

I think I have the answer. It comes down to the CollisionCheckerContext you've created.

1. CollisionChecker maintains its own collision checker contexts.
2. You've created your own CollisionCheckerContext completely decoupled from the checker.
   • This is the context you provide to your call to collision_checker.CalcContextRobotClearance().
3. Calls to checker.SetCollisionFilteredBetween() only affect the contexts that the checker knows about. So, that call didn't change your hand-rolled context instance.

The solution, I believe, is to eliminate your custom creation of the collision context (collision_context = CollisionCheckerContext(diagram)) and add the following line right after instantiating your collision_checker:

collision_context = collision_checker.MakeStandaloneModelContext()

This should give you a context that the checker knows about and updates to collision filters will be reflected in it and tests against the checker's expected state (a filter exists) and the context's actual state will align and your error should go away.