mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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.
This commit is contained in:
parent
f229743024
commit
d3864596b6
@ -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 <Interface::Direction dir>
|
||||
static void setStatus(const InterfaceState* s, InterfaceState::Status status);
|
||||
|
||||
protected:
|
||||
// associated/owning Stage instance
|
||||
Stage* me_;
|
||||
|
||||
@ -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<bool, unsigned int, double>
|
||||
struct Priority : std::tuple<Status, unsigned int, double>
|
||||
{
|
||||
Priority(unsigned int depth, double cost, bool enabled = true)
|
||||
: std::tuple<bool, unsigned int, double>(enabled, depth, cost) {
|
||||
Priority(unsigned int depth, double cost, Status status = ENABLED)
|
||||
: std::tuple<Status, unsigned int, double>(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;
|
||||
|
||||
@ -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<InterfaceState*>(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 <Interface::Direction dir>
|
||||
@ -406,20 +406,6 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
|
||||
impl->liftSolution(solution, solution->internalStart(), solution->internalEnd());
|
||||
}
|
||||
|
||||
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:
|
||||
@ -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<Interface::BACKWARD>(from, false);
|
||||
StagePrivate::setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_START);
|
||||
break;
|
||||
case PROPAGATE_BACKWARDS: // mark to as dead (forwards)
|
||||
markAsEnabled<Interface::FORWARD>(to, false);
|
||||
StagePrivate::setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_START);
|
||||
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);
|
||||
StagePrivate::setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_START);
|
||||
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to))
|
||||
markAsEnabled<Interface::FORWARD>(to, false);
|
||||
StagePrivate::setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_START);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
@ -148,6 +148,30 @@ 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
|
||||
|
||||
// actually enable/disable the state
|
||||
if (s->owner())
|
||||
s->owner()->updatePriority(const_cast<InterfaceState*>(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<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());
|
||||
|
||||
@ -705,7 +729,8 @@ 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: 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<opposite<other>()>(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<Connecting*>(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<other>(it, oit));
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user