Implement pruning inside-to-outside of a container

- Remove public onNewFailure() interface
  Moved to ContainerBasePrivate to reuse logic for serial and parallel containers.
- Add tests
This commit is contained in:
v4hn 2021-03-30 15:14:38 +02:00 committed by Robert Haschke
parent 8d25e794bc
commit 4e09d78239
7 changed files with 152 additions and 83 deletions

View File

@ -77,8 +77,6 @@ public:
/// called by a (direct) child when a new solution becomes available
virtual void onNewSolution(const SolutionBase& s) = 0;
/// called by a (direct) child when a solution failed
virtual void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) = 0;
protected:
ContainerBase(ContainerBasePrivate* impl);
@ -98,9 +96,7 @@ public:
protected:
void onNewSolution(const SolutionBase& s) override;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
protected:
SerialContainer(SerialContainerPrivate* impl);
};
@ -118,7 +114,6 @@ class ParallelContainerBase : public ContainerBase
public:
PRIVATE_CLASS(ParallelContainerBase)
ParallelContainerBase(const std::string& name = "parallel container");
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl);

View File

@ -125,6 +125,9 @@ public:
const auto& internalToExternalMap() const { return internal_external_.left; }
const auto& externalToInternalMap() const { return internal_external_.right; }
/// called by a (direct) child when a solution failed
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to);
protected:
ContainerBasePrivate(ContainerBase* me, const std::string& name);
@ -140,6 +143,10 @@ protected:
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
}
/// Set ENABLED / DISABLED status of the solution tree starting from s into given direction
template <Interface::Direction dir>
void setStatus(const InterfaceState* s, InterfaceState::Status status);
/// copy external_state to a child's interface and remember the link in internal_external map
template <Interface::Direction>
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);

View File

@ -157,10 +157,6 @@ 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 <Interface::Direction dir>
static void setStatus(const InterfaceState* s, InterfaceState::Status status);
protected:
// associated/owning Stage instance
Stage* me_;

View File

