InterfaceState::Priority::enabled()

To allow pruning, we need to enable and disable InterfaceStates to be considered for further
planning. In the past, we just indicated the disabled status with infinite costs.
However, because we might need to re-enable states (with previous state),
we need to separate these two concepts.
This commit is contained in:
Robert Haschke 2020-12-06 12:24:55 +01:00 committed by v4hn
parent f59e5170fb
commit 2a09daa42f
5 changed files with 44 additions and 35 deletions

View File

@ -81,18 +81,28 @@ public:
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path. * 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. * Preference ordering considers high-depth first and within same depth, minimal cost paths.
*/ */
struct Priority : public std::pair<unsigned int, double> struct Priority : std::tuple<bool, unsigned int, double>
{ {
Priority(unsigned int depth, double cost) : std::pair<unsigned int, double>(depth, cost) {} Priority(unsigned int depth, double cost, bool enabled = true)
: std::tuple<bool, unsigned int, double>(enabled, 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) {}
inline unsigned int depth() const { return this->first; } inline bool enabled() const { return std::get<0>(*this); }
inline double cost() const { return this->second; } inline unsigned int depth() const { return std::get<1>(*this); }
inline double cost() const { return std::get<2>(*this); }
// add priorities // add priorities
Priority operator+(const Priority& other) const { Priority operator+(const Priority& other) const {
return Priority(this->depth() + other.depth(), this->cost() + other.cost()); return Priority(depth() + other.depth(), cost() + other.cost(), enabled() || other.enabled());
} }
bool operator<(const Priority& other) const; // comparison operators
bool operator<(const Priority& rhs) const;
inline bool operator>(const Priority& rhs) const { return rhs < *this; }
inline bool operator<=(const Priority& rhs) const { return !(rhs < *this); }
inline bool operator>=(const Priority& rhs) const { return !(*this < rhs); }
}; };
using Solutions = std::deque<SolutionBase*>; using Solutions = std::deque<SolutionBase*>;

View File

