Rework Pruning (#221)

Author: rhaschke

core/include/moveit/task_constructor/container.h

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

core/include/moveit/task_constructor/stage_p.h

@@ -144,7 +144,7 @@ class StagePrivate
 	void spawn(InterfaceState&& state, 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);
 	bool storeFailures() const { return introspection_ != nullptr; }
 	void runCompute() {
@@ -157,6 +157,10 @@ class StagePrivate
 	/** 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_;
@@ -294,31 +298,53 @@ class ConnectingPrivate : public ComputeBasePrivate
 {
 	friend class Connecting;
 
-	using StatePair = std::pair<Interface::const_iterator, Interface::const_iterator>;
-	struct StatePairLess
+public:
+	struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>
 	{
-		bool operator()(const StatePair& x, const StatePair& y) const {
-			return x.first->priority() + x.second->priority() < y.first->priority() + y.second->priority();
+		using std::pair<Interface::const_iterator, Interface::const_iterator>::pair;  // inherit base constructors
+		bool operator<(const StatePair& rhs) const {
+			return less(first->priority(), second->priority(), rhs.first->priority(), rhs.second->priority());
+		}
+		static inline bool less(const InterfaceState::Priority& lhsA, const InterfaceState::Priority& lhsB,
+		                        const InterfaceState::Priority& rhsA, const InterfaceState::Priority& rhsB) {
+			unsigned char lhs = (lhsA.enabled() << 1) | lhsB.enabled();  // combine bits into two-digit binary number
+			unsigned char rhs = (rhsA.enabled() << 1) | rhsB.enabled();
+			if (lhs == rhs)  // if enabled status is identical
+				return lhsA + lhsB < rhsA + rhsB;  // compare the sums of both contributions
+			// one of the states in each pair should be enabled
+			assert(lhs != 0b00 && rhs != 0b00);
+			// both states valid (b11)
+			if (lhs == 0b11)
+				return true;
+			if (rhs == 0b11)
+				return false;
+			return lhs < rhs;  // disabled states in 1st component go before disabled states in 2nd component
 		}
 	};
 
-	template <Interface::Direction other>
-	inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second);
-
-public:
 	inline ConnectingPrivate(Connecting* me, const std::string& name);
 
 	InterfaceFlags requiredInterface() const override;
 	bool canCompute() const override;
 	void compute() override;
 
+	// Check whether there are pending feasible states that could connect to source
+	template <Interface::Direction dir>
+	bool hasPendingOpposites(const InterfaceState* source) const;
+
+	std::ostream& printPendingPairs(std::ostream& os = std::cerr) const;
+
 private:
+	// Create a pair of Interface states for pending list, such that the order (start, end) is maintained
+	template <Interface::Direction other>
+	inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second);
+
 	// get informed when new start or end state becomes available
 	template <Interface::Direction other>
 	void newState(Interface::iterator it, bool updated);
 
 	// ordered list of pending state pairs
-	ordered<StatePair, StatePairLess> pending;
+	ordered<StatePair> pending;
 };
 PIMPL_FUNCTIONS(Connecting)
 }  // namespace task_constructor

core/include/moveit/task_constructor/storage.h

@@ -76,23 +76,42 @@ class InterfaceState
 {
 	friend class SolutionBase;  // addIncoming() / addOutgoing() should be called only by SolutionBase
 	friend class Interface;  // allow Interface to set owner_ and priority_
+	friend class StagePrivate;
+
 public:
+	enum Status
+	{
+		ENABLED,  // state is actively considered during planning
+		DISABLED,  // state is disabled because a required connected state failed
+		DISABLED_FAILED,  // state that failed, causing the whole partial solution to be disabled
+	};
 	/** 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 : public std::pair<unsigned int, double>
+	struct Priority : std::tuple<Status, unsigned int, double>
 	{
-		Priority(unsigned int depth, double cost) : std::pair<unsigned int, double>(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 its status
+		Priority(const Priority& other, Status status) : Priority(other.depth(), other.cost(), status) {}
 
-		inline unsigned int depth() const { return this->first; }
-		inline double cost() const { return this->second; }
+		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(this->depth() + other.depth(), this->cost() + other.cost());
+			return Priority(depth() + other.depth(), cost() + other.cost(), std::min(status(), other.status()));
 		}
-		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*>;
 
@@ -147,19 +166,20 @@ class Interface : public ordered<InterfaceState*>
 	class iterator : public base_type::iterator
 	{
 	public:
-		using base_type::iterator::iterator;  // inherit base constructors
 		iterator(base_type::iterator other) : base_type::iterator(other) {}
 
 		InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); }
+
 		InterfaceState* operator->() const noexcept { return base_type::iterator::operator*(); }
 	};
 	class const_iterator : public base_type::const_iterator
 	{
 	public:
-		using base_type::const_iterator::const_iterator;  // inherit base constructors
 		const_iterator(base_type::const_iterator other) : base_type::const_iterator(other) {}
+		const_iterator(base_type::iterator other) : base_type::const_iterator(other) {}
 
 		const InterfaceState& operator*() const noexcept { return *base_type::const_iterator::operator*(); }
+
 		const InterfaceState* operator->() const noexcept { return base_type::const_iterator::operator*(); }
 	};
 
@@ -194,6 +214,9 @@ class Interface : public ordered<InterfaceState*>
 	using base_type::remove_if;
 };
 
+std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio);
+std::ostream& operator<<(std::ostream& os, const Interface& interface);
+
 class CostTerm;
 class StagePrivate;
 class ContainerBasePrivate;
@@ -370,14 +393,26 @@ inline const InterfaceState* state<Interface::BACKWARD>(const SolutionBase& solu
 
 /// Trait to retrieve outgoing (FORWARD) or incoming (BACKWARD) solution segments of a given state
 template <Interface::Direction dir>
-const InterfaceState::Solutions& trajectories(const InterfaceState* state);
+const InterfaceState::Solutions& trajectories(const InterfaceState& state);
+template <>
+inline const InterfaceState::Solutions& trajectories<Interface::FORWARD>(const InterfaceState& state) {
+	return state.outgoingTrajectories();
+}
+template <>
+inline const InterfaceState::Solutions& trajectories<Interface::BACKWARD>(const InterfaceState& state) {
+	return state.incomingTrajectories();
+}
+
+/// Trait to retrieve opposite direction (FORWARD <-> BACKWARD)
+template <Interface::Direction dir>
+constexpr Interface::Direction opposite();
 template <>
-inline const InterfaceState::Solutions& trajectories<Interface::FORWARD>(const InterfaceState* state) {
-	return state->outgoingTrajectories();
+inline constexpr Interface::Direction opposite<Interface::FORWARD>() {
+	return Interface::BACKWARD;
 }
 template <>
-inline const InterfaceState::Solutions& trajectories<Interface::BACKWARD>(const InterfaceState* state) {
-	return state->incomingTrajectories();
+inline constexpr Interface::Direction opposite<Interface::BACKWARD>() {
+	return Interface::FORWARD;
 }
 }  // namespace task_constructor
 }  // namespace moveit