Fix Pruning #309

rhaschke · 2021-11-19T14:48:24Z

Building on some new (failing) tests introduced by @JafarAbdi and @v4hn, this PR aims to improve (and eventually fix) pruning.
This follows these general ideas:

If a planning stage fails in a linear pipeline, we can skip all remaining planning stages as well (cols of left-most block)
However, we need to keep the states in the interfaces as we might re-enable a path later on (left-most cols of middle block)
A new CONNECT state becomes disabled too (together with its connected branch) (right col of middle block)
We shouldn‘t prune if there are alternative solutions (right block)

Keep the previous logic around for Generator stages. Note that this only makes sense for *pure* Generators and not for MonitoringGenerator, because for the latter we would expect monitored solutions to be passed individually (similar to pruning).

give an elaborate reason for an empty overload that doesn't call the parent.

Note that while this ensures other stages outside the Fallbacks container can compute as well, it does not solve the problem internally. A new incoming state will only ever be considered once the current stage cannot compute any more. We have no way of telling a child to compute for *a specific state* for now. So once we copied a state to its interface we have to let it compute until all possibilities are exhausted to detect whether or not it could generate a solution for it. If we wouldn't do so, there were no way of knowing when to fall back to the next child as long as the stage can still compute on *any* copied solution.

Setting up a demo for Fallbacks({CartesianPath,PTP,RRTConnect}) I found the logic did not work as expected yet. - process last job spec as well - ignore failures when looking for a solution - add more debug output

codecov · 2021-11-19T21:03:53Z

Codecov Report

Merging #309 (5956e70) into master (b675876) will increase coverage by 1.60%.
The diff coverage is 88.06%.

@@            Coverage Diff             @@
##           master     #309      +/-   ##
==========================================
+ Coverage   52.99%   54.59%   +1.60%     
==========================================
  Files         102      102              
  Lines        7598     7832     +234     
==========================================
+ Hits         4026     4275     +249     
+ Misses       3572     3557      -15

Impacted Files	Coverage Δ
core/include/moveit/task_constructor/container.h	`76.93% <0.00%> (-14.74%)`	⬇️
core/include/moveit/task_constructor/task_p.h	`100.00% <ø> (ø)`
core/include/moveit/task_constructor/utils.h	`100.00% <ø> (ø)`
core/include/moveit/task_constructor/storage.h	`89.70% <45.46%> (-5.65%)`	⬇️
core/src/container.cpp	`74.25% <88.55%> (+4.99%)`	⬆️
core/src/storage.cpp	`85.04% <89.48%> (+0.08%)`	⬆️
core/src/stage.cpp	`82.93% <90.77%> (+3.45%)`	⬆️
core/include/moveit/task_constructor/container_p.h	`95.00% <100.00%> (+1.25%)`	⬆️
core/include/moveit/task_constructor/stage_p.h	`91.55% <100.00%> (-0.23%)`	⬇️
core/src/task.cpp	`78.66% <100.00%> (-0.67%)`	⬇️
... and 14 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update b675876...5956e70. Read the comment docs.

Both, failed and pruned states might get re-enabled later! This also required rework (simplification) of the sorting function for pending pairs.

- Centrally distinguish between have owner() or not in InterfaceState::updatePriority() - Have a separate updateStatus() method to just update the pruning status - Split Interface::updatePriority() into a method taking the InterfaceState* and one taking an Interface::iterator (for efficiency) - Early return in container.cpp's updateStatePrios()

- Switch directions: FORWARD <-> BACKWARD to make the function reusable for status propagation. - We need to ignore the source state when looking for opposite states of the target state. Thus add both, source and target state arguments.

rhaschke · 2021-11-20T00:55:26Z

Finally fixed #308. The culprit was that setStatus() was propagating the FAILED status unconditionally through existing CONNECT solutions. However, in this case hasPendingOpposites() needs to be used to stop this propagation if other solution pairs are still feasible.

... to avoid explicit template initialization

rhaschke · 2021-11-20T11:32:52Z

Reenabling pruned solution branches

We need to distinguish the origin of a pruned branch:

If the origin is not a Connecting stage, a branch can only become re-enabled when a new solution appears within this branch.
Newly appearing CONNECT states should get disabled as well. For this mode, I suggest the Status name: PRUNED.
If the origin is a Connecting stage, we need to distinguish the two different situations already mentioned in Rework Pruning #221, namely:
- A newly appearing CONNECT state partnering an initially failed CONNECT state. In this case, the branch can be re-enabled immediately (right-most block). Name suggestion for Status: ARMED (for re-enabling).
- If the new CONNECT state appears in another Connecting stage, the pruned solution branch must not get re-enabled.
  Rather the new state (and its connected branch) should become pruned themselves. (left col of middle block).
  Only if a new state partners the initially failed state, the branch will become re-enabled (right col of middle block).

Summarizing, I suggest the following Status enums:

