From d3864596b685a0d4d982564f11d626a18cc2b859 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 8 Dec 2020 00:26:02 +0100 Subject: [PATCH] Replace Priority::enabled() with status() The key to pruning in the Connecting stage was the following: - Don't remove states during pruning, but only disable them. They might become re-enabled due to further input. - Distinguish START and END sides of a disabled solution tree to break their symmetry. The START side from where we started disabling, can be re-enabled by a new partner state in Connecting, the END side must not. This was important as, otherwise, the states would simply get re-enabled immediately. The END side only gets re-enabled if the START side actually connects the whole solution branch. --- .../include/moveit/task_constructor/stage_p.h | 4 +++ .../include/moveit/task_constructor/storage.h | 21 ++++++++---- core/src/container.cpp | 26 ++++----------- core/src/stage.cpp | 33 +++++++++++++++++-- core/src/storage.cpp | 25 ++++++++------ core/test/test_interface_state.cpp | 27 ++++++++------- 6 files changed, 84 insertions(+), 52 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 3d14731a..676a2149 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -157,6 +157,10 @@ public: /** compute cost for solution through configured CostTerm */ void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); + /// Set ENABLED / DISABLED status of the solution tree starting from s into given direction + template + static void setStatus(const InterfaceState* s, InterfaceState::Status status); + protected: // associated/owning Stage instance Stage* me_; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 888b1f5b..2b52064b 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -77,26 +77,33 @@ class InterfaceState friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase friend class Interface; // allow Interface to set owner_ and priority_ public: + enum Status + { + ENABLED, + DISABLED_START, // state that was the root cause for disabling due to failure + DISABLED_END // state on the other end of a disabled solution sequence + }; /** InterfaceStates are ordered according to two values: * Depth of interlinked trajectory parts and accumulated trajectory costs along that path. * Preference ordering considers high-depth first and within same depth, minimal cost paths. */ - struct Priority : std::tuple + struct Priority : std::tuple { - Priority(unsigned int depth, double cost, bool enabled = true) - : std::tuple(enabled, depth, cost) { + Priority(unsigned int depth, double cost, Status status = ENABLED) + : std::tuple(status, depth, cost) { assert(std::isfinite(cost)); } - // Constructor copying depth and cost, but modifying enabled status - Priority(const Priority& other, bool enabled) : Priority(other.depth(), other.cost(), enabled) {} + // Constructor copying depth and cost, but modifying its status + Priority(const Priority& other, Status status) : Priority(other.depth(), other.cost(), status) {} - inline bool enabled() const { return std::get<0>(*this); } + inline Status status() const { return std::get<0>(*this); } + inline bool enabled() const { return std::get<0>(*this) == ENABLED; } inline unsigned int depth() const { return std::get<1>(*this); } inline double cost() const { return std::get<2>(*this); } // add priorities Priority operator+(const Priority& other) const { - return Priority(depth() + other.depth(), cost() + other.cost(), enabled() || other.enabled()); + return Priority(depth() + other.depth(), cost() + other.cost(), std::min(status(), other.status())); } // comparison operators bool operator<(const Priority& rhs) const; diff --git a/core/src/container.cpp b/core/src/container.cpp index aebd2b92..a80b10a9 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -341,8 +341,8 @@ struct SolutionCollector inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) { if (state->owner()) // owner becomes NULL if state is removed from (pending) Interface list state->owner()->updatePriority(const_cast(state), - // update depth + cost, but keep current enabled status - InterfaceState::Priority(prio, state->priority().enabled())); + // update depth + cost, but keep current status + InterfaceState::Priority(prio, state->priority().status())); } template @@ -406,20 +406,6 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { impl->liftSolution(solution, solution->internalStart(), solution->internalEnd()); } -template -void markAsEnabled(const InterfaceState* s, bool enabled) { - if (s->priority().enabled() == enabled) - return; // nothing changing - - // actually enable/disable the state - if (s->owner()) - s->owner()->updatePriority(const_cast(s), InterfaceState::Priority(s->priority(), enabled)); - - // traverse solution tree - for (const SolutionBase* successor : trajectories(s)) - markAsEnabled(state(*successor), enabled); -} - void SerialContainer::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { switch (child.pimpl()->interfaceFlags()) { case GENERATE: @@ -427,19 +413,19 @@ void SerialContainer::onNewFailure(const Stage& child, const InterfaceState* fro // TODO: If child is a container, from and to might have associated solutions already! case PROPAGATE_FORWARDS: // mark from as dead (backwards) - markAsEnabled(from, false); + StagePrivate::setStatus(from, InterfaceState::Status::DISABLED_START); break; case PROPAGATE_BACKWARDS: // mark to as dead (forwards) - markAsEnabled(to, false); + StagePrivate::setStatus(to, InterfaceState::Status::DISABLED_START); break; case CONNECT: if (const Connecting* conn = dynamic_cast(&child)) { auto cimpl = conn->pimpl(); if (!cimpl->hasPendingOpposites(from)) - markAsEnabled(from, false); + StagePrivate::setStatus(from, InterfaceState::Status::DISABLED_START); if (!cimpl->hasPendingOpposites(to)) - markAsEnabled(to, false); + StagePrivate::setStatus(to, InterfaceState::Status::DISABLED_START); } break; } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 93a1bf0e..e1558f0c 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -148,6 +148,30 @@ bool StagePrivate::storeSolution(const SolutionBasePtr& solution, const Interfac return true; } +template +void StagePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) { + if (s->priority().status() == status) + return; // nothing changing + + // actually enable/disable the state + if (s->owner()) + s->owner()->updatePriority(const_cast(s), InterfaceState::Priority(s->priority(), status)); + + // To break symmetry between both ends of a partial solution sequence that gets disabled, + // we mark the start state with START and all other states down the tree with END. + // This allows us to re-enable the START side, while not (yet) consider the END side, + // when a new states arrives in a Connecting stage. + // The END side is only re-enabled if the START side actually was connected with a solution. + if (status == InterfaceState::Status::DISABLED_START) + status = InterfaceState::Status::DISABLED_END; // ensure that only the first state is marked as START + + // traverse solution tree + for (const SolutionBase* successor : trajectories(s)) + setStatus(state(*successor), status); +} +template void StagePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status); +template void StagePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status); + void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, const SolutionBasePtr& solution) { assert(nextStarts()); @@ -705,7 +729,8 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In template void ConnectingPrivate::newState(Interface::iterator it, bool updated) { if (updated) { // many pairs might be affected: resort - if (!it->priority().enabled()) // remove all pending pairs involving this state + if (it->priority().status() == InterfaceState::Status::DISABLED_END) + // remove all pending pairs involving this state pending.remove_if([it](const StatePair& p) { return std::get()>(p) == it; }); else pending.sort(); @@ -713,9 +738,11 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { assert(it->priority().enabled()); // new solutions are feasible, aren't they? InterfacePtr other_interface = pullInterface(other); for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { + // Don't re-enable states that are marked DISABLED_END if (static_cast(me_)->compatible(*it, *oit)) { - // if needed, re-enable the opposing state oit - oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), true)); + // re-enable the opposing state oit if its status is DISABLED_START + if (oit->priority().status() == InterfaceState::DISABLED_START) + oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), InterfaceState::ENABLED)); pending.insert(make_pair(it, oit)); } } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index d370cd0c..dcb01a50 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -70,15 +70,16 @@ InterfaceState::InterfaceState(const InterfaceState& other) : scene_(other.scene_), properties_(other.properties_), priority_(other.priority_) {} bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) const { - // disabled states always after enabled ones - if (enabled() != other.enabled()) - return enabled(); + // first order by status if that differs + if (status() != other.status()) + return status() < other.status(); - // both prios have same enabled_ state - if (depth() == other.depth()) - return cost() < other.cost(); - else + // then by depth if that differs + if (depth() != other.depth()) return depth() > other.depth(); // larger depth = smaller prio! + + // finally by cost + return cost() < other.cost(); } Interface::Interface(const Interface::NotifyFunction& notify) : notify_(notify) {} @@ -142,10 +143,14 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) { return os; } std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) { - static const char* red = "\033[31m"; - static const char* green = "\033[32m"; + // maps InterfaceState::Status values to output (color-changing) prefix + static const char* prefix[] = { + "\033[32me:", // ENABLED - green + "\033[33md:", // DISABLED - yellow + "\033[31mf:", // DISABLED_FAILED - red + }; static const char* color_reset = "\033[m"; - os << (prio.enabled() ? green : red) << prio.depth() << ":" << prio.cost() << color_reset; + os << prefix[prio.status()] << prio.depth() << ":" << prio.cost() << color_reset; return os; } diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 4e88f996..1de95a75 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -19,14 +19,15 @@ TEST(InterfaceStatePriority, compare) { EXPECT_TRUE(Prio(1, 42) < Prio(0, 0)); EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // at same depth, higher cost is larger - EXPECT_TRUE(Prio(0, 0, false) == Prio(0, 0, false)); - EXPECT_TRUE(Prio(1, 0, false) < Prio(0, 0, false)); - EXPECT_TRUE(Prio(1, 42, false) < Prio(0, 0, false)); - EXPECT_TRUE(Prio(0, 0, false) < Prio(0, 42, false)); + auto dstart = InterfaceState::DISABLED_START; + EXPECT_TRUE(Prio(0, 0, dstart) == Prio(0, 0, dstart)); + EXPECT_TRUE(Prio(1, 0, dstart) < Prio(0, 0, dstart)); + EXPECT_TRUE(Prio(1, 42, dstart) < Prio(0, 0, dstart)); + EXPECT_TRUE(Prio(0, 0, dstart) < Prio(0, 42, dstart)); // disabled prios are always larger than enabled ones - EXPECT_TRUE(Prio(0, 42) < Prio(1, 0, false)); - EXPECT_TRUE(Prio(1, 0) < Prio(0, 42, false)); + EXPECT_TRUE(Prio(0, 42) < Prio(1, 0, dstart)); + EXPECT_TRUE(Prio(1, 0) < Prio(0, 42, dstart)); // other comparison operators EXPECT_TRUE(Prio(0, 0) <= Prio(0, 0)); @@ -67,7 +68,7 @@ TEST(Interface, update) { i.updatePriority(*i.rbegin(), Prio(5, 0.0)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); - i.updatePriority(*i.begin(), Prio(6, 0, false)); + i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::DISABLED_START)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 })); } @@ -78,14 +79,16 @@ inline bool operator<(const PrioPair& lhs, const PrioPair& rhs) { PrioPair pair(Prio&& p1, Prio&& p2) { return std::make_pair(std::move(p1), std::move(p2)); } -PrioPair pair(bool e1, bool e2) { - return pair(Prio(0, 0, e1), Prio(0, 0, e2)); +PrioPair pair(InterfaceState::Status s1, InterfaceState::Status s2) { + return pair(Prio(0, 0, s1), Prio(0, 0, s2)); } TEST(StatePairs, compare) { EXPECT_TRUE(pair(Prio(1, 0), Prio(0, 1)) < pair(Prio(1, 1), Prio(0, 1))); EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0))); - EXPECT_TRUE(pair(true, true) < pair(true, false)); - EXPECT_TRUE(pair(true, true) < pair(false, true)); - EXPECT_TRUE(pair(false, true) < pair(true, false)); + auto good = InterfaceState::ENABLED; + auto bad = InterfaceState::DISABLED_START; + EXPECT_TRUE(pair(good, good) < pair(good, bad)); + EXPECT_TRUE(pair(good, good) < pair(bad, good)); + EXPECT_TRUE(pair(bad, good) < pair(good, bad)); }