@ -76,7 +76,7 @@ class InterfaceState
{
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
friend class Interface; // allow Interface to set owner_ and priority_
friend class StagePrivate;
friend class ContainerBasePrivate; // allow setting priority_ for pruning
public:
enum Status

View File

@ -106,6 +106,85 @@ void ContainerBasePrivate::compute() {
static_cast<ContainerBase*>(me_)->compute();
}
void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
switch (child.pimpl()->interfaceFlags()) {
case GENERATE:
// just ignore: the pair of (new) states isn't known to us anyway
// TODO: If child is a container, from and to might have associated solutions already!
break;
case PROPAGATE_FORWARDS: // mark from as failed (backwards)
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_FAILED);
break;
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_FAILED);
break;
case CONNECT:
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
auto cimpl = conn->pimpl();
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from))
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_FAILED);
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to))
setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_FAILED);
}
break;
}
// printChildrenInterfaces(*this, false, child);
}
template <Interface::Direction dir>
void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) {
if (s->priority().status() == status)
return; // nothing changing
// if we should disable the state, only do so when there is no enabled alternative path
if (status == InterfaceState::DISABLED) {
auto solution_is_enabled = [](auto&& solution) {
return state<opposite<dir>()>(*solution)->priority().enabled();
};
const auto& alternatives = trajectories<opposite<dir>()>(*s);
auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled);
if (alternative_path != alternatives.cend())
return;
}
// actually enable/disable the state
if (s->owner()) {
s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), status));
} else {
const_cast<InterfaceState*>(s)->priority_ = InterfaceState::Priority(s->priority(), status);
}
// if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface
if (parent()) {
auto external{ internalToExternalMap().find(s) };
if (external != internalToExternalMap().end()) {
// only escalate if there is no enabled alternative child
auto internals{ externalToInternalMap().equal_range(external->second) };
auto is_enabled = [](auto&& state_pair) { return state_pair.second->priority().enabled(); };
auto other_path{ std::find_if(internals.first, internals.second, is_enabled) };
if (other_path == internals.second)
parent()->pimpl()->setStatus<dir>(external->second, status);
return;
}
}
// To break symmetry between both ends of a partial solution sequence that gets disabled,
// we mark the first state with DISABLED_FAILED and all other states down the tree only with DISABLED.
// This allows us to re-enable the FAILED side, while not (yet) consider the DISABLED states again,
// when new states arrive in a Connecting stage.
// All DISABLED states are only re-enabled if the FAILED state actually gets connected.
if (status == InterfaceState::DISABLED_FAILED)
status = InterfaceState::DISABLED; // only the first state is marked as FAILED
// traverse solution tree
for (const SolutionBase* successor : trajectories<dir>(*s))
setStatus<dir>(state<dir>(*successor), status);
}
template void ContainerBasePrivate::setStatus<Interface::FORWARD>(const InterfaceState*, InterfaceState::Status);
template void ContainerBasePrivate::setStatus<Interface::BACKWARD>(const InterfaceState*, InterfaceState::Status);
template <Interface::Direction dir>
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
if (updated) {
@ -417,33 +496,6 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
impl->liftSolution(solution, solution->internalStart(), solution->internalEnd());
}
void SerialContainer::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
switch (child.pimpl()->interfaceFlags()) {
case GENERATE:
// just ignore: the pair of (new) states isn't known to us anyway
// TODO: If child is a container, from and to might have associated solutions already!
break;
case PROPAGATE_FORWARDS: // mark from as failed (backwards)
StagePrivate::setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_FAILED);
break;
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
StagePrivate::setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_FAILED);
break;
case CONNECT:
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
auto cimpl = conn->pimpl();
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from))
StagePrivate::setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_FAILED);
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to))
StagePrivate::setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_FAILED);
}
break;
}
// printChildrenInterfaces(*this, false, child);
}
SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(impl) {}
SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {}
@ -680,8 +732,6 @@ ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl)
ParallelContainerBase::ParallelContainerBase(const std::string& name)
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
void ParallelContainerBase::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {}
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)),
solution.start(), solution.end());

View File

