mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Actually implement pruning and re-enabling of states
This commit is contained in:
parent
19484cec64
commit
604a0da1e4
@ -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 <Interface::Direction dir>
|
||||
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 <Interface::Direction other>
|
||||
|
||||
@ -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 <Interface::Direction dir>
|
||||
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<InterfaceState*>(state), prio);
|
||||
if (state->owner()) // owner becomes NULL if state is removed from (pending) Interface list
|
||||
state->owner()->updatePriority(const_cast<InterfaceState*>(state),
|
||||
// update depth + cost, but keep current enabled status
|
||||
InterfaceState::Priority(prio, state->priority().enabled()));
|
||||
}
|
||||
|
||||
template <Interface::Direction dir>
|
||||
inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path,
|
||||
const InterfaceState::Priority& prio) {
|
||||
for (const SolutionBase* solution : partial_solution_path)
|
||||
updateStatePrio(state<dir>(*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<SolutionSequence>(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<Interface::BACKWARD>(in.first, prio);
|
||||
updateStatePrios<Interface::FORWARD>(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 <Interface::Direction dir>
|
||||
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<InterfaceState*>(s), InterfaceState::Priority(s->priority(), enabled));
|
||||
|
||||
// traverse solution tree
|
||||
for (const SolutionBase* successor : trajectories<dir>(s))
|
||||
markAsEnabled<dir>(state<dir>(*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<Interface::BACKWARD>(from, false);
|
||||
break;
|
||||
case PROPAGATE_BACKWARDS: // mark to as dead (forwards)
|
||||
markAsEnabled<Interface::FORWARD>(to, false);
|
||||
break;
|
||||
|
||||
case CONNECT:
|
||||
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
|
||||
auto cimpl = conn->pimpl();
|
||||
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from))
|
||||
markAsEnabled<Interface::BACKWARD>(from, false);
|
||||
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to))
|
||||
markAsEnabled<Interface::FORWARD>(to, false);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(impl) {}
|
||||
SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {}
|
||||
|
||||
@ -704,18 +704,47 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
|
||||
|
||||
template <Interface::Direction other>
|
||||
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<opposite<other>()>(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<Connecting*>(me_)->compatible(*it, *oit))
|
||||
if (static_cast<Connecting*>(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<other>(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 <Interface::Direction dir>
|
||||
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<dir>(candidate);
|
||||
static_assert(Interface::BACKWARD == 1, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!");
|
||||
const auto tgt = std::get<opposite<dir>()>(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<Interface::FORWARD>(const InterfaceState* source) const;
|
||||
template bool ConnectingPrivate::hasPendingOpposites<Interface::BACKWARD>(const InterfaceState* source) const;
|
||||
|
||||
bool ConnectingPrivate::canCompute() const {
|
||||
// Do we still have feasible pending state pairs?
|
||||
return !pending.empty() && pending.front().first->priority().enabled() &&
|
||||
|
||||
Loading…
Reference in New Issue
Block a user