From f57c258e74f6d817457a4dbdccb19f4bb525615a Mon Sep 17 00:00:00 2001 From: cpetersmeier Date: Mon, 19 Apr 2021 14:58:11 +0200 Subject: [PATCH] MTC Tutorial: Pick and Place --- .../moveit_task_constructor_tutorial.rst | 451 ++++++++++++++++++ 1 file changed, 451 insertions(+) diff --git a/doc/moveit_task_constructor/moveit_task_constructor_tutorial.rst b/doc/moveit_task_constructor/moveit_task_constructor_tutorial.rst index 50db746e3..096a7be55 100644 --- a/doc/moveit_task_constructor/moveit_task_constructor_tutorial.rst +++ b/doc/moveit_task_constructor/moveit_task_constructor_tutorial.rst @@ -1,6 +1,7 @@ MoveIt Task Constructor ======================= +.. _introductory_image: .. image:: images/mtc_example.png :width: 700px @@ -123,6 +124,456 @@ Combined with the possibility of using class inheritance it is possible to const In general, it is a good habit to encapsulate application-specific `MoveIt Task Constructor` code within a ros-node. +Demo: Pick and Place +-------------------- + +The following section discusses the pick and place demo in more detail. +It might give you some insight on the mechanics of the demo program and +inspire you in the design of your own task hierarchies. +To start, please follow the steps in section `Running the Demo`_. + +The ``PickPlaceTask`` class encapsulates a ros node that runs MTC code. +For the purpose of this tutorial, we reduce the discussion of the class to +the following memberfunctions and attributes: + ++------------------------+-----------------------------------------------------------------------------+ +| ``loadParameters()`` | | Initialize membervariables of the ``PickPlaceTask`` class from the ros | +| | | parameter server | ++------------------------+-----------------------------------------------------------------------------+ +| ``init()`` | | Construct the task hierarchy | ++------------------------+-----------------------------------------------------------------------------+ +| ``execute()`` | | Wrapper around built-in task execution capabilities | ++------------------------+-----------------------------------------------------------------------------+ + +``loadParameters()`` operates on the following parameters: + ++-----------------------------+------------------------------------------------------------------------+ +| planning group properties | | ``arm_group_name_ : string`` | +| | | ``eef_name_ : string`` | +| | | ``hand_group_name_ : string`` | +| | | ``hand_frame_ : string`` | ++-----------------------------+------------------------------------------------------------------------+ +| object + surface properties | | ``support_surfaces : vector`` | +| | | ``object_reference_frame : string`` | +| | | ``surface_link_ : string`` | +| | | ``object_name_ : string`` | +| | | ``world_frame_ : string`` | +| | | ``object_dimensions_ : string`` | ++-----------------------------+------------------------------------------------------------------------+ +| predefined pose targets | | ``hand_open_pose_ : string`` | +| | | ``hand_close_pose_ : string`` | +| | | ``arm_home_pose_ : string`` | ++-----------------------------+------------------------------------------------------------------------+ +| pick metrics | | ``grasp_frame_transform_ : Isometry3d`` | +| | | ``approach_object_min_dist_ : string`` | +| | | ``approach_object_max_dist_ : string`` | +| | | ``lift_object_min_dist_ : string`` | +| | | ``lift_object_max_dist_ : string`` | ++-----------------------------+------------------------------------------------------------------------+ +| place metrics | | ``place_pose_ : Pose`` | +| | | ``place_surface_offset_ : double`` | ++-----------------------------+------------------------------------------------------------------------+ + +The ``init`` function constructs the task hierarchy. +Consult the `introductory_image`_ for a visualization. +Lets discuss the structure briefly by going over some of the +highlights. + +The MoveIt Task Constructor is able to plan bi-directionally through the task hierarchy. +By inspecting the planning direction, which is displayed to the left of the stage name in the +`introductory_image`_, we can examine the planning direction. +`Pick Container`_ and `Place Container`_ stages, as well as the `Initial Setup`_ stages propagate their results +bi-directionally. We can use ``connect`` stages to find motion plans between these intermediary solutions. + + +Initial Setup +^^^^^^^^^^^^^ + +First, start with a clean task by resetting the internal task pointer, +constructing a new task object for the temporal task hierarchy assembly +and load the robot model. + +.. code-block:: c++ + + task_.reset(); + task_.reset(new moveit::task_constructor::Task()); + + Task& t = *task_; + t.stages()->setName(task_name_); + t.loadRobotModel(); + +Set up different planners to be available for stages later on. + +.. code-block:: c++ + + // Different planners plan robot movement: a cartesian , pipeline, or joint interpolation planner. + auto sampling_planner = std::make_shared(); + sampling_planner->setProperty("goal_joint_tolerance", 1e-5); + + // Cartesian planner + auto cartesian_planner = std::make_shared(); + cartesian_planner->setMaxVelocityScaling(1.0); + cartesian_planner->setMaxAccelerationScaling(1.0); + cartesian_planner->setStepSize(.01); + +Set up general task properties. For a detailled discussion on stage +configuration with properties, see the section on `Stage Configuration`_. + +Lets start with extracting the current state of a) the robot and b) the planning scene. +We will use the occasion and test if an object that we want to pick +is already attached to the robot by using a predicate filter wrapper stage. +The lambda expression evaluates if the wrapped-stage's solutions are rejected or passed. + +.. code-block:: c++ + + // maintain a pointer to this stage for later use in monitoring generator + Stage* current_state_ptr = nullptr; + { + auto current_state = std::make_unique("current state"); + + // Verify that object is not attached with a prediace filter wrapper + auto applicability_filter = std::make_unique( + "applicability test", std::move(current_state) + ); + + // Expression to test of object is already attached + 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; + }); + + // Add the wrapped stage to the task hierarchy + current_state_ptr = applicability_filter.get(); + t.add(std::move(applicability_filter)); + } + +Next, we prepare the manipulator by inserting a move stage to open the hand. +Note that we chose the sampling planner we defined earlier as an example. + +.. code-block:: c++ + + { + auto stage = std::make_unique("open hand", sampling_planner); + stage->setGroup(hand_group_name_); + stage->setGoal(hand_open_pose_); + t.add(std::move(stage)); + } + +Pick Container +^^^^^^^^^^^^^^ +Using a connect stage, we can join the solutions of the inital setup with +the solutions of the pick container stages by finding a valid motion plan +between the two. + +.. code-block:: c++ + + { + auto stage = std::make_unique( + "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)); + } + +**Create the serial container to contain the pick-related stages**. +To reuse previously defined properties, we copy them from the +task into the pick serial container. + +.. code-block:: c++ + + auto grasp = std::make_unique("pick object"); + t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); + grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); + +To carry out the pick movement, we first need to approach the desired object: +Note that we insert the following stages into our previously defined serial container, not in the +main task. + +.. code-block:: c++ + + { + // The arm moves along the z-dimension and stops right before the object. + auto stage = std::make_unique("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)); + } + +Remember that the first stage in our task hierarchy was a wrapped ``current state`` stage. +We insert a monitoring generator that takes the solution of that stage (i.e. the current state) + +Next, we insert a stage to generate the grasp pose. +``GenerateGraspPose`` is a derivation of the generic monitoring generator stage. +Thus it monitors another stage and processes its solutions. +In this case, the solution of the first stage of the task hierarchy (``CurrentState``) +is taken and possible grasp candidates are sampled around it in a specified interval. +Note how we configure most of the stage's properties using the preceding *Parent* stage. + +.. code-block:: c++ + + { + // Sample grasp pose candidates in angle increments around the z-axis of the object. + auto stage = std::make_unique("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 / 4); + stage->setMonitoredStage(current_state_ptr); // Hook into current state + + // Compute joint parameters for a target frame of the end effector. Each target frame equals a grasp pose + // from the previous stage times the inverse of the user-defined grasp_frame_tansform. + auto wrapper = std::make_unique("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)); + } + +Next up, once we found a grasping position, we need to ... + +1. Allow collision between the gripper and the object. +2. Close the gripper. +3. Attach the object. + +.. code-block:: c++ + + { + // Modify the planning scene (does not alter the robot's position) to permit picking up the object. + auto stage = std::make_unique("allow collision (hand,object)"); + stage->allowCollisions( + object, + t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), + true + ); + grasp->insert(std::move(stage)); + } + + { + auto stage = std::make_unique("close hand", sampling_planner); + stage->setGroup(hand_group_name_); + stage->setGoal(hand_close_pose_); + grasp->insert(std::move(stage)); + } + + // Attaching the object to the hand and lifting the object while guaranteeing that it does not touch the + // ground happens in the next four stages. + { + auto stage = std::make_unique("attach object"); + stage->attachObject(object, hand_frame_); + attach_object_stage = stage.get(); + grasp->insert(std::move(stage)); + } + +To lift the object up, the collisions with the object-supporting infrastructure have to be +temporarily disabled. That is because it is assumed that the object is placed onto, i.e. in +collision with a table surface. + +To this end, we need to ... + +1. Allow collision between the object and supporting infrastructure. +2. Perform the lifting motion the object up. +3. Forbid collision between the object and supporting infrastructure. + +.. code-block:: c++ + + { + auto stage = std::make_unique("allow collision (object,support)"); + stage->allowCollisions({ object }, support_surfaces_, true); + grasp->insert(std::move(stage)); + } + + { + auto stage = std::make_unique("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)); + } + + { + auto stage = std::make_unique("forbid collision (object,surface)"); + stage->allowCollisions({ object }, support_surfaces_, false); + grasp->insert(std::move(stage)); + } + +Finally, the grasp container can be added to the main task hierarchy. + +.. code-block:: c++ + + // Add grasp container to task + t.add(std::move(grasp)); + +Place Container +^^^^^^^^^^^^^^^ + +Similarly to the pick container, the place container gets parsed for planning asynchronously to the +rest of the stages. Using a connect stage, the outgoing interface of the place container can be +connected to the outgoing interface of the preceeding pick container. + +.. code-block:: c++ + + { + // Define the `move to place` (as the `move to pick`) stage as a Connect object. + auto stage = std::make_unique( + "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)); + } + +Initialize a serial container to contain the subordinate stages that are related to the pick task. + +.. code-block:: c++ + + auto place = std::make_unique("place object"); + t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); + place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); + +To place an object, first lower the manipulator. + +.. code-block:: c++ + + { + auto stage = std::make_unique("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(.03, .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)); + } + +Using a `target` pose that the object shall obtain, generate inverse kinematic solutions in which +the robot arm achieves this pose. + +.. code-block:: c++ + + { + // Generate Place Pose + auto stage = std::make_unique("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("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)); + } + +Next, perform the following steps to release the object from the manipulator. + +1. Open the gripper. +2. Disable collisions between the gripper and the grasped-object. +3. Detach the object. + +.. code-block:: c++ + + { + auto stage = std::make_unique("open hand", sampling_planner); + stage->setGroup(hand_group_name_); + stage->setGoal(hand_open_pose_); + place->insert(std::move(stage)); + } + + { + auto stage = std::make_unique("forbid collision (hand,object)"); + stage->allowCollisions( + object_name_, + t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), + false + ); + place->insert(std::move(stage)); + } + + { + auto stage = std::make_unique("detach object"); + stage->detachObject(object_name_, hand_frame_); + place->insert(std::move(stage)); + } + +Using the robot manipulator, retreat the end effector manipulator from the object after placing it down. + +.. code-block:: c++ + + { + auto stage = std::make_unique("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)); + } + +To this end, add the place container to the task hierarchy. + +.. code-block:: c++ + + // Add place container to task + t.add(std::move(place)); + +Final +^^^^^ + +To complete the sequence, the robot manipulator should be directed to +the starting position. This can, for example, be done by using a ``MoveTo`` +stage and computing the motion with a probabilistic sampling planner. + +.. code-block:: c++ + + { + auto stage = std::make_unique("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)); + } + Programmatic Extension ----------------------