ENABLED
ARMED: states of a failed Connecting stage that are eligible for re-enabling the branch
PRUNED

Before starting to implement this, it would be great to get some feedback, @v4hn. Next, I will look into #287.

rhaschke · 2021-11-20T14:10:42Z

TODOs

Rename Interface::Status FAILED -> ARMED
Recursively re-enable states when matching an ARMED state
Recursively prune new Connecting state if there is no enabled opposite
Propagate status updates across Connecting stage
Fix failing PruningContainerTests.ConnectReactivatesPrunedPaths for Fallbacks
Distinguish between priority and status updates when crossing container boundaries
Currently, we rely on copyState() calling setStatus() unconditionally on all updates:
https://github.com/ros-planning/moveit_task_constructor/blob/b675876d3b1b860a30330ff5145a0e5f23d243fc/core/src/container.cpp#L199-L205
However, setStatus() only updates the status, not the priority, i.e. cost and depth. Changing the condition to
if (updated.testFlag(Interface::STATUS)) makes some tests fail.

... to better indicate that such a state can be immediately re-enabled.

- Drop variable current_external_state_ - Instead encode the info that the external state wasn't yet forwarded to any child via stage = children().cend() - If all children have exhausted their solutions for this state, it is removed from the pending list

Adapt test results FallbacksFixturePropagate.computeFirstSuccessfulStagePerSolutionOnly due to 2e63c15: The order of computations has changed, because we lock the processed state as soon as it is forwarded to the first fallback child. In this case, after processing GEN1 und FWD1 once, we have the two states with costs 2, 4 in the queue. The first one, i.e. with cost 2 is forwarded to the child FWD2, which fails. In the next cycle, although we have new states in the queue (1, 2, 3, 4), we stick with state "2" and forward it two FWD3, which adds costs 210, resulting in 212. With previous code, the Fallback container switched to state "1", forwarded to FWD2.

Logger config can be more easily handled via ROSCONSOLE_CONFIG_FILE.

- Enable moving/swapping of other container impls (e.g. Fallbacks) - Clarify (via move semantics) that content of source impl will be lost - Get rid of friend declarations

... as we are now missing the implementation for CONNECT interfaces

@v4hn

Further factorize and simplify FallbacksPrivate classes employing ideas from @v4hn. The key difference between the variants his how they advance to the next job. Thus, the only virtual method required is nextJob().

Just set a flag when we received a full solution

- printChildrenInterfaces(): fix/add usage - printPendingPairs(): full colorization according to status

Let's consider the following simple situation, where generators produce solutions in the given order. GEN 1 3 Fallbacks |X GEN 2 4 When passing state 4 to the Fallbacks' connector, it forms pending pairs with both 1 and 3. Thus, the container needs to check whether 1-4 or 3-4 was processed when receiving a success or failure, to correctly forward the failed one to the next child.

... factoring out functionality shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator to switch to next child in nextJob().

rename method to emphasize that state updates are propagated to all children

Implement Fallbacks behavior for children of type Connecting. All other connect-like children are currently infeasible to handle, because we cannot forward a single job, i.e. a pair (from, to) to the next child, but only individual states. However, passing states, will cause creation of undesired state pairs as jobs in subsequent children.

rhaschke · 2022-01-05T19:48:20Z

I merged #311 here, resolving all conflicts. This finalizes this PR (and #311), proving that they nicely play together.
@v4hn, ideally merge by fast-forwarding to ubi-agni@master as I already continued development from there.

v4hn and others added 12 commits November 15, 2021 09:29

add hook to ParallelContainerBase to customize state propagation

f32e747

Implement state-wise Fallbacks

8719b1c

Keep the previous logic around for Generator stages. Note that this only makes sense for *pure* Generators and not for MonitoringGenerator, because for the latter we would expect monitored solutions to be passed individually (similar to pruning).

fallback generator can run a single job per compute call

3d5cecd

simplify onNewFailure

aa733fc

give an elaborate reason for an empty overload that doesn't call the parent.

cleanup: get rid of superfluous parameter

5a9cfc5

order external states

22809c0

fix fallbacks logic

887da5b

Setting up a demo for Fallbacks({CartesianPath,PTP,RRTConnect}) I found the logic did not work as expected yet. - process last job spec as well - ignore failures when looking for a solution - add more debug output

add demo illustrating useful fallbacks behavior

b6ac5b0

Fix -Wsign-compare

ed45970

Fix printChildrenInterfaces()

66e141d

Add more pruning tests

011e4be

rhaschke mentioned this pull request Nov 19, 2021

Bug in pruning #308

Closed

Disable failing tests

c617e33

rhaschke force-pushed the fix-pruning branch from 4368c41 to 011e4be Compare November 19, 2021 20:46

rhaschke added 2 commits November 19, 2021 21:49

Always skip pruning if there exist alternative enabled solutions

718170a

Improve readability

97c2130

rhaschke added 3 commits November 19, 2021 23:03

Never remove pending CONNECT pairs

