From 2a09daa42fc9155ad3a734464d27ce9008c369c5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 6 Dec 2020 12:24:55 +0100 Subject: [PATCH] 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. --- .../include/moveit/task_constructor/storage.h | 22 +++++++++++---- core/src/container.cpp | 4 +-- core/src/stage.cpp | 6 +--- core/src/storage.cpp | 19 ++++++------- core/test/test_interface_state.cpp | 28 +++++++++++-------- 5 files changed, 44 insertions(+), 35 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 5832e250..deca3360 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -81,18 +81,28 @@ public: * 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 : public std::pair + struct Priority : std::tuple { - Priority(unsigned int depth, double cost) : std::pair(depth, cost) {} + Priority(unsigned int depth, double cost, bool enabled = true) + : std::tuple(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 double cost() const { return this->second; } + inline bool enabled() const { return std::get<0>(*this); } + 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(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; diff --git a/core/src/container.cpp b/core/src/container.cpp index f66314e4..14a4f1c7 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -298,7 +298,7 @@ struct SolutionCollector solutions.emplace_back(std::make_pair(trace, prio)); } else { for (SolutionBase* successor : next) { - if (successor->isFailure()) + if (successor->isFailure()) // skip failure "solutions" continue; 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; // found a complete solution path connecting start to end? if (prio.depth() == children.size()) { - assert(std::isfinite(prio.cost())); + assert(prio.enabled()); assert(solution.empty()); // insert incoming solutions in reverse order solution.insert(solution.end(), in.first.rbegin(), in.first.rend()); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index aa3cdec6..2f071763 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -701,16 +701,12 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In template void ConnectingPrivate::newState(Interface::iterator it, bool updated) { - if (!std::isfinite(it->priority().cost())) - return; if (updated) { // many pairs might be affected: sort pending.sort(); } else { // new state: insert all pairs with other interface InterfacePtr other_interface = pullInterface(other); for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { - if (!std::isfinite(oit->priority().cost())) - break; if (static_cast(me_)->compatible(*it, *oit)) pending.insert(make_pair(it, oit)); } @@ -725,7 +721,7 @@ void ConnectingPrivate::compute() { const StatePair& top = pending.pop(); const InterfaceState& from = *top.first; const InterfaceState& to = *top.second; - assert(std::isfinite((from.priority() + to.priority()).cost())); + assert(from.priority().enabled() && to.priority().enabled()); static_cast(me_)->compute(from, to); } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index ad5e8314..f79ff86b 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -70,18 +70,15 @@ InterfaceState::InterfaceState(const InterfaceState& other) : scene_(other.scene_), properties_(other.properties_), priority_(other.priority_) {} bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) const { - // infinite costs go always last - if (std::isinf(this->cost()) && std::isinf(other.cost())) - return this->depth() > other.depth(); - else if (std::isinf(this->cost())) - return false; - else if (std::isinf(other.cost())) - return true; + // disabled states always after enabled ones + if (enabled() != other.enabled()) + return enabled(); - if (this->depth() == other.depth()) - return this->cost() < other.cost(); + // both prios have same enabled_ state + if (depth() == other.depth()) + return cost() < other.cost(); else - return this->depth() > other.depth(); + return depth() > other.depth(); // larger depth = smaller prio! } std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& p) { @@ -113,7 +110,7 @@ void Interface::add(InterfaceState& state) { else if (!state.outgoingTrajectories().empty()) it->priority_ = InterfaceState::Priority(1, state.outgoingTrajectories().front()->cost()); 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) moveFrom(it, container); diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 5aa887fc..54b26777 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -13,20 +13,26 @@ using namespace moveit::task_constructor; TEST(InterfaceStatePriority, compare) { using Prio = InterfaceState::Priority; - constexpr double inf = std::numeric_limits::infinity(); - 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, 42) < Prio(0, 0)); - EXPECT_TRUE(Prio(1, inf) > Prio(0, 0)); // infinite costs are always largest - EXPECT_TRUE(Prio(1, inf) < Prio(0, inf)); + EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // at same depth, higher cost is larger - EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // higher cost is larger - EXPECT_TRUE(Prio(0, 0) < Prio(0, inf)); - EXPECT_TRUE(Prio(0, 42) > Prio(0, 0)); - EXPECT_TRUE(Prio(0, inf) > Prio(0, 0)); + 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)); + + // 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; @@ -60,6 +66,6 @@ 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, std::numeric_limits::infinity())); + i.updatePriority(*i.begin(), Prio(6, 0, false)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); // larger priority is ignored }