Question: Planning abortion criteria

[sjahr]

I've created an MTC task very similar to the pick_and_place tutorial in moveit2 but I experience premature result with a planning failure. It looks like the Connect stage doesn't trigger the motion planner for all IK solutions. My console output looks like this:

  1  - ←   1 →   -  1 / current state
-  1 →   1 →   -  0 / connect to Pose
  -  1 →   1 ←   7  - / move to pose
  7  - ←   7 →   -  7 / pose IK
    1  - ←   1 →   -  1 / generate pose
-  0 →   0 →   -  0 / retreat from object

My understanding is that during planning only one of the 7 IK solution seems to be evaluated and the planning ends with a failure despite that is has not evaluated all of them. What are the abortion criteria of an MTC task or what could lead to this early failure? I am aware that there is a timeout property but I have not changed that and my understanding it that it is inf by default. Thanks in advance for your help

[rhaschke]

I'm confused about the indentation of stages in the console output. Shouldn't current state be at the same hierarchy level as connect to Pose (which seems to be a container)?
From this console output, it's hard to guess what happens. Please provide a minimal example reproducing the issue (have a look at the test cases in test_pruning.cpp). Maybe you are running into an issue with solution pruning.

[marioprats]

Hi Robert, I think we found a way to reproduce this as a unit test, following your test_pruning.cpp examples.
First here's a case that works as expected:

TEST_F(Pruning, PropagateFromContainerPrunningWorks)
{
  add(t, new GeneratorMockup({ 0 }));

  auto serial = add(t, new SerialContainer());
  add(*serial, new ConnectMockup({0,1,2}));
  add(*serial, new GeneratorMockup({0,1,2}));

  add(t, new ForwardMockup({ 0, INF, 1 }));

  EXPECT_TRUE(t.plan());
}

The output is:

  1  - ←   2 →   -  2 / task pipeline
    1  - ←   1 →   -  1 / GEN1
    -  1 →   3 →   -  0 / serial container
      -  1 →   3 ←   3  - / CON1
      3  - ←   3 →   -  3 / GEN2
    -  0 →   2 →   -  2 / FWD1

Two solutions come out of the pipeline as expected.

The issue happens when the last stage (ForwardMockup) in the pipeline fails in its first attempt. Here's the same code as above but ForwardMockup has INF cost on the first candidate:

TEST_F(Pruning, PropagateFromContainerPrunningIssue)
{
  add(t, new GeneratorMockup({ 0 }));

  auto serial = add(t, new SerialContainer());
  add(*serial, new ConnectMockup({0,1,2}));
  add(*serial, new GeneratorMockup({0,1,2}));

  add(t, new ForwardMockup({ INF, 0, 1 }));

  EXPECT_TRUE(t.plan());
}

And here's the task output.

  0  - ←   0 →   -  0 / task pipeline
    1  - ←   1 →   -  1 / GEN1
    -  1 →   1 →   -  0 / serial container
      -  1 →   1 ←   3  - / CON1
      3  - ←   3 →   -  3 / GEN2
    -  0 →   0 →   -  0 / FWD1

For some reason CON1 stops evaluating the rest of candidates, which should be feasible.

Hopefully this makes sense. Let me know if we can help in any way.
And thanks for your help on this!

[marioprats]

On a related note, removing the SerialContainer from the pipeline makes it work as expected:

TEST_F(Pruning, PropagateFromContainerPrunningIssueWithoutSerialContainer)
{
  add(t, new GeneratorMockup({ 0 }));

  add(t, new ConnectMockup({0,1,2}));
  add(t, new GeneratorMockup({0,1,2}));

  add(t, new ForwardMockup({ INF, 0, 1 }));

  EXPECT_TRUE(t.plan());
}

  1  - ←   2 →   -  2 / task pipeline
    1  - ←   1 →   -  1 / GEN1
    -  1 →   2 ←   3  - / CON1
    3  - ←   3 →   -  0 / GEN2
    -  0 →   2 →   -  2 / FWD1

[rhaschke]

Thanks for providing this minimal example. I will have a look into it.

[sjahr]

Thank you!

[rhaschke]

Inspecting this example in detail revealed several design issues of pruning.

1. InterfaceStates are only propagating from inside a container to its external interface if the container itself produces a solution. For this reason, the failing FW stage in your example gets only triggered after the CON stage already produced its first solution, thus providing a full solution of the serial container. This is not problematic per see, but I didn't have that in mind when designing the pruning process. Need to rethink, whether this has serious implications. Definitely, it delays pruning.
2. Updates to the InterfaceState's priority is problematic in general
   If the status, depth, or priority of an InterfaceState changes, we want to update all other InterfaceState's priorities within the same solution branch as well, such that all associated interfaces can reorder their states. This is achieved by setStatus<DIR>() into possibly both directions.
   However, the solution tree extending from a particular InterfaceState is not well-defined: If there is a container involved, we have two parallel solution branches: one inside the container and one outside. It suffices to update one branch (the inner one). To this end, we need to ensure that updates can propagate in and out from a container. Currently this is not ensured.
   The actual problem in the example is that the pruning flow enters the serial container from the wrong direction (because only the reading interface has a notify callback installed), then disabling the wrong heads of the CON stage.

solution:
Instead of relying on the notify function for updates, we could explicitly handle propagation from/into a container in setStatus(). We already have code there to propagate from inside to the outside.
Propagating priority updates through containers is sufficient as well: the outside connection will be updated automatically when leaving the container on the other side.