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!

[rhaschke]

For grasping we use the following pipeline:

Here ComputeIK generates a grasp pose with fingers still open, then collision with fingers is allowed, and finally fingers are closed. In your case, the problem is indeed that ComputeIK attempts to find the grasp pose with the suction cup on/in the object, working on the PlanningScene inherited from CurrentState, which doesn't yet have collisions allowed.
You would need the following structure:

CurrentState
ModifyPlanningScene (allow collisions between tool and object)     --|
ModifyPlanningScene (disallow collisions between tool and object)    |
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)

It would be simpler, if ComputeIK would compute a non-collision pose, and enable collisions and approach afterwards.

[DaniGarciaLopez]

That actually works! I've never thought about using two ModifyPlanningScenes altogether to disable collisions of the monitored stage. It's a bit of boilerplate, in my opinion, but a feasible way to accomplish it at the moment.

However, when executing the task, a contact is found between the tool and the object, despite the planning being successful. I thought this might be corrected with your fix (#561), but it looks like there is still a problem.

I've prepared a simple repro to show the issue here. This is the log:

[move_group-4] [INFO] [1715174528.203091420] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'object' (type 'Object') and 'panda_leftfinger' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[move_group-4] [INFO] [1715174528.203094460] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-4] [INFO] [1715174528.203097992] [moveit_ros.plan_execution]: Trajectory component '7/10' is invalid after scene update
[move_group-4] [INFO] [1715174528.203099870] [moveit_ros.plan_execution]: Stopping execution because the path to execute became invalid(probably the environment changed)
[move_group-4] [INFO] [1715174528.203113053] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Cancelling execution for panda_arm_controller
[ros2_control_node-5] [INFO] [1715174528.203299931] [panda_arm_controller]: Got request to cancel goal
[move_group-4] [INFO] [1715174528.203532894] [moveit_ros.trajectory_execution_manager]: Stopped trajectory execution.
[move_group-4] [INFO] [1715174528.246139697] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'panda_arm_controller' successfully finished
[move_group-4] [INFO] [1715174528.246198979] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status PREEMPTED ...

[DaniGarciaLopez]

Friendly ping @rhaschke

[rhaschke]

I had a look into your example. The problem is, that backbwards-directed ModifyPlanningScene stages are not supported in Humble, because MoveIt Humble is missing a key fix for this to work (moveit/moveit2#2745).
I filed a backport of it: moveit/moveit2#2850. As soon as this gets merged, we can drop the corresponding MTC code reversion and it should work. (Your example works in MoveIt1).

Apart from that, your pipeline is not very meaningful: While you can disable collision checking during planning and execution (while approaching the object with closed fingers), the actual execution will surely move or topple the object due to a collision. Are you sure, you really want to do that?

broken.mp4

[DaniGarciaLopez]

I filed a backport of it: moveit/moveit2#2850. As soon as this gets merged, we can drop the corresponding MTC code reversion and it should work. (Your example works in MoveIt1).

Thanks a lot for your effort, Robert. As soon as the PR is merged, I will try it on my setup and close the issue if it is also resolved on my end.

Apart from that, your pipeline is not very meaningful: While you can disable collision checking during planning and execution (while approaching the object with closed fingers), the actual execution will surely move or topple the object due to a collision. Are you sure, you really want to do that?

The main reason for this is that our setup involves planning with objects that have many contact points, such as screwing a bolt into a nut. We could use collision meshes with a higher polygon count, but this significantly increases the planning time. To mitigate this, we use low-poly meshes, which requires us to have the final picking stage in collision.

Picking the cylinder with the gripper closed was the simplest way I found to illustrate a ComputeIK stage in collision within the pick/place demo.