@ -138,7 +138,7 @@ bool StagePrivate::storeSolution(const SolutionBasePtr& solution, const Interfac
if (solution->isFailure()) {
++num_failures_;
if (parent())
parent()->onNewFailure(*me(), from, to);
parent()->pimpl()->onNewFailure(*me(), from, to);
if (!storeFailures())
return false; // drop solution
failures_.push_back(solution);
@ -148,44 +148,6 @@ bool StagePrivate::storeSolution(const SolutionBasePtr& solution, const Interfac
return true;
}
template <Interface::Direction dir>
void StagePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) {
if (s->priority().status() == status)
return; // nothing changing
// if we should disable the state, only do so when there is no enabled alternative path
if (status == InterfaceState::DISABLED) {
auto solution_is_enabled = [](auto&& solution) {
return state<opposite<dir>()>(*solution)->priority().enabled();
};
const auto& alternatives = trajectories<opposite<dir>()>(*s);
auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled);
if (alternative_path != alternatives.cend())
return;
}
if (s->owner()) {
// actually enable/disable the state
s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), status));
} else {
const_cast<InterfaceState*>(s)->priority_ = InterfaceState::Priority(s->priority(), status);
}
// To break symmetry between both ends of a partial solution sequence that gets disabled,
// we mark the first state with DISABLED_FAILED and all other states down the tree only with DISABLED.
// This allows us to re-enable the FAILED side, while not (yet) consider the DISABLED states again,
// when new states arrive in a Connecting stage.
// All DISABLED states are only re-enabled if the FAILED state actually gets connected.
if (status == InterfaceState::DISABLED_FAILED)
status = InterfaceState::DISABLED; // only the first state is marked as FAILED
// traverse solution tree
for (const SolutionBase* successor : trajectories<dir>(*s))
setStatus<dir>(state<dir>(*successor), status);
}
template void StagePrivate::setStatus<Interface::FORWARD>(const InterfaceState* s, InterfaceState::Status status);
template void StagePrivate::setStatus<Interface::BACKWARD>(const InterfaceState* s, InterfaceState::Status status);
void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, const SolutionBasePtr& solution) {
assert(nextStarts());

View File

@ -198,7 +198,7 @@ TEST(Pruning, PropagatorFailure) {
t.add(Stage::pointer(new GeneratorMockup({ 0 })));
t.add(Stage::pointer(new ForwardMockup({ inf })));
t.plan();
EXPECT_FALSE(t.plan());
ASSERT_EQ(t.solutions().size(), 0);
// ForwardMockup fails, so the backward stage should never compute
@ -218,7 +218,7 @@ TEST(Pruning, PruningMultiForward) {
// fail to extend the second solution
t.add(Stage::pointer(new ForwardMockup({ 0, inf })));
t.plan();
EXPECT_TRUE(t.plan());
// the second (infeasible) solution in the last stage must not disable
// the earlier partial solution just because they share stage solutions
@ -287,14 +287,14 @@ TEST(Pruning, PropagateInsideContainerBoundaries) {
add(*c, new GeneratorMockup({ 0 }));
t.add(std::move(c));
t.plan();
EXPECT_FALSE(t.plan());
// the failure in the backward stage (outside the container)
// should prune the expected computation of con
EXPECT_EQ(con->calls_, 0);
}
TEST(Pruning, DISABLED_PropagateOutsideContainerBoundaries) {
TEST(Pruning, PropagateOutsideContainerBoundaries) {
resetIds();
Task t;
t.setRobotModel(getModel());
@ -306,8 +306,67 @@ TEST(Pruning, DISABLED_PropagateOutsideContainerBoundaries) {
add(*c, new ForwardMockup());
t.add(std::move(c));
t.plan();
EXPECT_FALSE(t.plan());
// the failure inside the container should prune computing of back
EXPECT_EQ(back->calls_, 0);
}
TEST(Pruning, PropagateOutsideParallelContainerBoundariesSinglePathPull) {
resetIds();
Task t;
t.setRobotModel(getModel());
auto back = add(t, new BackwardMockup());
add(t, new GeneratorMockup({ 0 }));
auto c{ new Alternatives };
add(*c, new ForwardMockup({ inf }));
add(t, c);
EXPECT_FALSE(t.plan());
// the failure in Alternatives must prune computing back
EXPECT_EQ(back->calls_, 0);
}
TEST(Pruning, PropagateOutsideParallelContainerBoundariesSinglePathPush) {
resetIds();
Task t;
t.setRobotModel(getModel());
auto c{ new SerialContainer };
add(*c, new BackwardMockup({ inf }));
add(*c, new GeneratorMockup({ 0 }));
add(t, c);
auto con = add(t, new Connect());
add(t, new GeneratorMockup({ 0 }));
EXPECT_FALSE(t.plan());
// currently this is trivially true because containers only push full solutions
// (so c never contributes a solution con could operate on), but it might change
// in the future and the failure inside the container must still prune computing con
EXPECT_EQ(con->calls_, 0);
}
TEST(Pruning, PropagateOutsideParallelContainerBoundariesMultiplePaths) {
resetIds();
Task t;
t.setRobotModel(getModel());
auto back = add(t, new BackwardMockup());
add(t, new GeneratorMockup({ 0 }));
auto c{ new Alternatives };
auto s1{ new SerialContainer };
add(*s1, new Connect());
add(*s1, new GeneratorMockup({ 0 }));
add(*c, s1);
add(*c, new ForwardMockup({ inf }));
add(t, c);
EXPECT_TRUE(t.plan());
// the failure in one branch of Alternatives must not prune computing back
EXPECT_EQ(back->calls_, 1);
}