support modifying wrappers in user stages

Possible features that require this interface might be
- trajectory reparameterization,
- trajectory optimization as post-processing (like MoveIt's CHOMP adapter),
- multi-trajectory blending (similar to Pilz' sequence planner)

Also remove invalid interfaces.
Parallel containers must always forward solutions of their child stages,
so simple spawning is not allowed without a child solution.

Author: v4hn

core/include/moveit/task_constructor/container.h

@@ -126,12 +126,11 @@ class ParallelContainerBase : public ContainerBase
 	/// lift child solution to external interface, adapting the costs and comment
 	void liftSolution(const SolutionBase& solution, double cost, std::string comment);
 
-	/// spawn a new solution with given state as start and end
-	void spawn(InterfaceState&& state, SubTrajectory&& trajectory);
-	/// propagate a solution forwards
-	void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory);
-	/// propagate a solution backwards
-	void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& trajectory);
+	/// lift a modified solution based on the solution of a child stage
+	void liftModifiedSolution(SolutionBasePtr&& new_solution, const SolutionBase& child_solution);
+	/// lift a modified solution, changing the (single!) new associated start or end InterfaceState
+	void liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_propagated_state,
+	                          const SolutionBase& child_solution);
 };
 
 /** Plan for different alternatives in parallel.

core/include/moveit/task_constructor/container_p.h

@@ -150,9 +150,9 @@ class ContainerBasePrivate : public StagePrivate
 	/// copy external_state to a child's interface and remember the link in internal_external map
 	template <Interface::Direction>
 	void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
-	/// lift solution from internal to external level
+	/// lift solution from internal to external level, possibly replacing the generated InterfaceState with new_external
 	void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
-	                  const InterfaceState* internal_to);
+	                  const InterfaceState* internal_to, const InterfaceState* new_external = nullptr);
 
 	/// protected writable overloads
 	inline auto& internalToExternalMap() { return internal_external_.left; }

core/src/container.cpp

@@ -203,37 +203,45 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
 }
 
 void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
-                                        const InterfaceState* internal_to) {
-	computeCost(*internal_from, *internal_to, *solution);
+                                        const InterfaceState* internal_to, const InterfaceState* new_external) {
+	const bool create_from{ requiredInterface().testFlag(WRITES_PREV_END) };
+	const bool create_to{ requiredInterface().testFlag(WRITES_NEXT_START) };
 
-	// map internal to external states
-	auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* {
-		auto it = internalToExternalMap().find(internal);
-		if (it != internalToExternalMap().end())
-			return const_cast<InterfaceState*>(it->second);
+	// external states, nullptr if they don't exist yet
+	const InterfaceState* external_from{ create_from ? new_external : internalToExternalMap().at(internal_from) };
+	const InterfaceState* external_to{ create_to ? new_external : internalToExternalMap().at(internal_to) };
 
-		InterfaceState* external = &*states_.insert(states_.end(), InterfaceState(*internal));
+	// computeCost
+	computeCost(external_from ? *external_from : InterfaceState(*internal_from),
+	            external_to ? *external_to : InterfaceState(*internal_to), *solution);
+
+	// storeSolution
+	if (!storeSolution(solution, external_from, external_to)) {
+		return;
+	}
+
+	auto create_state = [this, new_external](const InterfaceState& internal) {
+		InterfaceState* external{ &*states_.insert(states_.cend(), new_external ? *new_external : internal) };
 		internalToExternalMap().insert(std::make_pair(internal, external));
-		created = true;
 		return external;
 	};
-	bool created_from = false;
-	bool created_to = false;
-	InterfaceState* external_from = find_or_create_external(internal_from, created_from);
-	InterfaceState* external_to = find_or_create_external(internal_to, created_to);
 
-	if (!storeSolution(solution, external_from, external_to))
-		return;
+	if (create_from)
+		external_from = create_state(*internal_from);
+	if (create_to)
+		external_to = create_state(*internal_to);
 
-	// connect solution to start/end state
+	// connect solution to states
 	solution->setStartState(*external_from);
 	solution->setEndState(*external_to);
 
-	// spawn created states in external interfaces
-	if (created_from)
-		prevEnds()->add(*external_from);
-	if (created_to)
-		nextStarts()->add(*external_to);
+	// spawn new external states
+	if (!solution->isFailure()) {
+		if (create_from)
+			prevEnds()->add(*const_cast<InterfaceState*>(external_from));
+		if (create_to)
+			nextStarts()->add(*const_cast<InterfaceState*>(external_to));
+	}
 
 	newSolution(solution);
 }
@@ -737,16 +745,20 @@ void ParallelContainerBase::liftSolution(const SolutionBase& solution, double co
 	                      solution.start(), solution.end());
 }
 
-void ParallelContainerBase::spawn(InterfaceState&& state, SubTrajectory&& t) {
-	pimpl()->StagePrivate::spawn(std::move(state), std::make_shared<SubTrajectory>(std::move(t)));
-}
+void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& modified_solution, const SolutionBase &child_solution) {
+	// child_solution is correctly prepared by a child of this container
+	assert(child_solution.creator());
+	assert(child_solution.creator()->parent() == this);
 
-void ParallelContainerBase::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
-	pimpl()->StagePrivate::sendForward(from, std::move(to), std::make_shared<SubTrajectory>(std::move(t)));
+	pimpl()->liftSolution(std::move(modified_solution),
+	                      child_solution.start(), child_solution.end());
 }
 
-void ParallelContainerBase::sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) {
-	pimpl()->StagePrivate::sendBackward(std::move(from), to, std::make_shared<SubTrajectory>(std::move(t)));
+void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr &&new_solution, InterfaceState &&new_propagated_state, const SolutionBase &child_solution) {
+	assert(child_solution.creator());
+	assert(child_solution.creator()->parent() == this);
+
+	pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_propagated_state);
 }
 
 WrapperBasePrivate::WrapperBasePrivate(WrapperBase* me, const std::string& name)

core/src/stages/compute_ik.cpp

@@ -330,13 +330,13 @@ void ComputeIK::compute() {
 	                                     ->getParentJointModel()
 	                                     ->getDescendantLinkModels();
 	if (colliding) {
-		SubTrajectory solution;
+		auto solution = std::make_shared<SubTrajectory>();
 		generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
-		std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
-		solution.markAsFailure();
+		std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution->markers()));
+		solution->markAsFailure();
 		// TODO: visualize collisions
-		solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
-		spawn(InterfaceState(sandbox_scene), std::move(solution));
+		solution->setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
+		liftModifiedSolution(solution, InterfaceState(sandbox_scene), s);
 		return;
 	} else
 		generateVisualMarkers(sandbox_state, appender, links_to_visualize);
@@ -389,18 +389,18 @@ void ComputeIK::compute() {
 		for (size_t i = previous; i != ik_solutions.size(); ++i) {
 			// create a new scene for each solution as they will have different robot states
 			planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
-			SubTrajectory solution;
-			solution.setComment(s.comment());
+			auto solution = std::make_shared<SubTrajectory>();
+			solution->setComment(s.comment());
 
 			// frames at target pose and ik frame
-			rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
-			rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
+			rviz_marker_tools::appendFrame(solution->markers(), target_pose_msg, 0.1, "ik frame");
+			rviz_marker_tools::appendFrame(solution->markers(), ik_pose_msg, 0.1, "ik frame");
 
 			if (succeeded && i + 1 == ik_solutions.size())
 				// compute cost as distance to compare_pose
-				solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
+				solution->setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
 			else  // found an IK solution, but this was not valid
-				solution.markAsFailure();
+				solution->markAsFailure();
 
 			// set scene's robot state
 			robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
@@ -409,7 +409,7 @@ void ComputeIK::compute() {
 
 			InterfaceState state(scene);
 			forwardProperties(*s.start(), state);
-			spawn(std::move(state), std::move(solution));
+			liftModifiedSolution(solution, std::move(state), s);
 		}
 
 		// TODO: magic constant should be a property instead ("current_seed_only", or equivalent)
@@ -421,15 +421,15 @@ void ComputeIK::compute() {
 
 	if (ik_solutions.empty()) {  // failed to find any solution
 		planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
-		SubTrajectory solution;
+		auto solution = std::make_shared<SubTrajectory>();
 
-		solution.markAsFailure();
-		solution.setComment(s.comment() + " no IK found");
+		solution->markAsFailure();
+		solution->setComment(s.comment() + " no IK found");
 
 		// ik target link placement
-		std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
+		std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution->markers()));
 
-		spawn(InterfaceState(scene), std::move(solution));
+		liftModifiedSolution(solution, InterfaceState(scene), s);
 	}
 }
 }  // namespace stages