@ -298,7 +298,7 @@ struct SolutionCollector
solutions.emplace_back(std::make_pair(trace, prio)); solutions.emplace_back(std::make_pair(trace, prio));
} else { } else {
for (SolutionBase* successor : next) { for (SolutionBase* successor : next) {
if (successor->isFailure()) if (successor->isFailure()) // skip failure "solutions"
continue; continue;
trace.push_back(successor); trace.push_back(successor);
@ -345,7 +345,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
InterfaceState::Priority prio = in.second + InterfaceState::Priority(1u, current.cost()) + out.second; InterfaceState::Priority prio = in.second + InterfaceState::Priority(1u, current.cost()) + out.second;
// found a complete solution path connecting start to end? // found a complete solution path connecting start to end?
if (prio.depth() == children.size()) { if (prio.depth() == children.size()) {
assert(std::isfinite(prio.cost())); assert(prio.enabled());
assert(solution.empty()); assert(solution.empty());
// insert incoming solutions in reverse order // insert incoming solutions in reverse order
solution.insert(solution.end(), in.first.rbegin(), in.first.rend()); solution.insert(solution.end(), in.first.rbegin(), in.first.rend());

View File

@ -701,16 +701,12 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
template <Interface::Direction other> template <Interface::Direction other>
void ConnectingPrivate::newState(Interface::iterator it, bool updated) { void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
if (!std::isfinite(it->priority().cost()))
return;
if (updated) { if (updated) {
// many pairs might be affected: sort // many pairs might be affected: sort
pending.sort(); pending.sort();
} else { // new state: insert all pairs with other interface } else { // new state: insert all pairs with other interface
InterfacePtr other_interface = pullInterface(other); InterfacePtr other_interface = pullInterface(other);
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
if (!std::isfinite(oit->priority().cost()))
break;
if (static_cast<Connecting*>(me_)->compatible(*it, *oit)) if (static_cast<Connecting*>(me_)->compatible(*it, *oit))
pending.insert(make_pair<other>(it, oit)); pending.insert(make_pair<other>(it, oit));
} }
@ -725,7 +721,7 @@ void ConnectingPrivate::compute() {
const StatePair& top = pending.pop(); const StatePair& top = pending.pop();
const InterfaceState& from = *top.first; const InterfaceState& from = *top.first;
const InterfaceState& to = *top.second; const InterfaceState& to = *top.second;
assert(std::isfinite((from.priority() + to.priority()).cost())); assert(from.priority().enabled() && to.priority().enabled());
static_cast<Connecting*>(me_)->compute(from, to); static_cast<Connecting*>(me_)->compute(from, to);
} }

View File

@ -70,18 +70,15 @@ InterfaceState::InterfaceState(const InterfaceState& other)
: scene_(other.scene_), properties_(other.properties_), priority_(other.priority_) {} : scene_(other.scene_), properties_(other.properties_), priority_(other.priority_) {}
bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) const { bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) const {
// infinite costs go always last // disabled states always after enabled ones
if (std::isinf(this->cost()) && std::isinf(other.cost())) if (enabled() != other.enabled())
return this->depth() > other.depth(); return enabled();
else if (std::isinf(this->cost()))
return false;
else if (std::isinf(other.cost()))
return true;
if (this->depth() == other.depth()) // both prios have same enabled_ state
return this->cost() < other.cost(); if (depth() == other.depth())
return cost() < other.cost();
else else
return this->depth() > other.depth(); return depth() > other.depth(); // larger depth = smaller prio!
} }
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& p) { std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& p) {
@ -113,7 +110,7 @@ void Interface::add(InterfaceState& state) {
else if (!state.outgoingTrajectories().empty()) else if (!state.outgoingTrajectories().empty())
it->priority_ = InterfaceState::Priority(1, state.outgoingTrajectories().front()->cost()); it->priority_ = InterfaceState::Priority(1, state.outgoingTrajectories().front()->cost());
else // otherwise, assume priority was well defined before else // otherwise, assume priority was well defined before
assert(it->priority_ >= InterfaceState::Priority(1, 0.0)); assert(it->priority_.enabled() && it->priority_.depth() >= 1u);
// move list node into interface's state list (sorted by priority) // move list node into interface's state list (sorted by priority)
moveFrom(it, container); moveFrom(it, container);

View File

@ -13,20 +13,26 @@
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
TEST(InterfaceStatePriority, compare) { TEST(InterfaceStatePriority, compare) {
using Prio = InterfaceState::Priority; using Prio = InterfaceState::Priority;
constexpr double inf = std::numeric_limits<double>::infinity();
EXPECT_TRUE(Prio(0, 0) == Prio(0, 0)); EXPECT_TRUE(Prio(0, 0) == Prio(0, 0));
EXPECT_TRUE(Prio(0, inf) == Prio(0, inf));
EXPECT_TRUE(Prio(1, 0) < Prio(0, 0)); // higher depth is smaller EXPECT_TRUE(Prio(1, 0) < Prio(0, 0)); // higher depth is smaller
EXPECT_TRUE(Prio(1, 42) < Prio(0, 0)); EXPECT_TRUE(Prio(1, 42) < Prio(0, 0));
EXPECT_TRUE(Prio(1, inf) > Prio(0, 0)); // infinite costs are always largest EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // at same depth, higher cost is larger
EXPECT_TRUE(Prio(1, inf) < Prio(0, inf));
EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // higher cost is larger EXPECT_TRUE(Prio(0, 0, false) == Prio(0, 0, false));
EXPECT_TRUE(Prio(0, 0) < Prio(0, inf)); EXPECT_TRUE(Prio(1, 0, false) < Prio(0, 0, false));
EXPECT_TRUE(Prio(0, 42) > Prio(0, 0)); EXPECT_TRUE(Prio(1, 42, false) < Prio(0, 0, false));
EXPECT_TRUE(Prio(0, inf) > Prio(0, 0)); EXPECT_TRUE(Prio(0, 0, false) < Prio(0, 42, false));
// 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));
// other comparison operators
EXPECT_TRUE(Prio(0, 0) <= Prio(0, 0));
EXPECT_TRUE(Prio(0, 0) <= Prio(0, 1));
EXPECT_TRUE(Prio(0, 0) > Prio(1, 10));
EXPECT_TRUE(Prio(0, 0) >= Prio(0, 0));
EXPECT_TRUE(Prio(0, 10) >= Prio(0, 0));
} }
using Prio = InterfaceState::Priority; using Prio = InterfaceState::Priority;
@ -60,6 +66,6 @@ TEST(Interface, update) {
i.updatePriority(*i.rbegin(), Prio(5, 0.0)); i.updatePriority(*i.rbegin(), Prio(5, 0.0));
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 }));
i.updatePriority(*i.begin(), Prio(6, std::numeric_limits<double>::infinity())); i.updatePriority(*i.begin(), Prio(6, 0, false));
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); // larger priority is ignored EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); // larger priority is ignored
} }