allowCollisions from ModifyPlanningScene doesn't update the ACM of ComputeIK

[DaniGarciaLopez]

I recently realized that when attempting to allow the collision of an object using a ModifyPlanningScene stage and having a ComputeIK stage within it, collisions are not allowed. It seems this occurs because ComputeIK receives a monitored stage that doesn't have the AllowedCollisionMatrix updated with the changes made by the ModifyPlanningScene stage.

As a more pragmatic example of this issue, let's consider a scenario where we have a vacuum gripper end effector, and we want to allow the tool to partially intersect with the object to be picked. I suppose anyone new to MTC would write something like this:

CurrentState
Connect
ModifyPlanningScene (allow collisions between tool and object)
MoveRelative (linear approach towards the object)
ComputeIK
  GeneratePose
ModifyPlanningScene (attach object)
MoveRelative (raise the object)
ModifyPlanningScene (forbid collisions between tool and object)

However, this setup doesn't work. We need to add more ModifyPlanningScene stages to correctly disable collisions:

CurrentState
Connect
ModifyPlanningScene (allow collisions between tool and object)
MoveRelative (linear approach towards the object)
ModifyPlanningScene (forbid collisions between tool and object)
ComputeIK
  GeneratePose
ModifyPlanningScene (attach object)
ModifyPlanningScene (allow collisions between tool and object)
MoveRelative (raise the object)
ModifyPlanningScene (forbid collisions between tool and object)

Yet, ComputeIK still detects a collision. Even trying something like this doesn't work either:

...
ModifyPlanningScene (allow collisions between tool and object)
MoveRelative (linear approach towards the object)
ComputeIK
  GeneratePose
ModifyPlanningScene (forbid collisions between tool and object)
...

As far as I'm concerned, the current workaround, as explained here, is to use setIgnoreCollisions(true) to disable all collisions in ComputeIK. However, this can lead to unexpected results as it disables ALL collisions, making it difficult to debug because other parts of the scene might be colliding.

Our current workaround involves modifying the ComputeIK stage by adding an allowCollisions method, enabling us to manually set the pairs of links we want to allow collision for (without needing to allow ALL collisions). I can prepare a PR if you find it useful.

Nevertheless, I believe this behavior is very unintuitive, and it took me quite a while to realize what was wrong. I was thinking that perhaps one solution could be to individually add an allowCollisions method in each stage (similar to what we did with ComputeIK), so we wouldn't need to use ModifyPlanningScene to allow/forbid collisions anymore. This would address the ComputeIK issue, the need to use more ModifyPlanningScene stages than expected, making the list of stages cleaner, and simplifying the logic of allowing/forbidding collisions depending on the stage direction which might be tricky to understand and was a common problem looking back at the issues of this repo.

Let me know what you think about this or if there is any current fix to this issue. Thanks!