diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 872d5c33..7fe118e3 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -324,6 +324,10 @@ public: bool canCompute() const override; void compute() override; + // Check whether there are pending feasible states that could connect to source + template + bool hasPendingOpposites(const InterfaceState* source) const; + private: // Create a pair of Interface states for pending list, such that the order (start, end) is maintained template diff --git a/core/src/container.cpp b/core/src/container.cpp index 4ea046c0..c3e05c3f 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -281,7 +281,7 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) { return os; } -/** Collect all potential solutions originating from start */ +/** Collect all partial solution sequences originating from start into given direction */ template struct SolutionCollector { @@ -299,9 +299,7 @@ struct SolutionCollector solutions.emplace_back(std::make_pair(trace, prio)); } else { for (SolutionBase* successor : next) { - if (successor->isFailure()) // skip failure "solutions" - continue; - + assert(!successor->isFailure()); // We shouldn't have invalid solutions trace.push_back(successor); traverse(*successor, prio + InterfaceState::Priority(1, successor->cost())); trace.pop_back(); @@ -316,11 +314,21 @@ struct SolutionCollector }; inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) { - if (state->owner()) - state->owner()->updatePriority(const_cast(state), 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())); +} + +template +inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path, + const InterfaceState::Priority& prio) { + for (const SolutionBase* solution : partial_solution_path) + updateStatePrio(state(*solution), prio); } void SerialContainer::onNewSolution(const SolutionBase& current) { + assert(!current.isFailure()); auto impl = pimpl(); const Stage* creator = current.creator(); auto& children = impl->children(); @@ -344,9 +352,9 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { for (auto& in : incoming.solutions) { for (auto& out : outgoing.solutions) { InterfaceState::Priority prio = in.second + InterfaceState::Priority(1u, current.cost()) + out.second; + assert(prio.enabled()); // found a complete solution path connecting start to end? if (prio.depth() == children.size()) { - assert(prio.enabled()); assert(solution.empty()); // insert incoming solutions in reverse order solution.insert(solution.end(), in.first.rbegin(), in.first.rend()); @@ -356,20 +364,60 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { solution.insert(solution.end(), out.first.begin(), out.first.end()); // store solution in sorted list sorted.insert(std::make_shared(std::move(solution), prio.cost(), this)); - } else if (prio.depth() > 1) { - // update state priorities at both ends of newly created partial solution - updateStatePrio((in.first.empty() ? current : *in.first.back()).start(), prio); - updateStatePrio((out.first.empty() ? current : *out.first.back()).end(), prio); + } + if (prio.depth() > 1) { + // update state priorities along the whole partial solution path + updateStatePrio(current.start(), prio); + updateStatePrio(current.end(), prio); + updateStatePrios(in.first, prio); + updateStatePrios(out.first, prio); } } } - // finally store + announce new solutions to external interface + // finally, store + announce new solutions to external interface for (const auto& solution : sorted) impl->liftSolution(solution, solution->internalStart(), solution->internalEnd()); } -void SerialContainer::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {} +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: + break; // 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! + + case PROPAGATE_FORWARDS: // mark from as dead (backwards) + markAsEnabled(from, false); + break; + case PROPAGATE_BACKWARDS: // mark to as dead (forwards) + markAsEnabled(to, false); + break; + + case CONNECT: + if (const Connecting* conn = dynamic_cast(&child)) { + auto cimpl = conn->pimpl(); + if (!cimpl->hasPendingOpposites(from)) + markAsEnabled(from, false); + if (!cimpl->hasPendingOpposites(to)) + markAsEnabled(to, false); + } + break; + } +} SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(impl) {} SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {} diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 6dc43749..5ceb7614 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -704,18 +704,47 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In template void ConnectingPrivate::newState(Interface::iterator it, bool updated) { - if (updated) { - // many pairs might be affected: sort - pending.sort(); + if (updated) { // many pairs might be affected: resort + if (!it->priority().enabled()) // remove all pending pairs involving this state + pending.remove_if([it](const StatePair& p) { return std::get()>(p) == it; }); + else + pending.sort(); } else { // new state: insert all pairs with other interface + 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) { - if (static_cast(me_)->compatible(*it, *oit)) + if (static_cast(me_)->compatible(*it, *oit)) { + // if needed, re-enable the opposing state oit + oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), true)); pending.insert(make_pair(it, oit)); + } } } } +// Check whether there are pending feasible states that could connect to source. +// If not, we exhausted all solution candidates for source and thus should mark it as failure. +template +inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const { + for (const auto& candidate : this->pending) { + static_assert(Interface::FORWARD == 0, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); + const auto src = std::get(candidate); + static_assert(Interface::BACKWARD == 1, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); + const auto tgt = std::get()>(candidate); + + if (&*src == source && tgt->priority().enabled()) + return true; + + // early stopping when only infeasible pairs are to come + if (!std::get<0>(candidate)->priority().enabled()) + break; + } + return false; +} +// explicitly instantiate templates for both directions +template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const; +template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const; + bool ConnectingPrivate::canCompute() const { // Do we still have feasible pending state pairs? return !pending.empty() && pending.front().first->priority().enabled() &&