Possible bug in ModifyPlanningScene

[JafarAbdi]

I'm not sure if this's a bug in ModifyPlanningScene or a misuse from my side, so I have the following task

CurrentState
Connect                  
MoveRelative           ------------------------------------
ModifyPlanningScene                                       | 
GeneratePose(monitor CurrentState)                        |   Collision Enabled Between EE and Octomap
ModifyPlanningScene                                       |
...                    ------------------------------------
ModifyPlanningScene (Forbid collision)

The reason why I have two ModifyPlanningScene between GeneratePose is that I need the collision to be disabled for the MoveRelative (maybe could be replaced with #253)

This's isn't working because in modify_planning_scene.cpp#L90-L92 we revert the user choice in the backward case so allowing collision become forbid collision, is this a bug or intended behavior?

If ModifyPlanningScene is only meant to be used as forward should it inherit from PropagatingForward?

[rhaschke]

long story short: yes, if a ModifyPlanningScene stage is applied in backwards fashion, its actions are actually reversed as well.
This was to make the linear sequence look correct when read from the top, i.e. going from Connect to MoveRelative to GeneratePose.
This is interesting for serial containers that can apply their stages in forward and reverse order. For example, I have a Grasp and an UnGrasp container, applying essentially the same series of stages, simply in reverse order.

[AndyZe]

I think I might be facing the same issue on a different project. We would like the suction cups to be able to touch the object during a MoveRelative but not before the MoveRelative.

[v4hn]

maybe could be replaced with #253

That is an interesting idea, though not as straight-forward as you might expect. The modifying wrapper was mostly meant to modify solutions of children stages, not the stages passed to them. And if I understood you correctly that's what you would want here to adapt the ACM.

As Robert explained the ModifyPlanningScene container is meant to be defined in temporal order, not causal.
That has some problems in itself, but I don't think the one you describe actually exists? Can't you just invert the logic there?

The main issue I am aware of is that you cannot specify "delete" operations and reverse them unless you would also provide the whole object that is deleted in this step. You can work around it with a custom stage, but it might make sense to address this in an API in the ModifyPlanningScene class.

[AndyZe]

Can't you just invert the logic there?

You mean we should set allowCollisions() to false to allow the collision? Wow, that would be nonintuitive. I'll try it.

[v4hn]

You mean we should set allowCollisions() to false to allow the collision? Wow, that would be unintuitive. I'll try it.

I'm not sure why you think this behavior is unintuitive. I think it's one of the few sane parts of the pipeline. :-)
The correct phrasing is "set allowCollisions() to false to have the collisions enabled before the stage and disabled after the stage.
The generated solution is always interpreted start to end and so are the defined stages when you read through the task.
So if you see a Stage with allowCollisions(false), you know that when you get there during execution of a solution the collisions will be forbidden before executing the next subsolutions.

In theory the logical flow you are focused on here could be seen as an implementation detail that is rather independent of the task specification.
But the monitoring structure and backward planning make it efficient/possible for the modular framework to solve the tasks.

[schornakj]

@rhaschke @v4hn I'll post here since I'm working on the same code as @JafarAbdi. As he said, we're working on a task where the robot needs to be able to partly intersect with the planning scene's collision octomap during the final part of a grasp motion, but needs to avoid collision with the octomap during other motions.

In more detail, our task looks like this:

CurrentState
Connect (freespace motion to pre-approach pose -- we want to avoid colliding with <octomap> here)
SerialContainer
  ModifyPlanningScene (allow collisions between <octomap> and the gripper links)
  MoveRelative (do a linear approach move towards the object to grasp)
  ModifyPlanningScene (forbid collisions between <octomap> and the gripper links)
  ComputeIK (we call setIgnoreCollisions(true) for this stage)
    GeneratePose (uses a pose passed in from outside the context of the task, and is set to monitor the first CurrentState stage)
  ModifyPlanningScene (allow collisions between <octomap> and the gripper links)
  MoveTo (move the gripper planning group to be closed)
  MoveRelative (lift the gripper along the world Z+ axis and, hopefully, the object too)
  MoveRelative (reverse the initial linear approach move)
  ModifyPlanningScene (forbid collisions between <octomap> and the gripper links)

There are a few things I wanted to ask about:

