Restore pruning

If a stage fails to find a solution, this often implies that further planning
on the open end(s) of connected InterfaceStates is not needed anymore.
Thus the InterfaceStates along all connected solution paths will be marked as disabled.
They are not removed from the pending state lists though, because they might get
reactivated by solutions found in future.

To this end, we introduced the method ContainerBase::onNewFailure().
This commit is contained in:
Robert Haschke 2020-12-06 23:01:28 +01:00 committed by v4hn
parent 6c18e2b482
commit 19484cec64
8 changed files with 47 additions and 21 deletions

View File

@ -77,6 +77,8 @@ public:
/// called by a (direct) child when a new solution becomes available /// called by a (direct) child when a new solution becomes available
virtual void onNewSolution(const SolutionBase& s) = 0; virtual void onNewSolution(const SolutionBase& s) = 0;
/// called by a (direct) child when a solution failed
virtual void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) = 0;
protected: protected:
ContainerBase(ContainerBasePrivate* impl); ContainerBase(ContainerBasePrivate* impl);
@ -95,8 +97,8 @@ public:
void compute() override; void compute() override;
protected: protected:
/// called by a (direct) child when a new solution becomes available
void onNewSolution(const SolutionBase& s) override; void onNewSolution(const SolutionBase& s) override;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
protected: protected:
SerialContainer(SerialContainerPrivate* impl); SerialContainer(SerialContainerPrivate* impl);
@ -116,6 +118,7 @@ class ParallelContainerBase : public ContainerBase
public: public:
PRIVATE_CLASS(ParallelContainerBase) PRIVATE_CLASS(ParallelContainerBase)
ParallelContainerBase(const std::string& name = "parallel container"); ParallelContainerBase(const std::string& name = "parallel container");
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
protected: protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl); ParallelContainerBase(ParallelContainerBasePrivate* impl);

View File

@ -144,7 +144,7 @@ public:
void spawn(InterfaceState&& state, const SolutionBasePtr& solution); void spawn(InterfaceState&& state, const SolutionBasePtr& solution);
void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution); void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);
bool storeSolution(const SolutionBasePtr& solution); bool storeSolution(const SolutionBasePtr& solution, const InterfaceState* from, const InterfaceState* to);
void newSolution(const SolutionBasePtr& solution); void newSolution(const SolutionBasePtr& solution);
bool storeFailures() const { return introspection_ != nullptr; } bool storeFailures() const { return introspection_ != nullptr; }
void runCompute() { void runCompute() {

View File

@ -390,6 +390,18 @@ template <>
inline const InterfaceState::Solutions& trajectories<Interface::BACKWARD>(const InterfaceState* state) { inline const InterfaceState::Solutions& trajectories<Interface::BACKWARD>(const InterfaceState* state) {
return state->incomingTrajectories(); return state->incomingTrajectories();
} }
/// Trait to retrieve opposite direction (FORWARD <-> BACKWARD)
template <Interface::Direction dir>
constexpr Interface::Direction opposite();
template <>
inline constexpr Interface::Direction opposite<Interface::FORWARD>() {
return Interface::BACKWARD;
}
template <>
inline constexpr Interface::Direction opposite<Interface::BACKWARD>() {
return Interface::FORWARD;
}
} // namespace task_constructor } // namespace task_constructor
} // namespace moveit } // namespace moveit

View File

@ -122,9 +122,7 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
const InterfaceState* internal_to) { const InterfaceState* internal_to) {
computeCost(*internal_from, *internal_to, *solution); computeCost(*internal_from, *internal_to, *solution);
if (!storeSolution(solution)) // map internal to external states
return;
auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* { auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* {
auto it = internal_to_external_.find(internal); auto it = internal_to_external_.find(internal);
if (it != internal_to_external_.end()) if (it != internal_to_external_.end())
@ -140,6 +138,9 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
InterfaceState* external_from = find_or_create_external(internal_from, created_from); InterfaceState* external_from = find_or_create_external(internal_from, created_from);
InterfaceState* external_to = find_or_create_external(internal_to, created_to); InterfaceState* external_to = find_or_create_external(internal_to, created_to);
if (!storeSolution(solution, external_from, external_to))
return;
// connect solution to start/end state // connect solution to start/end state
solution->setStartState(*external_from); solution->setStartState(*external_from);
solution->setEndState(*external_to); solution->setEndState(*external_to);
@ -368,6 +369,8 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
impl->liftSolution(solution, solution->internalStart(), solution->internalEnd()); impl->liftSolution(solution, solution->internalStart(), solution->internalEnd());
} }
void SerialContainer::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {}
SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(impl) {} SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(impl) {}
SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {} SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {}
@ -602,6 +605,8 @@ ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl)
ParallelContainerBase::ParallelContainerBase(const std::string& name) ParallelContainerBase::ParallelContainerBase(const std::string& name)
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {} : ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
void ParallelContainerBase::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {}
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) { void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)), pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)),
solution.start(), solution.end()); solution.start(), solution.end());

