Improve comments for pick-and-place task (moveit#238)

Author: FabianSchuetze

demo/src/pick_place_task.cpp

@@ -144,6 +144,7 @@ void PickPlaceTask::loadParameters() {
 	rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
 }
 
+// Initialize the task pipeline, defining individual movement stages
 bool PickPlaceTask::init() {
 	ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
 	const std::string object = object_name_;
@@ -153,10 +154,13 @@ bool PickPlaceTask::init() {
 	task_.reset();
 	task_.reset(new moveit::task_constructor::Task());
 
+	// Individual movement stages are collected within the Task object
 	Task& t = *task_;
 	t.stages()->setName(task_name_);
 	t.loadRobotModel();
 
+	/* Create planners used in various stages. Various options are available,
+	   namely Cartesian, MoveIt pipeline, and joint interpolation. */
 	// Sampling planner
 	auto sampling_planner = std::make_shared<solvers::PipelinePlanner>();
 	sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
@@ -201,7 +205,7 @@ bool PickPlaceTask::init() {
 	 *                                                  *
 	 ***************************************************/
 	Stage* initial_state_ptr = nullptr;
-	{  // Open Hand
+	{
 		auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
 		stage->setGroup(hand_group_name_);
 		stage->setGoal(hand_open_pose_);
@@ -214,7 +218,8 @@ bool PickPlaceTask::init() {
 	 *               Move to Pick                       *
 	 *                                                  *
 	 ***************************************************/
-	{  // Move-to pre-grasp
+	// Connect initial open-hand state with pre-grasp pose defined in the following
+	{
 		auto stage = std::make_unique<stages::Connect>(
 		    "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
 		stage->setTimeout(5.0);
@@ -229,6 +234,7 @@ bool PickPlaceTask::init() {
 	 ***************************************************/
 	Stage* pick_stage_ptr = nullptr;
 	{
+		// A SerialContainer combines several sub-stages, here for picking the object
 		auto grasp = std::make_unique<SerialContainer>("pick object");
 		t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
 		grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
@@ -237,10 +243,11 @@ bool PickPlaceTask::init() {
   ---- *               Approach Object                    *
 		 ***************************************************/
 		{
+			// Move the eef link forward along its z-axis by an amount within the given min-max range
 			auto stage = std::make_unique<stages::MoveRelative>("approach object", cartesian_planner);
 			stage->properties().set("marker_ns", "approach_object");
-			stage->properties().set("link", hand_frame_);
-			stage->properties().configureInitFrom(Stage::PARENT, { "group" });
+			stage->properties().set("link", hand_frame_);  // link to perform IK for
+			stage->properties().configureInitFrom(Stage::PARENT, { "group" });  // inherit group from parent stage
 			stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_);
 
 			// Set hand forward direction
@@ -255,29 +262,31 @@ bool PickPlaceTask::init() {
   ---- *               Generate Grasp Pose                *
 		 ***************************************************/
 		{
-			// Sample grasp pose
+			// Sample grasp pose candidates in angle increments around the z-axis of the object
 			auto stage = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
 			stage->properties().configureInitFrom(Stage::PARENT);
 			stage->properties().set("marker_ns", "grasp_pose");
 			stage->setPreGraspPose(hand_open_pose_);
-			stage->setObject(object);
+			stage->setObject(object);  // object to sample grasps for
 			stage->setAngleDelta(M_PI / 12);
 			stage->setMonitoredStage(initial_state_ptr);  // hook into successful initial-phase solutions
 
-			// Compute IK
+			// Compute IK for sampled grasp poses
 			auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage));
-			wrapper->setMaxIKSolutions(8);
+			wrapper->setMaxIKSolutions(8);  // limit number of solutions
 			wrapper->setMinSolutionDistance(1.0);
-			wrapper->setIKFrame(grasp_frame_transform_, hand_frame_);
-			wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
-			wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
+			wrapper->setIKFrame(grasp_frame_transform_, hand_frame_);  // define virtual frame to reach the target_pose
+			wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });  // inherit properties from parent
+			wrapper->properties().configureInitFrom(Stage::INTERFACE,
+			                                        { "target_pose" });  // inherit property from child solution
 			grasp->insert(std::move(wrapper));
 		}
 
 		/****************************************************
   ---- *               Allow Collision (hand object)   *
 		 ***************************************************/
 		{
+			// Modify planning scene (w/o altering the robot's pose) to allow touching the object for picking
 			auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (hand,object)");
 			stage->allowCollisions(
 			    object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
@@ -300,7 +309,7 @@ bool PickPlaceTask::init() {
 		 ***************************************************/
 		{
 			auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
-			stage->attachObject(object, hand_frame_);
+			stage->attachObject(object, hand_frame_);  // attach object to hand_frame_
 			grasp->insert(std::move(stage));
 		}
 
@@ -352,6 +361,7 @@ bool PickPlaceTask::init() {
 	 *                                                    *
 	 *****************************************************/
 	{
+		// Connect the grasped state to the pre-place state, i.e. realize the object transport
 		auto stage = std::make_unique<stages::Connect>(
 		    "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
 		stage->setTimeout(5.0);
@@ -364,6 +374,7 @@ bool PickPlaceTask::init() {
 	 *          Place Object                              *
 	 *                                                    *
 	 *****************************************************/
+	// All placing sub-stages are collected within a serial container again
 	{
 		auto place = std::make_unique<SerialContainer>("place object");
 		t.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
@@ -498,13 +509,6 @@ bool PickPlaceTask::execute() {
 	moveit_msgs::MoveItErrorCodes execute_result;
 
 	execute_result = task_->execute(*task_->solutions().front());
-	// // If you want to inspect the goal message, use this instead:
-	// actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>
-	// execute("execute_task_solution", true); execute.waitForServer();
-	// moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal;
-	// task_->solutions().front()->toMsg(execute_goal.solution);
-	// execute.sendGoalAndWait(execute_goal);
-	// execute_result = execute.getResult()->error_code;
 
 	if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) {
 		ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_result.val);