Add/remove stages depending on the result of a `PredicateFilter`

[DaniGarciaLopez]

I am trying to add or remove a series of stages based on a calculation performed inside a PredicateFilter. The main idea is to have a generic "Pick" container that detects the location of a collision object and constructs the task differently depending on its location. For example, if we call "pick object" and it is inside a drawer, we would execute some preliminary stages to open the drawer. On the other hand, if it is on the table, we would pick it directly.

I am not sure how to pass the condition calculated in the lambda function of setPredicate to the necessary stages. I tried setting the result to a property or using a global variable within the container, but the value is not updated. I suspect this is because setPredicate is only executed when we call task->plan(), not when the task is constructed.

Any ideas on how to do this properly?

[DaniGarciaLopez]

I've just realized that I can achieve this by using a Fallbacks container with a PredicateFilter in each branch to check for each location. Here's an example:

Fallbacks
  PredicateFilter (check if the object is in the drawer)
    SerialContainer
      ...
      ...
      (stages defining the movement to open the drawer)

  PredicateFilter (check if the object is on the table)
    NoOp (don't add additional stages if the object is on the table)

I was previously looking for a solution like this because it adds less boilerplate in simple cases where you just need to modify some properties of the stage. Although, it doesn't work as explained in the first comment:

std::string current_location;

{
  auto dummy_state = std::make_unique<mtc::stages::NoOp>("dummy state");
  auto applicability_filter = std::make_unique<mtc::stages::PredicateFilter>("applicability check", std::move(dummy_state));

  applicability_filter->setPredicate([this, object, &current_location](const mtc::SolutionBase &s, std::string &comment) {
    auto scene = s.start()->scene();
    
    // Calculate the current location of the object
    (...)
    current_location = calculated_location;

    return true;
  });
  container->insert(std::move(applicability_filter));
}

(...)

{
  auto stage = std::make_unique<mtc::stages::MoveRelative>("approach", cartesian_planner_);
  
  (...)

  if (current_location == "drawer") {
    stage->setMinMaxDistance(0.03, 0.05);
    vec.vector.z = -1.0;
  } else if (current_location == "shelf") {
    stage->setMinMaxDistance(0.1, 0.15);
    vec.vector.x = -1.0;
  } else {
    stage->setMinMaxDistance(0.05, 0.15);
    vec.vector.z = 1.0;
  }
  stage->setDirection(vec);
  container->insert(std::move(stage));
}

I'm going to close the issue, as it seems that using Fallbacks mainly solves what I need. However, if you know any other way of doing it, don't hesitate to comment it.

[rhaschke]

Both, the Fallbacks and Alternatives container are suitable to solve your problem.

[DaniGarciaLopez]

I slightly prefer Fallbacks as it implies that only one branch can be successful at a time. This allows us to use the last branch without a PredicateFilter to provide the default behavior in case no other condition is met. Although this is also possible with Alternatives, we would need to add a PredicateFilter that negates the conditions of the other branches, which could be more error-prone.

Is it possible to do something similar to the code I sent?

Thanks @rhaschke

[rhaschke]

I'm not fully sure, whether or not you have an open question here. I don't think there is currently another way of implementing your task with something else than a Fallsbacks container with appropriate predicate filters. Note that this approach is rather inefficient: Solutions will be first generated by the wrapped SerialContainer and then filtered (instead of directly switching to the right SerialContainer). I'd suggest to implement your problem with two independent Tasks, selecting one or the other based on your location estimation. One could also implement a new Switch-Case-Container, that - based on some property of the incoming trajectory - decides to use one of n children for processing the solution.

[DaniGarciaLopez]

Thanks for pointing out the performance implications of this approach, I didn't realize that. Currently, we only have simple MoveRelative stages within each fallback branch, so there is no significant drawback in planning time. If, in the future, we have more complex needs, I will consider implementing a Switch-Case Container, I think it is the best solution for this case.

[DaniGarciaLopez]

I'm reopening the issue as I encountered a problem with this approach. If MoveRelative succeeds, it propagates correctly to PredicateFilter. We force PredicateFilter to always fail and it triggers correctly the second branch of the Fallbacks container. Everything good here:

However, if MoveRelative fails, the failure is not propagated to the PredicateFilter and the second branch of the Fallbacks is never calculated:

This is the relevant code:

auto fallbacks = std::make_unique<mtc::Fallbacks>("Fallbacks");

// First fallback branch     
{
  auto stage = std::make_unique<mtc::stages::MoveRelative>("MoveRelative: force it to fail", cartesian_planner_);
  (...) // Creating a collision to force it to fail

  auto filter = std::make_unique<mtc::stages::PredicateFilter>("PredicateFilter: force it to fail", std::move(stage));
  filter->setPredicate([](const mtc::SolutionBase &s, std::string &comment) {
    return false;
  });

  fallbacks->add(std::move(filter));
}

// Second fallback branch     
{
  auto stage = std::make_unique<mtc::stages::MoveRelative>("MoveRelative: force it to succeed", cartesian_planner_);
  (...)

  auto filter = std::make_unique<mtc::stages::PredicateFilter>("PredicateFilter: force it to succeed", std::move(stage));
  filter->setPredicate([](const mtc::SolutionBase &s, std::string &comment) {
    return true;
  });

  fallbacks->add(std::move(filter));
}

// Default fallback, just passing through
{
  auto stage = std::make_unique<mtc::stages::NoOp>("NoOp: Default fallback");
  fallbacks->add(std::move(stage));
}

task->add(std::move(fallbacks));

Why a failed stage is not propagated to trigger another branch of the Fallbacks container? Is there a bug here @rhaschke?

[rhaschke]

This indeed looks like a bug. Need to investigate...

[rhaschke]

Sorry for the late reply. I wrote a unittest (#594) for your example, but that doesn't fail. Could you please double check your code and/or provide a failing minimal example - ideally based on the proposed unittest?

[DaniGarciaLopez]

Thank you for taking the time to write a test! It actually fails on my setup. I am using ROS2, which might be the reason. Sorry for not specifying this at first. I tested with the latest commits from the humble branch.

Here is the test output:

build/moveit_task_constructor_core/Testing/20240716-1314/Test.xml: 12 tests, 0 errors, 1 failure, 0 skipped
- moveit_task_constructor_core-test-fallback
  <<< failure message
    -- run_test.py: invoking following command in '/home/dani/support_ws/build/moveit_task_constructor_core/test':
     - /home/dani/support_ws/build/moveit_task_constructor_core/test/moveit_task_constructor_core-test-fallback --gtest_output=xml:/home/dani/support_ws/build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-fallback.gtest.xml
    Running main() from gmock_main.cc
    [==========] Running 10 tests from 3 test suites.
    [----------] Global test environment set-up.
    [----------] 1 test from FallbacksFixtureGenerator
    [ RUN      ] FallbacksFixtureGenerator.stayWithFirstSuccessful
    [       OK ] FallbacksFixtureGenerator.stayWithFirstSuccessful (4 ms)
    [----------] 1 test from FallbacksFixtureGenerator (4 ms total)
    
    [----------] 8 tests from FallbacksFixturePropagate
    [ RUN      ] FallbacksFixturePropagate.failingNoSolutions
      0  - ←   0 →   -  0 / task pipeline
        1  - ←   1 →   -  0 / GEN1
        -  0 →   0 →   -  0 / Fallbacks
          -  0 →   0 →   -  0 / FWD1
          -  0 →   0 →   -  0 / FWD2
    Failing stage(s):
    [       OK ] FallbacksFixturePropagate.failingNoSolutions (0 ms)
    [ RUN      ] FallbacksFixturePropagate.failingWithFailedSolutions
      0  - ←   0 →   -  0 / task pipeline
        1  - ←   1 →   -  0 / GEN1
        -  0 →   0 →   -  0 / Fallbacks
          -  0 →   0 →   -  0 / FWD1
          -  0 →   0 →   -  0 / FWD2
    Failing stage(s):
    FWD1 (0/1)
    [       OK ] FallbacksFixturePropagate.failingWithFailedSolutions (0 ms)
    [ RUN      ] FallbacksFixturePropagate.computeFirstSuccessfulStageOnly
    [       OK ] FallbacksFixturePropagate.computeFirstSuccessfulStageOnly (1 ms)
    [ RUN      ] FallbacksFixturePropagate.computeFirstSuccessfulStagePerSolutionOnly
    [       OK ] FallbacksFixturePropagate.computeFirstSuccessfulStagePerSolutionOnly (0 ms)
    [ RUN      ] FallbacksFixturePropagate.successfulWithMixedSolutions
    [       OK ] FallbacksFixturePropagate.successfulWithMixedSolutions (0 ms)
    [ RUN      ] FallbacksFixturePropagate.successfulWithMixedSolutions2
    [       OK ] FallbacksFixturePropagate.successfulWithMixedSolutions2 (0 ms)
    [ RUN      ] FallbacksFixturePropagate.activeChildReset
    [       OK ] FallbacksFixturePropagate.activeChildReset (0 ms)
    [ RUN      ] FallbacksFixturePropagate.filterPropagatesFailures
      0  - ←   0 →   -  0 / task pipeline
        1  - ←   1 →   -  0 / GEN1
        -  0 →   0 →   -  0 / Fallbacks
          -  1 →   0 →   -  0 / filter
            -  0 →   0 →   -  0 / FWD1
          -  1 →   0 →   -  0 / filter
            -  1 →   0 →   -  0 / FWD2
          -  1 →   0 →   -  0 / no-op
    Failing stage(s):
    FWD1 (0/1)
    /home/dani/support_ws/src/moveit_task_constructor/core/test/test_fallback.cpp:180: Failure
    Value of: t.plan()
      Actual: false
    Expected: true
    /home/dani/support_ws/src/moveit_task_constructor/core/test/test_fallback.cpp:181: Failure
    Value of: costs
    Expected: has 1 element that is equal to 2
      Actual: {}
    [  FAILED  ] FallbacksFixturePropagate.filterPropagatesFailures (0 ms)
    [----------] 8 tests from FallbacksFixturePropagate (1 ms total)
    
    [----------] 1 test from FallbacksFixtureConnect
    [ RUN      ] FallbacksFixtureConnect.connectStageInsideFallbacks
    [       OK ] FallbacksFixtureConnect.connectStageInsideFallbacks (0 ms)
    [----------] 1 test from FallbacksFixtureConnect (0 ms total)
    
    [----------] Global test environment tear-down
    [==========] 10 tests from 3 test suites ran. (5 ms total)
    [  PASSED  ] 9 tests.
    [  FAILED  ] 1 test, listed below:
    [  FAILED  ] FallbacksFixturePropagate.filterPropagatesFailures
    
     1 FAILED TEST
      YOU HAVE 2 DISABLED TESTS
    
    -- run_test.py: return code 1
    -- run_test.py: inject classname prefix into gtest result file '/home/dani/support_ws/build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-fallback.gtest.xml'
    -- run_test.py: verify result file '/home/dani/support_ws/build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-fallback.gtest.xml'
  >>>
build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-container.gtest.xml: 19 tests, 0 errors, 0 failures, 0 skipped
build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-cost-queue.gtest.xml: 11 tests, 0 errors, 0 failures, 0 skipped
build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-cost-terms.gtest.xml: 9 tests, 0 errors, 0 failures, 0 skipped
build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-fallback.gtest.xml: 12 tests, 0 errors, 1 failure, 2 skipped
- moveit_task_constructor_core.FallbacksFixturePropagate filterPropagatesFailures
  <<< failure message
    /home/dani/support_ws/src/moveit_task_constructor/core/test/test_fallback.cpp:180
    Value of: t.plan()
      Actual: false
    Expected: true
    /home/dani/support_ws/src/moveit_task_constructor/core/test/test_fallback.cpp:181
    Value of: costs
    Expected: has 1 element that is equal to 2
      Actual: {}
  >>>
build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-interface-state.gtest.xml: 3 tests, 0 errors, 0 failures, 0 skipped
build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-mockups.gtest.xml: 2 tests, 0 errors, 0 failures, 0 skipped
build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-properties.gtest.xml: 13 tests, 0 errors, 0 failures, 0 skipped
build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-pruning.gtest.xml: 13 tests, 0 errors, 0 failures, 0 skipped
build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-serial.gtest.xml: 3 tests, 0 errors, 0 failures, 0 skipped
build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/moveit_task_constructor_core-test-stage.gtest.xml: 4 tests, 0 errors, 0 failures, 0 skipped
build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/test-move-relative.xunit.xml: 2 tests, 0 errors, 0 failures, 0 skipped
build/moveit_task_constructor_core/test_results/moveit_task_constructor_core/test-move-to.xunit.xml: 2 tests, 0 errors, 0 failures, 0 skipped

Summary: 105 tests, 0 errors, 2 failures, 2 skipped

[DaniGarciaLopez]

It seems that in my last message I was not using the latest commits from both MTC and moveit2. Using the latest changes has apparently resolved the issue, as the test you provided no longer fails and the example I shared works as expected:

I will do some checks over the next few days, but it seems we can close this issue for now.