1ddf7dd

Both, failed and pruned states might get re-enabled later! This also required rework (simplification) of the sorting function for pending pairs.

Drop unused and misleading Direction enums

4170a1c

rhaschke force-pushed the fix-pruning branch from c88d353 to 29d1e44 Compare November 19, 2021 22:04

Fix hasPendingOpposites()

06ae5dd

- Switch directions: FORWARD <-> BACKWARD to make the function reusable for status propagation. - We need to ignore the source state when looking for opposite states of the target state. Thus add both, source and target state arguments.

rhaschke added 2 commits November 20, 2021 06:24

Fix test Pruning.NoPruningIfAlternativesExist

52dc494

Switch order of function declarations

1dda3d1

... to avoid explicit template initialization

rhaschke force-pushed the fix-pruning branch from a3ab8de to 1dda3d1 Compare November 20, 2021 05:37

Rename Interface::Status FAILED -> ARMED

3c4ef68

... to better indicate that such a state can be immediately re-enabled.

rhaschke added 3 commits November 22, 2021 01:39

Simplify computePropagate()

dcb6857

- Drop variable current_external_state_ - Instead encode the info that the external state wasn't yet forwarded to any child via stage = children().cend() - If all children have exhausted their solutions for this state, it is removed from the pending list

debugging helper function

5c235ab

Reintroduce pending state

2e63c15

rhaschke marked this pull request as ready for review November 22, 2021 07:53

rhaschke requested review from JafarAbdi and v4hn November 22, 2021 07:53

rhaschke added a commit to v4hn/moveit_task_constructor that referenced this pull request Nov 22, 2021

Merge PR moveit#309: fix pruning

c4b2004

rhaschke added 9 commits November 24, 2021 15:07

Remove logger configuration

c822fd3

Logger config can be more easily handled via ROSCONSOLE_CONFIG_FILE.

pre-commit autoupdate

b84bb87

static TaskPrivate::swap() -> ContainerBasePrivate::operator=()

e67b325

- Enable moving/swapping of other container impls (e.g. Fallbacks) - Clarify (via move semantics) that content of source impl will be lost - Get rid of friend declarations

Factorize implementation of FallbacksPrivate into 3 classes

8dd8022

Disable failing test FallbacksFixtureConnect.connectStageInsideFallbacks

070c6e9

... as we are now missing the implementation for CONNECT interfaces

Rework FallbacksPrivate*

7237e81

Further factorize and simplify FallbacksPrivate classes employing ideas from @v4hn. The key difference between the variants his how they advance to the next job. Thus, the only virtual method required is nextJob().

Simplify: job_has_solutions_

e296bd7

Just set a flag when we received a full solution

CI: asan with debug symbols

0587129

rhaschke mentioned this pull request Nov 26, 2021

Fallbacks reworked: Factorize FallbacksPrivate into 3 separate classes #311

Merged

5 tasks

rhaschke added 10 commits November 28, 2021 18:28

Improve debug output

4be4486

- printChildrenInterfaces(): fix/add usage - printPendingPairs(): full colorization according to status

Improve comments

442d39a

reset(new Interface()) -> std::make_shared<Interface>()

b2c116e

Improve readability

7af3d8e

FallbacksPrivate::nextChild()

b82b70e

... factoring out functionality shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator to switch to next child in nextJob().

FallbacksPrivateCommon: shared between Generator + Propagator

986d3c8

ParallelContainerBasePrivate::propagateStateTo*All*Children

7a04a9f

rename method to emphasize that state updates are propagated to all children

Merge PR moveit#311: fix Fallbacks

5956e70

rhaschke merged commit c7b2067 into moveit:master Feb 2, 2022

rhaschke deleted the fix-pruning branch May 8, 2022 09:47

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Fix Pruning #309

Fix Pruning #309

Uh oh!

rhaschke commented Nov 19, 2021 •

edited

Loading

Uh oh!

codecov bot commented Nov 19, 2021 •

edited

Loading

Uh oh!

rhaschke commented Nov 20, 2021

Uh oh!

rhaschke commented Nov 20, 2021 •

edited

Loading

Uh oh!

rhaschke commented Nov 20, 2021 •

edited

Loading

Uh oh!

rhaschke commented Jan 5, 2022

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Fix Pruning #309

Fix Pruning #309

Uh oh!

Conversation

rhaschke commented Nov 19, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

codecov bot commented Nov 19, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

rhaschke commented Nov 20, 2021

Uh oh!

rhaschke commented Nov 20, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Reenabling pruned solution branches

Uh oh!

rhaschke commented Nov 20, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

TODOs

Uh oh!

rhaschke commented Jan 5, 2022

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

rhaschke commented Nov 19, 2021 •

edited

Loading

codecov bot commented Nov 19, 2021 •

edited

Loading

rhaschke commented Nov 20, 2021 •

edited

Loading

rhaschke commented Nov 20, 2021 •

edited

Loading