I have a system that consists of a robotic manipulator and an object. I want to evaluate signed distances between all collision geometries in the system while excluding collisions between the fingertips of the end-effector and the convex geometries that make up the collision geometry of the object. However, when I use the CollisionFilterManager to try to apply the relevant exclusions, my code still computes signed distances between fingertips and object geometries when calling ComputeSignedDistancePairClosestPoints() much later on downstream.
I have a container class that has the plant and its SceneGraph as attributes. When initializing this class, I try to filter the collisions. Here are the relevant parts of the code:
class SimplifiedClass:
def __init__(self, ...):
# initializing plant, contexts, and query object port
self.diagram = function_generating_diagram_with_plant(...)
self.plant = self.diagram.GetSubsystemByName("plant")
self.scene_graph = self.diagram.GetSubsystemByName("scene_graph")
_diag_context = self.diagram.CreateDefaultContext()
self.plant_context = self.plant.GetMyMutableContextFromRoot(_diag_context)
self.sg_context = self.scene_graph.GetMyMutableContextFromRoot(_diag_context)
self.qo_port = self.scene_graph.get_query_output_port()
# applying filters
cfm = self.scene_graph.collision_filter_manager()
inspector = self.query_object.inspector()
fingertip_geoms = []
obj_collision_geoms = []
gids = inspector.GetAllGeometryIds()
for g in gids:
name = inspector.GetName(g)
# if name.endswith("ds_collision") or name.endswith("collision_1"):
if name.endswith("tip_collision_1") and "algr" in name:
fingertip_geoms.append(g)
elif name.startswith("obj_collision"):
obj_collision_geoms.append(g)
ftip_set = GeometrySet(fingertip_geoms)
obj_set = GeometrySet(obj_collision_geoms)
cfm.Apply(
CollisionFilterDeclaration()
.ExcludeBetween(ftip_set, obj_set)
.ExcludeWithin(obj_set)
)
@property
def query_object(self):
return self.qo_port.Eval(self.sg_context)
def function_that_computes_signed_distances(self, ...):
# this function calls ComputeSignedDistancePairClosestPoints(), which
# computes signed distances even between filtered geometry pairs
I've confirmed that the correct geometry IDs are being retrieved during initialization (at least the names are correct), but if I do a simple test where I see how many SignedDistancePairs are returned from calling ComputeSignedDistancePairClosestPoints() before and after the attempt to apply the filter, the same number of signed distance pairs are returned, which implies the filter had no effect even immediately after declaring it. I also confirm the geometries that should be filtered are not by examining the names associated with signed distance pairs during my downstream function call.
Is there an obvious bug in my code? If not, where else could the bug be located besides here?
The problem is an age-old problem: model vs context.
In short, SceneGraph stores an interior model so you can construct as you go. When you create a context a copy of that model is placed in the context. That copy is independent. If you continue to modify SceneGraph's model, you'll only observe a change in future contexts you allocate.
In your code above, you've already allocated a context. You acquire a collision filter manager using cfm = self.scene_graph.collision_filter_manager(). This is the SceneGraph model version. You want the other one where you get the manager from the context: self.scene_graph.collision_filter_manager(self.sg_context) as documented here.
Alternatively, you can modify the collision filters before you allocate a context. Or throw out the old context and reallocate. All are viable choices.