cleanup / renaming

* Rename pruneInterface() -> resolveInterface()
* Rename accepted (interface) -> expected
* Improve exception strings

Author: rhaschke

core/include/moveit/task_constructor/container_p.h

@@ -106,7 +106,7 @@ class ContainerBasePrivate : public StagePrivate
 	void validateConnectivity() const override;
 
 	// Containers derive their required interface from their children
-	// UNKNOWN until pruneInterface was called
+	// UNKNOWN until resolveInterface was called
 	InterfaceFlags requiredInterface() const override { return required_interface_; }
 
 	// forward these methods to the public interface for containers
@@ -139,7 +139,7 @@ class ContainerBasePrivate : public StagePrivate
 	auto& internalToExternalMap() { return internal_to_external_; }
 	const auto& internalToExternalMap() const { return internal_to_external_; }
 
-	// set in pruneInterface()
+	// set in resolveInterface()
 	InterfaceFlags required_interface_;
 
 private:
@@ -170,7 +170,7 @@ class SerialContainerPrivate : public ContainerBasePrivate
 	SerialContainerPrivate(SerialContainer* me, const std::string& name);
 
 	// called by parent asking for pruning of this' interface
-	void pruneInterface(InterfaceFlags accepted) override;
+	void resolveInterface(InterfaceFlags expected) override;
 	// validate connectivity of chain
 	void validateConnectivity() const override;
 
@@ -215,7 +215,7 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
 	ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name);
 
 	// called by parent asking for pruning of this' interface
-	void pruneInterface(InterfaceFlags accepted) override;
+	void resolveInterface(InterfaceFlags expected) override;
 
 	void validateConnectivity() const override;
 
@@ -251,7 +251,7 @@ class MergerPrivate : public ParallelContainerBasePrivate
 	typedef std::function<void(SubTrajectory&&)> Spawner;
 	MergerPrivate(Merger* me, const std::string& name);
 
-	void pruneInterface(InterfaceFlags accepted) override;
+	void resolveInterface(InterfaceFlags expected) override;
 
 	void onNewPropagateSolution(const SolutionBase& s);
 	void onNewGeneratorSolution(const SolutionBase& s);

core/include/moveit/task_constructor/stage_p.h

@@ -71,15 +71,11 @@ class StagePrivate
 	/// actually configured interface of this stage (only valid after init())
 	InterfaceFlags interfaceFlags() const;
 
-	/** Interface required by this stage
-	 *
-	 * If the interface is unknown (because it is auto-detected from context), return UNKNOWN */
+	/// Retrieve interface required by this stage, UNKNOWN if auto-detected from context
 	virtual InterfaceFlags requiredInterface() const = 0;
 
-	/** Prune interface to comply with the given propagation direction
-	 *
-	 * PropagatingEitherWay uses this in restrictDirection() */
-	virtual void pruneInterface(InterfaceFlags /* accepted */) {}
+	/// Resolve interface/propagation direction to comply with the given external interface
+	virtual void resolveInterface(InterfaceFlags /* expected */) {}
 
 	/// validate connectivity of children (after init() was done)
 	virtual void validateConnectivity() const;
@@ -220,8 +216,8 @@ class PropagatingEitherWayPrivate : public ComputeBasePrivate
 	InterfaceFlags requiredInterface() const override;
 	// initialize pull interfaces for given propagation directions
 	void initInterface(PropagatingEitherWay::Direction dir);
-	// prune interface to the given propagation direction
-	void pruneInterface(InterfaceFlags accepted) override;
+	// resolve interface to the given propagation direction
+	void resolveInterface(InterfaceFlags expected) override;
 
 	bool canCompute() const override;
 	void compute() override;

core/src/container.cpp

@@ -422,23 +422,23 @@ void SerialContainerPrivate::validateInterface(const StagePrivate& child, Interf
 }
 
 // called by parent asking for pruning of this' interface
-void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
+void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
 	// we need to have some children to do the actual work
 	if (children().empty())
 		throw InitStageException(*me(), "no children");
 
