mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
f59e5170fb
commit
2a09daa42f
@ -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<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 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<SolutionBase*>;
|
||||
|
||||
|
||||
@ -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());
|
||||
|
||||
@ -701,16 +701,12 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
|
||||
|
||||
template <Interface::Direction other>
|
||||
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<Connecting*>(me_)->compatible(*it, *oit))
|
||||
pending.insert(make_pair<other>(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<Connecting*>(me_)->compute(from, to);
|
||||
}
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -13,20 +13,26 @@
|
||||
using namespace moveit::task_constructor;
|
||||
TEST(InterfaceStatePriority, compare) {
|
||||
using Prio = InterfaceState::Priority;
|
||||
constexpr double inf = std::numeric_limits<double>::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<double>::infinity()));
|
||||
i.updatePriority(*i.begin(), Prio(6, 0, false));
|
||||
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); // larger priority is ignored
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user