From db74cd95f9803f6a4df686e05dab50fcae0e4d6b Mon Sep 17 00:00:00 2001
From: mrceki <cenkcetin123@hotmail.com.tr>
Date: Tue, 28 Feb 2023 17:05:14 +0300
Subject: [PATCH] =?UTF-8?q?build=20etmekten=20s=C4=B1k=C4=B1ld=C4=B1m=20d?=
 =?UTF-8?q?=C3=BCnyay=C4=B1=20python=20kurtaracak?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 air_moveit_config/scripts/pickplace.py  | 183 +++++++++
 air_moveit_config/src/pick_ball_MTC.cpp | 511 ++++++++++++++++++++++++
 2 files changed, 694 insertions(+)
 create mode 100755 air_moveit_config/scripts/pickplace.py
 create mode 100644 air_moveit_config/src/pick_ball_MTC.cpp

diff --git a/air_moveit_config/scripts/pickplace.py b/air_moveit_config/scripts/pickplace.py
new file mode 100755
index 0000000..f35faf9
--- /dev/null
+++ b/air_moveit_config/scripts/pickplace.py
@@ -0,0 +1,183 @@
+#! /usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+from moveit_commander.roscpp_initializer import roscpp_initialize
+from moveit.task_constructor import core, stages
+from moveit_commander import PlanningSceneInterface
+from geometry_msgs.msg import PoseStamped, TwistStamped
+import time
+import rospy
+
+
+roscpp_initialize("pickplace")
+rospy.loginfo("Z<OERJGHSDFKJSDFJKASDJKLFASJKLDFJKLASDFJKLKASJDFJKLASDLF")
+# [pickAndPlaceTut1]
+# Specify robot parameters
+arm = "air"
+eef = "gripper"
+# [pickAndPlaceTut1]
+
+# [pickAndPlaceTut2]
+# Specify object parameters
+object_name = "grasp_object"
+object_radius = 0.02
+
+# Start with a clear planning scene
+psi = PlanningSceneInterface(synchronous=True)
+psi.remove_world_object()
+
+# [initCollisionObject]
+# Grasp object properties
+objectPose = PoseStamped()
+objectPose.header.frame_id = "world"
+objectPose.pose.orientation.x = 1.0
+objectPose.pose.position.x = 0.55
+objectPose.pose.position.y = -0.25
+objectPose.pose.position.z = 0.7
+# [initCollisionObject]
+
+# Add the grasp object to the planning scene
+psi.add_cylinder(object_name, objectPose, height=0.25, radius=0.02)
+# [pickAndPlaceTut2]
+
+# [pickAndPlaceTut3]
+# Create a task
+task = core.Task("PandaPickPipelineExample")
+task.enableIntrospection()
+# [pickAndPlaceTut3]
+
+# [pickAndPlaceTut4]
+# Start with the current state
+task.add(stages.CurrentState("current"))
+
+# [initAndConfigConnect]
+# Create a planner instance that is used to connect
+# the current state to the grasp approach pose
+pipeline = core.PipelinePlanner()
+pipeline.planner = "RRTConnectkConfigDefault"
+planners = [(arm, pipeline)]
+
+# Connect the two stages
+task.add(stages.Connect("connect1", planners))
+# [initAndConfigConnect]
+# [pickAndPlaceTut4]
+
+# [pickAndPlaceTut5]
+# [initAndConfigGenerateGraspPose]
+# The grasp generator spawns a set of possible grasp poses around the object
+grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose")
+grasp_generator.angle_delta = 0.3
+grasp_generator.pregrasp = "open"
+grasp_generator.grasp = "closed"
+grasp_generator.setMonitoredStage(task["current"])  # Generate solutions for all initial states
+# [initAndConfigGenerateGraspPose]
+# [pickAndPlaceTut5]
+
+# [pickAndPlaceTut6]
+# [initAndConfigSimpleGrasp]
+# SimpleGrasp container encapsulates IK calculation of arm pose as well as finger closing
+simpleGrasp = stages.SimpleGrasp(grasp_generator, "Grasp")
+# Set frame for IK calculation in the center between the fingers
+ik_frame = PoseStamped()
+ik_frame.header.frame_id = "qbhand_base_link"
+ik_frame.pose.position.y = 0.04
+ik_frame.pose.position.z = 0.13
+ik_frame.pose.orientation.w= 1
+ik_frame.pose.orientation.x=-0.5646338
+ik_frame.pose.orientation.y=-0.5646338
+ik_frame.pose.orientation.z=0.5646338
+simpleGrasp.setIKFrame(ik_frame)
+# [initAndConfigSimpleGrasp]
+# [pickAndPlaceTut6]
+
+# [pickAndPlaceTut7]
+# [initAndConfigPick]
+# Pick container comprises approaching, grasping (using SimpleGrasp stage), and lifting of object
+pick = stages.Pick(simpleGrasp, "Pick")
+pick.eef = eef
+pick.object = object_name
+
+# Twist to approach the object
+approach = TwistStamped()
+approach.header.frame_id = "world"
+approach.twist.linear.z = -1.0
+pick.setApproachMotion(approach, 0.00, 0.25) # (approach, 0.03, 0.1)
+
+# Twist to lift the object
+lift = TwistStamped()
+lift.header.frame_id = "qbhand_base_link"
+lift.twist.linear.z = - 1.0
+pick.setLiftMotion(lift, 0.00, 0.25) # (lift, 0.03, 0.1)
+# [pickAndPlaceTut7]
+
+# [pickAndPlaceTut8]
+# Add the pick stage to the task's stage hierarchy
+task.add(pick)
+# [initAndConfigPick]
+# [pickAndPlaceTut8]
+
+# [pickAndPlaceTut9]
+# Connect the Pick stage with the following Place stage
+task.add(stages.Connect("connect2", planners))
+# [pickAndPlaceTut9]
+
+# [pickAndPlaceTut10]
+# [initAndConfigGeneratePlacePose]
+# Define the pose that the object should have after placing
+placePose = objectPose
+placePose.pose.orientation.x = 1
+placePose.pose.position.x += -0.1
+placePose.pose.position.y += 0.15  # shift object by 20cm along y axis
+
+# Generate Cartesian place poses for the object
+place_generator = stages.GeneratePlacePose("Generate Place Pose")
+place_generator.setMonitoredStage(task["Pick"])
+place_generator.object = object_name
+place_generator.pose = placePose
+# [initAndConfigGeneratePlacePose]
+# [pickAndPlaceTut10]
+
+# [initAndConfigSimpleUnGrasp]
+# The SimpleUnGrasp container encapsulates releasing the object at the given Cartesian pose
+# [pickAndPlaceTut11]
+simpleUnGrasp = stages.SimpleUnGrasp(place_generator, "UnGrasp")
+# [pickAndPlaceTut11]
+
+# [pickAndPlaceTut12]
+# [initAndConfigPlace]
+# Place container comprises placing, ungrasping, and retracting
+place = stages.Place(simpleUnGrasp, "Place")
+place.eef = eef
+place.object = object_name
+place.eef_frame = "qbhand_base_link"
+#place.eef_frame = "panda_link8"
+# [initAndConfigSimpleUnGrasp]
+
+# Twist to retract from the object
+retract = TwistStamped()
+retract.header.frame_id = "world"
+retract.twist.linear.y = 1.0
+place.setRetractMotion(retract, 0.00, 0.25) #(retract, 0.03, 0.1)
+
+# Twist to place the object
+placeMotion = TwistStamped()
+placeMotion.header.frame_id = "qbhand_base_link"
+placeMotion.twist.linear.y = 1.0
+place.setPlaceMotion(placeMotion, 0.00, 0.25) #(placeMotion, 0.03, 0.1)
+
+# Add the place pipeline to the task's hierarchy
+task.add(place)
+# [initAndConfigPlace]
+# [pickAndPlaceTut12]
+
+# [pickAndPlaceTut13]
+if task.plan():
+    task.publish(task.solutions[0])
+
+# avoid ClassLoader warning
+del pipeline
+del planners
+# [pickAndPlaceTut13]
+
+# Prevent the program from exiting, giving you the opportunity to inspect solutions in rviz
+time.sleep(3600)
diff --git a/air_moveit_config/src/pick_ball_MTC.cpp b/air_moveit_config/src/pick_ball_MTC.cpp
new file mode 100644
index 0000000..f36cd17
--- /dev/null
+++ b/air_moveit_config/src/pick_ball_MTC.cpp
@@ -0,0 +1,511 @@
+/*********************************************************************
+ * BSD 3-Clause License
+ *
+ * Copyright (c) 2019 PickNik LLC.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  * Neither the name of the copyright holder nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Henning Kayser, Simon Goldstein
+   Desc:   A demo to show MoveIt Task Constructor in action
+*/
+
+#include <moveit_task_constructor_demo/pick_place_task.h>
+#include <rosparam_shortcuts/rosparam_shortcuts.h>
+
+namespace moveit_task_constructor_demo {
+
+constexpr char LOGNAME[] = "moveit_task_constructor_demo";
+constexpr char PickPlaceTask::LOGNAME[];
+
+void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) {
+	if (!psi.applyCollisionObject(object))
+		throw std::runtime_error("Failed to spawn object: " + object.id);
+}
+
+moveit_msgs::CollisionObject createTable(ros::NodeHandle& pnh) {
+	std::string table_name, table_reference_frame;
+	std::vector<double> table_dimensions;
+	geometry_msgs::Pose pose;
+	std::size_t errors = 0;
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose);
+	rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
+
+	moveit_msgs::CollisionObject object;
+	object.id = table_name;MoveTo
+	object.primitive_poses.push_back(pose);
+	return object;
+}
+
+moveit_msgs::CollisionObject createObject(ros::NodeHandle& pnh) {
+	std::string object_name, object_reference_frame;
+	std::vector<double> object_dimensions;
+	geometry_msgs::Pose pose;
+	std::size_t error = 0;
+	error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name);
+	error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame);
+	error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions);
+	error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose);
+	rosparam_shortcuts::shutdownIfError(LOGNAME, error);
+
+	moveit_msgs::CollisionObject object;
+	object.id = object_name;
+	object.header.frame_id = object_reference_frame;
+	object.primitives.resize(1);
+	object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
+	object.primitives[0].dimensions = object_dimensions;
+	pose.position.z += 0.5 * object_dimensions[0];
+	object.primitive_poses.push_back(pose);
+	return object;
+}
+
+void setupDemoScene(ros::NodeHandle& pnh) {
+	// Add table and object to planning scene
+	ros::Duration(1.0).sleep();  // Wait for ApplyPlanningScene service
+	moveit::planning_interface::PlanningSceneInterface psi;
+	if (pnh.param("spawn_table", true))
+		spawnObject(psi, createTable(pnh));
+	spawnObject(psi, createObject(pnh));
+}
+
+PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh)
+  : pnh_(pnh), task_name_(task_name) {
+	loadParameters();
+}
+
+void PickPlaceTask::loadParameters() {
+	/****************************************************
+	 *                                                  *
+	 *               Load Parameters                    *
+	 *                                                  *
+	 ***************************************************/
+	ROS_INFO_NAMED(LOGNAME, "Loading task parameters");
+
+	// Planning group properties
+	size_t errors = 0;
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_group_name", arm_group_name_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_group_name", hand_group_name_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "eef_name", eef_name_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_frame", hand_frame_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "world_frame", world_frame_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "grasp_frame_transform", grasp_frame_transform_);
+
+	// Predefined pose targets
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_open_pose", hand_open_pose_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_close_pose", hand_close_pose_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_home_pose", arm_home_pose_);
+
+	// Target object
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_name", object_name_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_dimensions", object_dimensions_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_reference_frame", object_reference_frame_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "surface_link", surface_link_);
+	support_surfaces_ = { surface_link_ };
+
+	// Pick/Place metrics
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_min_dist", approach_object_min_dist_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_max_dist", approach_object_max_dist_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_min_dist", lift_object_min_dist_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_max_dist", lift_object_max_dist_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_surface_offset", place_surface_offset_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_pose", place_pose_);
+	rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
+}
+
+bool PickPlaceTask::init() {
+	ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
+	const std::string object = object_name_;
+
+	// Reset ROS introspection before constructing the new object
+	// TODO(v4hn): global storage for Introspection services to enable one-liner
+	task_.reset();
+	task_.reset(new moveit::task_constructor::Task());
+
+	Task& t = *task_;
+	t.stages()->setName(task_name_);
+	t.loadRobotModel();
+
+	// Sampling planner
+	auto sampling_planner = std::make_shared<solvers::PipelinePlanner>();
+	sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
+
+	// Cartesian planner
+	auto cartesian_planner = std::make_shared<solvers::CartesianPath>();
+	cartesian_planner->setMaxVelocityScaling(1.0);
+	cartesian_planner->setMaxAccelerationScaling(1.0);
+	cartesian_planner->setStepSize(.01);
+
+	// Set task properties
+	t.setProperty("group", arm_group_name_);
+	t.setProperty("eef", eef_name_);
+	t.setProperty("hand", hand_group_name_);
+	t.setProperty("hand_grasping_frame", hand_frame_);
+	t.setProperty("ik_frame", hand_frame_);
+
+	/****************************************************
+	 *                                                  *
+	 *               Current State                      *
+	 *                                                  *
+	 ***************************************************/
+	Stage* current_state_ptr = nullptr;  // Forward current_state on to grasp pose generator
+	{
+		auto current_state = std::make_unique<stages::CurrentState>("current state");
+
+		// Verify that object is not attached
+		auto applicability_filter =
+		    std::make_unique<stages::PredicateFilter>("applicability test", std::move(current_state));
+		applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) {
+			if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
+				comment = "object with id '" + object + "' is already attached and cannot be picked";
+				return false;
+			}
+			return true;
+		});
+
+		current_state_ptr = applicability_filter.get();
+		t.add(std::move(applicability_filter));
+	}
+
+	/****************************************************
+	 *                                                  *
+	 *               Open Hand                          *
+	 *                                                  *
+	 ***************************************************/
+	{  // Open Hand
+		auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
+		stage->setGroup(hand_group_name_);
+		stage->setGoal(hand_open_pose_);
+		t.add(std::move(stage));
+	}
+
+	/****************************************************
+	 *                                                  *
+	 *               Move to Pick                       *
+	 *                                                  *
+	 ***************************************************/
+	{  // Move-to pre-grasp
+		auto stage = std::make_unique<stages::Connect>(
+		    "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
+		stage->setTimeout(5.0);
+		stage->properties().configureInitFrom(Stage::PARENT);
+		t.add(std::move(stage));
+	}
+
+	/****************************************************
+	 *                                                  *
+	 *               Pick Object                        *
+	 *                                                  *
+	 ***************************************************/
+	Stage* attach_object_stage = nullptr;  // Forward attach_object_stage to place pose generator
+	{
+		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" });
+
+		/****************************************************
+  ---- *               Approach Object                    *
+		 ***************************************************/
+		{
+			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->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_);
+
+			// Set hand forward direction
+			geometry_msgs::Vector3Stamped vec;
+			vec.header.frame_id = hand_frame_;
+			vec.vector.z = 1.0;
+			stage->setDirection(vec);
+			grasp->insert(std::move(stage));
+		}
+
+		/****************************************************
+  ---- *               Generate Grasp Pose                *
+		 ***************************************************/
+		{
+			// Sample grasp pose
+			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->setAngleDelta(M_PI / 12);
+			stage->setMonitoredStage(current_state_ptr);  // Hook into current state
+
+			// Compute IK
+			auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage));
+			wrapper->setMaxIKSolutions(8);
+			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" });
+			grasp->insert(std::move(wrapper));
+		}
+
+		/****************************************************
+  ---- *               Allow Collision (hand object)   *
+		 ***************************************************/
+		{
+			auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (hand,object)");
+			stage->allowCollisions(
+			    object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
+			    true);
+			grasp->insert(std::move(stage));
+		}
+
+		/****************************************************
+  ---- *               Close Hand                      *
+		 ***************************************************/
+		{
+			auto stage = std::make_unique<stages::MoveTo>("close hand", sampling_planner);
+			stage->setGroup(hand_group_name_);
+			stage->setGoal(hand_close_pose_);
+			grasp->insert(std::move(stage));
+		}
+
+		/****************************************************
+  .... *               Attach Object                      *
+		 ***************************************************/
+		{
+			auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
+			stage->attachObject(object, hand_frame_);
+			attach_object_stage = stage.get();
+			grasp->insert(std::move(stage));
+		}
+
+		/****************************************************
+  .... *               Allow collision (object support)   *
+		 ***************************************************/
+		{
+			auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (object,support)");
+			stage->allowCollisions({ object }, support_surfaces_, true);
+			grasp->insert(std::move(stage));
+		}
+
+		/****************************************************
+  .... *               Lift object                        *
+		 ***************************************************/
+		{
+			auto stage = std::make_unique<stages::MoveRelative>("lift object", cartesian_planner);
+			stage->properties().configureInitFrom(Stage::PARENT, { "group" });
+			stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_);
+			stage->setIKFrame(hand_frame_);
+			stage->properties().set("marker_ns", "lift_object");
+
+			// Set upward direction
+			geometry_msgs::Vector3Stamped vec;
+			vec.header.frame_id = world_frame_;
+			vec.vector.z = 1.0;
+			stage->setDirection(vec);
+			grasp->insert(std::move(stage));
+		}
+
+		/****************************************************
+  .... *               Forbid collision (object support)  *
+		 ***************************************************/
+		{
+			auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (object,surface)");
+			stage->allowCollisions({ object }, support_surfaces_, false);
+			grasp->insert(std::move(stage));
+		}
+
+		// Add grasp container to task
+		t.add(std::move(grasp));
+	}
+
+	/******************************************************
+	 *                                                    *
+	 *          Move to Place                             *
+	 *                                                    *
+	 *****************************************************/
+	{
+		auto stage = std::make_unique<stages::Connect>(
+		    "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
+		stage->setTimeout(5.0);
+		stage->properties().configureInitFrom(Stage::PARENT);
+		t.add(std::move(stage));
+	}
+
+	/******************************************************
+	 *                                                    *
+	 *          Place Object                              *
+	 *                                                    *
+	 *****************************************************/
+	{
+		auto place = std::make_unique<SerialContainer>("place object");
+		t.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
+		place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" });
+
+		/******************************************************
+  ---- *          Lower Object                              *
+		 *****************************************************/
+		{
+			auto stage = std::make_unique<stages::MoveRelative>("lower object", cartesian_planner);
+			stage->properties().set("marker_ns", "lower_object");
+			stage->properties().set("link", hand_frame_);
+			stage->properties().configureInitFrom(Stage::PARENT, { "group" });
+			stage->setMinMaxDistance(.0, .13);
+
+			// Set downward direction
+			geometry_msgs::Vector3Stamped vec;
+			vec.header.frame_id = world_frame_;
+			vec.vector.z = -1.0;
+			stage->setDirection(vec);
+			place->insert(std::move(stage));
+		}
+
+		/******************************************************
+  ---- *          Generate Place Pose                       *
+		 *****************************************************/
+		{
+			// Generate Place Pose
+			auto stage = std::make_unique<stages::GeneratePlacePose>("generate place pose");
+			stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" });
+			stage->properties().set("marker_ns", "place_pose");
+			stage->setObject(object);
+
+			// Set target pose
+			geometry_msgs::PoseStamped p;
+			p.header.frame_id = object_reference_frame_;
+			p.pose = place_pose_;
+			p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_;
+			stage->setPose(p);
+			stage->setMonitoredStage(attach_object_stage);  // Hook into attach_object_stage
+
+			// Compute IK
+			auto wrapper = std::make_unique<stages::ComputeIK>("place pose IK", std::move(stage));
+			wrapper->setMaxIKSolutions(2);
+			wrapper->setIKFrame(grasp_frame_transform_, hand_frame_);
+			wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
+			wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
+			place->insert(std::move(wrapper));
+		}
+
+		/******************************************************
+  ---- *          Open Hand                              *
+		 *****************************************************/
+		{
+			auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
+			stage->setGroup(hand_group_name_);
+			stage->setGoal(hand_open_pose_);
+			place->insert(std::move(stage));
+		}
+
+		/******************************************************
+  ---- *          Forbid collision (hand, object)        *
+		 *****************************************************/
+		{
+			auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (hand,object)");
+			stage->allowCollisions(object_name_, *t.getRobotModel()->getJointModelGroup(hand_group_name_), false);
+			place->insert(std::move(stage));
+		}
+
+		/******************************************************
+  ---- *          Detach Object                             *
+		 *****************************************************/
+		{
+			auto stage = std::make_unique<stages::ModifyPlanningScene>("detach object");
+			stage->detachObject(object_name_, hand_frame_);
+			place->insert(std::move(stage));
+		}
+
+		/******************************************************
+  ---- *          Retreat Motion                            *
+		 *****************************************************/
+		{
+			auto stage = std::make_unique<stages::MoveRelative>("retreat after place", cartesian_planner);
+			stage->properties().configureInitFrom(Stage::PARENT, { "group" });
+			stage->setMinMaxDistance(.12, .25);
+			stage->setIKFrame(hand_frame_);
+			stage->properties().set("marker_ns", "retreat");
+			geometry_msgs::Vector3Stamped vec;
+			vec.header.frame_id = hand_frame_;
+			vec.vector.z = -1.0;
+			stage->setDirection(vec);
+			place->insert(std::move(stage));
+		}
+
+		// Add place container to task
+		t.add(std::move(place));
+	}
+
+	/******************************************************
+	 *                                                    *
+	 *          Move to Home                              *
+	 *                                                    *
+	 *****************************************************/
+	{
+		auto stage = std::make_unique<stages::MoveTo>("move home", sampling_planner);
+		stage->properties().configureInitFrom(Stage::PARENT, { "group" });
+		stage->setGoal(arm_home_pose_);
+		stage->restrictDirection(stages::MoveTo::FORWARD);
+		t.add(std::move(stage));
+	}
+
+	// prepare Task structure for planning
+	try {
+		t.init();
+	} catch (InitStageException& e) {
+		ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e);
+		return false;
+	}
+
+	return true;
+}
+
+bool PickPlaceTask::plan() {
+	ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
+	int max_solutions = pnh_.param<int>("max_solutions", 10);
+
+	return static_cast<bool>(task_->plan(max_solutions));
+}
+
+bool PickPlaceTask::execute() {
+	ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory");
+	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()->fillMessage(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);
+		return false;
+	}
+
+	return true;
+}
+}  // namespace moveit_task_constructor_demo