-	if (!(accepted & START_IF_MASK))
+	if (!(expected & START_IF_MASK))
 		throw InitStageException(*me(), "unknown start interface");
 
 	Stage& first = *children().front();
 	Stage& last = *children().back();
 
 	// FIRST child
-	first.pimpl()->pruneInterface(accepted & START_IF_MASK);
+	first.pimpl()->resolveInterface(expected & START_IF_MASK);
 	// connect first child's (start) push interface
 	setChildsPushBackwardInterface(first.pimpl());
 	// validate that first child's and this container's start interfaces match
-	validateInterface<START_IF_MASK>(*first.pimpl(), accepted);
+	validateInterface<START_IF_MASK>(*first.pimpl(), expected);
 	// connect first child's (start) pull interface
 	if (const InterfacePtr& target = first.pimpl()->starts())
 		starts_.reset(new Interface(
@@ -448,14 +448,14 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
 	for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) {
 		StagePrivate* child_impl = (**it).pimpl();
 		StagePrivate* previous_impl = (**previous_it).pimpl();
-		child_impl->pruneInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
+		child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
 		connect(*previous_impl, *child_impl);
 	}
 
 	// connect last child's (end) push interface
 	setChildsPushForwardInterface(last.pimpl());
 	// validate that last child's and this container's end interfaces match
-	validateInterface<END_IF_MASK>(*last.pimpl(), accepted);
+	validateInterface<END_IF_MASK>(*last.pimpl(), expected);
 	// connect last child's (end) pull interface
 	if (const InterfacePtr& target = last.pimpl()->ends())
 		ends_.reset(new Interface(
@@ -552,7 +552,7 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti
 ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
   : ContainerBasePrivate(me, name) {}
 
-void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
+void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
 	// we need to have some children to do the actual work
 	if (children().empty())
 		throw InitStageException(*me(), "no children");
@@ -563,8 +563,8 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
 	for (const Stage::pointer& child : children()) {
 		try {
 			auto child_impl = child->pimpl();
-			child_impl->pruneInterface(accepted);
-			validateInterfaces(*child_impl, accepted, first);
+			child_impl->resolveInterface(expected);
+			validateInterfaces(*child_impl, expected, first);
 			// initialize push connections of children according to their demands
 			setChildsPushForwardInterface(child_impl);
 			setChildsPushBackwardInterface(child_impl);
@@ -579,16 +579,16 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
 		throw exceptions;
 
 	// States received by the container need to be copied to all children's pull interfaces.
-	if (accepted & READS_START)
+	if (expected & READS_START)
 		starts().reset(new Interface([this](Interface::iterator external, bool updated) {
 			this->onNewExternalState(Interface::FORWARD, external, updated);
 		}));
-	if (accepted & READS_END)
+	if (expected & READS_END)
 		ends().reset(new Interface([this](Interface::iterator external, bool updated) {
 			this->onNewExternalState(Interface::BACKWARD, external, updated);
 		}));
 
-	required_interface_ = accepted;
+	required_interface_ = expected;
 }
 
 void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external,
@@ -746,8 +746,8 @@ void Fallbacks::onNewSolution(const SolutionBase& s) {
 
 MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
 
-void MergerPrivate::pruneInterface(InterfaceFlags accepted) {
-	ContainerBasePrivate::pruneInterface(accepted);
+void MergerPrivate::resolveInterface(InterfaceFlags expected) {
+	ContainerBasePrivate::resolveInterface(expected);
 	switch (interfaceFlags()) {
 		case PROPAGATE_FORWARDS:
 		case PROPAGATE_BACKWARDS:

core/src/stage.cpp

@@ -117,7 +117,7 @@ void StagePrivate::validateConnectivity() const {
 	InterfaceFlags required = requiredInterface();
 	InterfaceFlags actual = interfaceFlags();
 	if ((required & actual) != required) {
-		boost::format desc("interface %1% %2% does not satisfy required interface %3% %4%");
+		boost::format desc("actual interface %1% %2% does not match required interface %3% %4%");
 		desc % flowSymbol<START_IF_MASK>(actual) % flowSymbol<END_IF_MASK>(actual);
 		desc % flowSymbol<START_IF_MASK>(required) % flowSymbol<END_IF_MASK>(required);
 		throw InitStageException(*me(), desc.str());
@@ -420,24 +420,24 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction
 	}
 }
 
-void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
-	if (accepted == UNKNOWN)
+void PropagatingEitherWayPrivate::resolveInterface(InterfaceFlags expected) {
+	if (expected == UNKNOWN)
 		throw InitStageException(*me(), "cannot initialize to unknown interface");
 
 	auto dir = PropagatingEitherWay::AUTO;
-	if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START)
+	if ((expected & START_IF_MASK) == READS_START || (expected & END_IF_MASK) == WRITES_NEXT_START)
 		dir = PropagatingEitherWay::FORWARD;
-	else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END)
+	else if ((expected & START_IF_MASK) == WRITES_PREV_END || (expected & END_IF_MASK) == READS_END)
 		dir = PropagatingEitherWay::BACKWARD;
 	else {
-		boost::format desc("propagator cannot handle external interface %1% %2%");
-		desc % flowSymbol<START_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(accepted);
+		boost::format desc("propagator cannot satisfy expected interface %1% %2%");
+		desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
 		throw InitStageException(*me(), desc.str());
 	}
 	if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) {
-		boost::format desc("configured interface (%1% %2%) does not match external one (%3% %4%)");
+		boost::format desc("configured interface (%1% %2%) does not match expected one (%3% %4%)");
 		desc % flowSymbol<START_IF_MASK>(required_interface_) % flowSymbol<END_IF_MASK>(required_interface_);
-		desc % flowSymbol<START_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(accepted);
+		desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
 		throw InitStageException(*me(), desc.str());
 	}
 	initInterface(dir);

core/src/task.cpp

@@ -261,7 +261,7 @@ void Task::init() {
 	// and *afterwards* initialize all children recursively
 	stages()->init(impl->robot_model_);
 	// task expects its wrapped child to push to both ends, this triggers interface resolution
-	stages()->pimpl()->pruneInterface(InterfaceFlags({ GENERATE }));
+	stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE }));
 
 	// provide introspection instance to all stages
 	impl->setIntrospection(impl->introspection_.get());

core/test/test_container.cpp

@@ -200,19 +200,19 @@ class InitTest : public ::testing::Test
 		append(container, types);
 		try {
 			container.init(robot_model);
-			// prune interfaces based on provided external interface (start, end)
-			InterfaceFlags accepted;
+			// resolve interfaces based on provided external interface (start, end)
+			InterfaceFlags expected;
 			if (start)
-				accepted |= WRITES_PREV_END;
+				expected |= WRITES_PREV_END;
 			else
-				accepted |= READS_START;
+				expected |= READS_START;
 
 			if (end)
-				accepted |= WRITES_NEXT_START;
+				expected |= WRITES_NEXT_START;
 			else
-				accepted |= READS_END;
+				expected |= READS_END;
 
-			container.pimpl()->pruneInterface(accepted);
+			container.pimpl()->resolveInterface(expected);
 			container.pimpl()->validateConnectivity();
 			if (!expect_failure)
 				return;  // as expected
@@ -456,9 +456,9 @@ TEST_F(SerialTest, interface_detection) {
 	EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
 
 	// derive propagation direction from outer interface
-	EXPECT_INIT_SUCCESS(false, true, ANY);  // should be pruned to FW
+	EXPECT_INIT_SUCCESS(false, true, ANY);  // should be resolved to FW
 	EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
-	EXPECT_INIT_SUCCESS(true, false, ANY);  // should be pruned to BW
+	EXPECT_INIT_SUCCESS(true, false, ANY);  // should be resolved to BW
 	EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
 
 	EXPECT_INIT_SUCCESS(false, true, ANY, ANY);  // -> ->

demo/scripts/grasp.py

@@ -0,0 +1,46 @@
+#! /usr/bin/env python
+# -*- coding: utf-8 -*-
+
+from std_msgs.msg import Header
+from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3, PoseStamped, Pose, Point, Quaternion
+from moveit.task_constructor import core, stages
+import moveit_commander
+import rospy
+import numpy
+
+from moveit.python_tools import roscpp_init
+roscpp_init("mtc_tutorial")
+rospy.init_node('mtc_tutorial_py', anonymous=False)
+
+def create_object():
+    scene = moveit_commander.PlanningSceneInterface(synchronous=True)
+    scene.remove_world_object();
+    pose = PoseStamped()
+    pose.header.frame_id = "world"
+    pose.pose.position.x = 0.5
+    pose.pose.position.y = numpy.random.uniform(-0.1, 0.1)
+    pose.pose.position.z = 0.16
+    pose.pose.orientation.w = 1.0
+    scene.add_box('object', pose, size=(0.05, 0.05, 0.2))
+
+group = "panda_arm"
+eef_frame = "panda_link8"
+
+# Cartesian interpolation planner
+cartesian = core.CartesianPath()
+
+task = core.Task()
+
+# start from current robot state
+task.add(stages.CurrentState("current state"))
+
+move = stages.MoveTo("to object", cartesian)
+move.group = group
+header = Header(frame_id = "object")
+move.setGoal(PoseStamped(header=header, pose=Pose(position=Point(0,0,0.18),orientation=Quaternion(0.92388, -0.38268, 0, 0))))
+task.add(move)
+
+create_object()
+if task.plan():
+    task.publish(task.solutions[0])
+rospy.spin()

demo/scripts/pick.py

@@ -0,0 +1,65 @@
+#! /usr/bin/env python
+# -*- coding: utf-8 -*-
+
+from std_msgs.msg import Header
+from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3, PoseStamped, Pose, Point, Quaternion
+from moveit.task_constructor import core, stages
+import moveit_commander
+import rospy
+import numpy
+
+from moveit.python_tools import roscpp_init
+roscpp_init("mtc_tutorial")
+rospy.init_node('mtc_tutorial_py', anonymous=False)
+
+group = 'panda_arm'
+eef = 'panda_hand'
+eef_frame = "panda_link8"
+
+sampling_planner = core.JointInterpolationPlanner()
+cartesian_planner = core.CartesianPath()
+
+task = core.Task()
+
+task.properties.update({'group': group, 'eef': eef, 'hand': eef, 'hand_grasping_frame': eef, 'ik_frame': eef_frame})
+
+currstate = stages.CurrentState('current state')
+#task.add(currstate)  # Adding it to the task results in error for argument types in setMonitoredStage in GenerateGraspPose
+
+open_hand = stages.MoveTo("open hand", sampling_planner)
+open_hand.group = eef
+open_hand.setGoal('open')
+task.add(open_hand)
+
+connect = stages.Connect('move to pick', [(group, sampling_planner)])
+connect.timeout = 5
+connect.properties.configureInitFrom(core.PARENT)
+task.add(connect)
+
+grasp = core.SerialContainer('pick object')
+task.properties.exposeTo(grasp.properties, ['eef', 'hand', 'group', 'ik_frame'])
+grasp.properties.configureInitFrom(core.PARENT, ['eef', 'hand', 'group', 'ik_frame'])
+
+approach_object = stages.MoveRelative("approach_object", cartesian_planner)
+approach_object.properties.update({'marker_ns': 'approach_object', 'link': eef_frame})
+approach_object.properties.configureInitFrom(core.PARENT, ['group'])
+approach_object.min_distance = 0.01
+approach_object.max_distance = 0.1
+print(approach_object.properties.__getitem__('group'))   # Why is this None? how to get properties from within SerialContainer?
+approach_object.setDirection(TwistStamped(header=Header(frame_id = eef_frame), twist=Twist(linear=Vector3(0,0,0.1))))
+grasp.insert(approach_object)
+
+generatepose = stages.GenerateGraspPose('generate grasp pose')
+generatepose.properties.configureInitFrom(core.PARENT)
+generatepose.properties.update({'marker_ns': 'grasp_pose'})
+generatepose.pregrasp = 'open'
+generatepose.object = 'base'
+generatepose.angle_delta = numpy.pi/12
+generatepose.setMonitoredStage(currstate)
+
+# To be continued
+task.add(grasp)
+
+if task.plan():
+    task.publish(task.solutions[0])
+rospy.spin()