• While this task does find a trajectory that intersects the octomap during the final part of the grasp while avoiding it during the initial freespace motion, when we go to execute the motion the MoveIt Trajectory Execution Manager halts the motion before the approach move is performed because it detects that we're about to go into collision with the octomap. Shouldn't the planning scene diffs associated with each subtrajectory make this set of collisions permissible by modifying the planning scene's Allowed Collision Matrix before the motion is executed?
• It seems like we needed to add more ModifyPlanningScene stages than expected to produce the right behavior. In particular the ones right before and after the ComputeIK stage seem like they should not be necessary, since the one at the beginning of the SerialContainer should allow these collisions for the subsequent stages. Am I misunderstanding the propagation of the result of these stages throughout the task?
• What is the right way to generate a pose that is in collision with some planning scene object? I think the reason why we need to use setIgnoreCollisions(true) is that the state of the scene produce by the CurrentState stage which is monitored by the GeneratePose stage has an ACM that forbids collision between the octomap and the gripper links. Is there a way to modify the monitored stage to allow certain collisions before using it to generate poses?

[v4hn]

- While this task does find a trajectory that intersects the octomap during the final part of the grasp while avoiding it during the initial freespace motion, when we go to execute the motion the MoveIt Trajectory Execution Manager halts the motion before the approach move is performed because it detects that we're about to go into collision with the octomap. Shouldn't the planning scene diffs associated with each subtrajectory make this set of collisions permissible by modifying the planning scene's Allowed Collision Matrix before the motion is executed?

This is a bug in solution extraction. Extracting the planning scene diffs happens by taking [the diff of the end scene](https://github.com/ros-planning/moveit_task_constructor/blob/master/core/src/storage.cpp#L222), but that only works when the end scene is a diff on top of the start scene (in forward propagators). The missing part for you is that in backward stages, the planning scene change should be extracted as the inverted scene diff of the start scene (as this was generated as a diff over the end scene). But generally Connecting and Generator-type stages can yield solutions with unrelated planning scene interface states as well, so we actually need a method to generate a full diff from two decoupled PlanningScenes to use here.

- It seems like I needed to add more `ModifyPlanningScene` stages than expected to produce the right behavior. In particular the ones right before and after the ComputeIK stage seem like they should not be necessary, since the one at the beginning of the SerialContainer should allow these collisions for the subsequent stages. Am I misunderstanding the propagation of the result of these stages throughout the task?

Academically speaking we would extract the modifications to the planning scene in your first modifier stage and apply them to solutions that are passed from the monitored stage to the monitor. But that would create a lot more overhead and would be even harder to debug than what we have now.

- What is the right way to generate a pose that is in collision with some planning scene object? I think the reason why we need to use `setIgnoreCollisions(true)` is that the state of the scene produce by the CurrentState stage which is monitored by the GeneratePose stage has an ACM that forbids collision between the octomap and the gripper links. Is there a way to modify the monitored stage to allow certain collisions before using it to generate poses?

wrapping your monitor in scene modifiers is a workaround on your end. It should work. But ideally you are right in that the solution spawned by the monitor should already have the correct acm modifications already. A better way to achieve the same would be either (1) implement your own GeneratePose stage that applies these modifications before spawning or (2) wrap the GeneratePose stage in a modifying wrapper ([unfinished PR](#253) that applies the modifications.

[AndyZe]

Just wondering, have you considered inverting the logic internally so the user doesn't need to? It would be pretty nice if allowCollisions(true) always meant the same thing

[schornakj]

@v4hn Thank you for writing up that explanation. @JafarAbdi and I are going to implement the update to planning scene diff propagation you mentioned.

[v4hn]

You mean we should set `allowCollisions()` to false to allow the collision? Wow, that would be unintuitive. I'll try it.

I'm not sure why you think this behavior is unintuitive. I think it's one of the few sane parts of the pipeline. :-) The correct phrasing is "set `allowCollisions()` to false to have the collisions enabled *before* the stage and disabled *after* the stage. The generated solution is always interpreted start to end and so are the defined stages when you read through the task. So if you see a Stage with `allowCollisions(false)`, you know that when you get there during execution of a solution the collisions will be forbidden before executing the next subsolutions. In theory the logical flow you are focused on here could be seen as an implementation detail that is rather independent of the task specification. But the monitoring structure and backward planning make it efficient/possible for the modular framework to solve the tasks.