mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
8d25e794bc
commit
4e09d78239
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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());
|
||||
|
||||
@ -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());
|
||||
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user