View File

@ -129,13 +129,16 @@ void StagePrivate::validateConnectivity() const {
} }
} }
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { bool StagePrivate::storeSolution(const SolutionBasePtr& solution, const InterfaceState* from,
const InterfaceState* to) {
solution->setCreator(me()); solution->setCreator(me());
if (introspection_) if (introspection_)
introspection_->registerSolution(*solution); introspection_->registerSolution(*solution);
if (solution->isFailure()) { if (solution->isFailure()) {
++num_failures_; ++num_failures_;
if (parent())
parent()->onNewFailure(*me(), from, to);
if (!storeFailures()) if (!storeFailures())
return false; // drop solution return false; // drop solution
failures_.push_back(solution); failures_.push_back(solution);
@ -150,7 +153,7 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to,
computeCost(from, to, *solution); computeCost(from, to, *solution);
if (!storeSolution(solution)) if (!storeSolution(solution, &from, nullptr))
return; // solution dropped return; // solution dropped
me()->forwardProperties(from, to); me()->forwardProperties(from, to);
@ -172,7 +175,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
computeCost(from, to, *solution); computeCost(from, to, *solution);
if (!storeSolution(solution)) if (!storeSolution(solution, nullptr, &to))
return; // solution dropped return; // solution dropped
me()->forwardProperties(to, from); me()->forwardProperties(to, from);
@ -193,7 +196,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution
computeCost(state, state, *solution); computeCost(state, state, *solution);
if (!storeSolution(solution)) if (!storeSolution(solution, nullptr, nullptr))
return; // solution dropped return; // solution dropped
auto from = states_.insert(states_.end(), InterfaceState(state)); // copy auto from = states_.insert(states_.end(), InterfaceState(state)); // copy
@ -213,7 +216,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) { void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
computeCost(from, to, *solution); computeCost(from, to, *solution);
if (!storeSolution(solution)) if (!storeSolution(solution, &from, &to))
return; // solution dropped return; // solution dropped
solution->setStartState(from); solution->setStartState(from);
@ -714,7 +717,9 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
} }
bool ConnectingPrivate::canCompute() const { bool ConnectingPrivate::canCompute() const {
return !pending.empty(); // Do we still have feasible pending state pairs?
return !pending.empty() && pending.front().first->priority().enabled() &&
pending.front().second->priority().enabled();
} }
void ConnectingPrivate::compute() { void ConnectingPrivate::compute() {

View File

@ -127,14 +127,15 @@ Interface::container_type Interface::remove(iterator it) {
} }
void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) { void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) {
if (priority < state->priority()) { // only allow decreasing of priority (smaller is better) if (priority == state->priority())
auto it = std::find(begin(), end(), state); // find iterator to state return; // nothing to do
assert(it != end()); // state should be part of this interface
state->priority_ = priority; // update priority auto it = std::find(begin(), end(), state); // find iterator to state
update(it); assert(it != end()); // state should be part of this interface
if (notify_) state->priority_ = priority; // update priority
notify_(it, true); update(it); // update position in ordered list
} if (notify_)
notify_(it, true); // notify callback
} }
void SolutionBase::setCreator(Stage* creator) { void SolutionBase::setCreator(Stage* creator) {

View File

@ -68,7 +68,7 @@ TEST(Interface, update) {
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 }));
i.updatePriority(*i.begin(), Prio(6, 0, false)); 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({ 3, 6 }));
} }
using PrioPair = std::pair<Prio, Prio>; using PrioPair = std::pair<Prio, Prio>;

View File

@ -172,7 +172,7 @@ TEST(ConnectConnect, Pruning) {
++expected_cost; ++expected_cost;
} }
EXPECT_EQ(c2->calls_, 3u); EXPECT_EQ(c2->calls_, 3u);
// EXPECT_EQ(c1->calls_, 6u); // TODO: avoid compute() calls on failure of remaining part EXPECT_EQ(c1->calls_, 6u); // TODO: avoid compute() calls on failure of remaining part
} }
// https://github.com/ros-planning/moveit_task_constructor/issues/218 // https://github.com/ros-planning/moveit_task_constructor/issues/218