rhaschke
commented
6 months ago 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
I recently realized that when attempting to allow the collision of an object using a
ModifyPlanningScenestage and having aComputeIKstage within it, collisions are not allowed. It seems this occurs becauseComputeIKreceives a monitored stage that doesn't have theAllowedCollisionMatrixupdated with the changes made by theModifyPlanningScenestage.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:
However, this setup doesn't work. We need to add more
ModifyPlanningScenestages to correctly disable collisions:Yet,
ComputeIKstill detects a collision. Even trying something like this doesn't work either:As far as I'm concerned, the current workaround, as explained here, is to use
setIgnoreCollisions(true)to disable all collisions inComputeIK. 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
ComputeIKstage by adding anallowCollisionsmethod, 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
allowCollisionsmethod in each stage (similar to what we did withComputeIK), so we wouldn't need to useModifyPlanningSceneto allow/forbid collisions anymore. This would address theComputeIKissue, the need to use moreModifyPlanningScenestages 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!