From 2b18369550526373bd54fe1c747e4479009909ec Mon Sep 17 00:00:00 2001 From: FabianSchuetze Date: Tue, 23 Feb 2021 08:13:58 +0100 Subject: [PATCH 1/5] documenting instructions for the pick place task --- demo/src/pick_place_task.cpp | 97 ++++++++++++++++++++++++++---------- 1 file changed, 70 insertions(+), 27 deletions(-) diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 74b08f8a6..e3f8df317 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -82,12 +82,20 @@ void PickPlaceTask::loadParameters() { rosparam_shortcuts::shutdownIfError(LOGNAME, errors); } +// The required sequences for a successful pick-and-place task are defined below. The entire process is comprised of six +// "high level" movements: Checking if the object to be picked is already attached to the robot arm, opening the robot's +// hand, moving the robot to the pre-grasp position, grasping the object, moving the robot to a pre-place position, +// placing the object, and finally, moving the robot to a standstill position. These six sequences are listed on the +// least indented column of the "Motion Planning Tasks" widget in Rviz. These sequences are not planned or executed but +// defined as part of moveit's task constructor object below. The stages are be described in more detail below. + void PickPlaceTask::init() { ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); const std::string object = object_name_; // Reset ROS introspection before constructing the new object // TODO(henningkayser): verify this is a bug, fix if possible + // The structure of the pick-and-place process is collected inside a moveit::task_constructor::Task. task_.reset(); task_.reset(new moveit::task_constructor::Task()); @@ -95,7 +103,8 @@ void PickPlaceTask::init() { t.stages()->setName(task_name_); t.loadRobotModel(); - // Sampling planner + //The robot movement is planned by different planners: a cartesian planner, a pipeline planner, or a joint + //interpolation planner. auto sampling_planner = std::make_shared(); sampling_planner->setProperty("goal_joint_tolerance", 1e-5); @@ -117,6 +126,7 @@ void PickPlaceTask::init() { * Current State * * * ***************************************************/ + // Before beginning with the operation, one needs to verify if the object is not already attached to the robot. Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator { auto current_state = std::make_unique("current state"); @@ -153,6 +163,9 @@ void PickPlaceTask::init() { * Move to Pick * * * ***************************************************/ + // This is the second major phase of picking the object. This phase moves to the start of the picking location. The + // movement is generated by a stage::Connect object which itself does not declare any movements but acts as a bridge + // between two stages that are precisely defined. { // Move-to pre-grasp auto stage = std::make_unique( "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); @@ -168,6 +181,10 @@ void PickPlaceTask::init() { ***************************************************/ Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator { + // The third major stage of the pick-and-place sequence is defined: picking the object. The grasp is defined + // inside a SerialContainer. A SerialContainer consists of a sequence of subordinate tasks. Stages other than + // these subordinate tasks can communicate only with the entire SerialContainer and do not need to know each + // subordinate task. 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" }); @@ -176,7 +193,11 @@ void PickPlaceTask::init() { ---- * Approach Object * ***************************************************/ { - auto stage = std::make_unique("approach object", cartesian_planner); + // This movement approaches the object and stops right before it through a MoveRelative stage. The stage + // resembles the Connect because it does not define an end or starting point but move across space as defined + // as a geometryMsgs. In contrast to the Connect stage, however, the planner does not have any flexibility in + // the cartesian structure of the movement as this is precisely defined. + 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" }); @@ -194,16 +215,24 @@ void PickPlaceTask::init() { ---- * Generate Grasp Pose * ***************************************************/ { - // Sample grasp pose - 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 / 12); - stage->setMonitoredStage(current_state_ptr); // Hook into current state - - // Compute IK + // The object grasp is defined by combining two stages, a GenerateGraspPose and a ComputeIK pose. The + // generateGraspPose latches several possible target poses onto the object to be grasped. The + // GenerateGraspPose circles in AngleDelta increments along the object creating one target pose in each + // instance. + 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 + + // The ComputeIK stage defines the grasp pose for the end-effector. The ComputeIK stage attempts to compute + // the appropriate joint parameters from two components: The target posed sampled from the GenerateGraspPose + // stage and the user-defined grasp_frame_transform: The proposed solution is post-multiplied by the + // inverse of the grasp_frame_transform to yield a target frame to be attained by the robot's end-effector. + // If an IK solution is found, the end effectors frame times the rasp_frame_transform equals the frame + // generated in the GenerateGraspPose stage and the object can be picked. auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); wrapper->setMaxIKSolutions(8); wrapper->setMinSolutionDistance(1.0); @@ -216,19 +245,25 @@ void PickPlaceTask::init() { /**************************************************** ---- * Allow Collision (hand 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)); - } + { + // After the grasp frame has been determined, the next step is to pick up the object. At first, the planning + // scene needs to be modified to allow the robot to pick up the object. This stage does not alter the + // robot's position. + 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)); + } /**************************************************** ---- * Close Hand * ***************************************************/ { - auto stage = std::make_unique("close hand", sampling_planner); + // To close the hand, the robot moves from the position of an open hand to a position with a closed hand. + // Similar to moving from the initial pose to the pose with `open hand` as done in the beginning, the MoveTo + // stage is used. + auto stage = std::make_unique("close hand", sampling_planner); stage->setGroup(hand_group_name_); stage->setGoal(hand_close_pose_); grasp->insert(std::move(stage)); @@ -237,6 +272,8 @@ void PickPlaceTask::init() { /**************************************************** .... * Attach Object * ***************************************************/ + // The next four stages modify the planning scene again by attaching the object to the hand, and lifting the + // object while guaranteeing that the cylinder does not touch the ground. { auto stage = std::make_unique("attach object"); stage->attachObject(object, hand_frame_); @@ -274,11 +311,11 @@ void PickPlaceTask::init() { /**************************************************** .... * Forbid collision (object support) * ***************************************************/ - { - auto stage = std::make_unique("forbid collision (object,surface)"); - stage->allowCollisions({ object }, support_surfaces_, false); - 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)); + } // Add grasp container to task t.add(std::move(grasp)); @@ -290,7 +327,10 @@ void PickPlaceTask::init() { * * *****************************************************/ { - auto stage = std::make_unique( + // Similar to the `move to pick` stage, the `move to place` stage is defined implicitly as a Connect stage: No + // robot movement on its own is defined but the movement bridges the previous `pick object` and the following + // `place object` serial containers. + 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); @@ -302,7 +342,10 @@ void PickPlaceTask::init() { * Place Object * * * *****************************************************/ - { + // Placing the object constitutes again a sequence of operations that are combined into a SerialContainer so that + // outside stages only need to communicate with the boundaries of the SerialContainer. The process is very similar + // to the pick object process described above and thus not illustrated. + { auto place = std::make_unique("place object"); t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); From 162f66c76c3bbd5137d77f2125d7249fa59f5196 Mon Sep 17 00:00:00 2001 From: FabianSchuetze Date: Tue, 23 Feb 2021 09:36:34 +0100 Subject: [PATCH 2/5] apply clang-format --- demo/src/pick_place_task.cpp | 126 +++++++++++++++++------------------ 1 file changed, 63 insertions(+), 63 deletions(-) diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index e3f8df317..36b24e5cc 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -95,7 +95,7 @@ void PickPlaceTask::init() { // Reset ROS introspection before constructing the new object // TODO(henningkayser): verify this is a bug, fix if possible - // The structure of the pick-and-place process is collected inside a moveit::task_constructor::Task. + // The structure of the pick-and-place process is collected inside a moveit::task_constructor::Task. task_.reset(); task_.reset(new moveit::task_constructor::Task()); @@ -103,8 +103,8 @@ void PickPlaceTask::init() { t.stages()->setName(task_name_); t.loadRobotModel(); - //The robot movement is planned by different planners: a cartesian planner, a pipeline planner, or a joint - //interpolation planner. + // The robot movement is planned by different planners: a cartesian planner, a pipeline planner, or a joint + // interpolation planner. auto sampling_planner = std::make_shared(); sampling_planner->setProperty("goal_joint_tolerance", 1e-5); @@ -126,7 +126,7 @@ void PickPlaceTask::init() { * Current State * * * ***************************************************/ - // Before beginning with the operation, one needs to verify if the object is not already attached to the robot. + // Before beginning with the operation, one needs to verify if the object is not already attached to the robot. Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator { auto current_state = std::make_unique("current state"); @@ -163,9 +163,9 @@ void PickPlaceTask::init() { * Move to Pick * * * ***************************************************/ - // This is the second major phase of picking the object. This phase moves to the start of the picking location. The - // movement is generated by a stage::Connect object which itself does not declare any movements but acts as a bridge - // between two stages that are precisely defined. + // This is the second major phase of picking the object. This phase moves to the start of the picking location. The + // movement is generated by a stage::Connect object which itself does not declare any movements but acts as a bridge + // between two stages that are precisely defined. { // Move-to pre-grasp auto stage = std::make_unique( "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); @@ -181,10 +181,10 @@ void PickPlaceTask::init() { ***************************************************/ Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator { - // The third major stage of the pick-and-place sequence is defined: picking the object. The grasp is defined - // inside a SerialContainer. A SerialContainer consists of a sequence of subordinate tasks. Stages other than - // these subordinate tasks can communicate only with the entire SerialContainer and do not need to know each - // subordinate task. + // The third major stage of the pick-and-place sequence is defined: picking the object. The grasp is defined + // inside a SerialContainer. A SerialContainer consists of a sequence of subordinate tasks. Stages other than + // these subordinate tasks can communicate only with the entire SerialContainer and do not need to know each + // subordinate task. 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" }); @@ -193,11 +193,11 @@ void PickPlaceTask::init() { ---- * Approach Object * ***************************************************/ { - // This movement approaches the object and stops right before it through a MoveRelative stage. The stage - // resembles the Connect because it does not define an end or starting point but move across space as defined - // as a geometryMsgs. In contrast to the Connect stage, however, the planner does not have any flexibility in - // the cartesian structure of the movement as this is precisely defined. - auto stage = std::make_unique("approach object", cartesian_planner); + // This movement approaches the object and stops right before it through a MoveRelative stage. The stage + // resembles the Connect because it does not define an end or starting point but move across space as defined + // as a geometryMsgs. In contrast to the Connect stage, however, the planner does not have any flexibility in + // the cartesian structure of the movement as this is precisely defined. + 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" }); @@ -215,24 +215,24 @@ void PickPlaceTask::init() { ---- * Generate Grasp Pose * ***************************************************/ { - // The object grasp is defined by combining two stages, a GenerateGraspPose and a ComputeIK pose. The - // generateGraspPose latches several possible target poses onto the object to be grasped. The - // GenerateGraspPose circles in AngleDelta increments along the object creating one target pose in each - // instance. - 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 - - // The ComputeIK stage defines the grasp pose for the end-effector. The ComputeIK stage attempts to compute - // the appropriate joint parameters from two components: The target posed sampled from the GenerateGraspPose - // stage and the user-defined grasp_frame_transform: The proposed solution is post-multiplied by the - // inverse of the grasp_frame_transform to yield a target frame to be attained by the robot's end-effector. - // If an IK solution is found, the end effectors frame times the rasp_frame_transform equals the frame - // generated in the GenerateGraspPose stage and the object can be picked. + // The object grasp is defined by combining two stages, a GenerateGraspPose and a ComputeIK pose. The + // generateGraspPose latches several possible target poses onto the object to be grasped. The + // GenerateGraspPose circles in AngleDelta increments along the object creating one target pose in each + // instance. + 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 + + // The ComputeIK stage defines the grasp pose for the end-effector. The ComputeIK stage attempts to compute + // the appropriate joint parameters from two components: The target posed sampled from the GenerateGraspPose + // stage and the user-defined grasp_frame_transform: The proposed solution is post-multiplied by the + // inverse of the grasp_frame_transform to yield a target frame to be attained by the robot's end-effector. + // If an IK solution is found, the end effectors frame times the rasp_frame_transform equals the frame + // generated in the GenerateGraspPose stage and the object can be picked. auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); wrapper->setMaxIKSolutions(8); wrapper->setMinSolutionDistance(1.0); @@ -245,25 +245,25 @@ void PickPlaceTask::init() { /**************************************************** ---- * Allow Collision (hand object) * ***************************************************/ - { - // After the grasp frame has been determined, the next step is to pick up the object. At first, the planning - // scene needs to be modified to allow the robot to pick up the object. This stage does not alter the - // robot's position. - 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)); - } + { + // After the grasp frame has been determined, the next step is to pick up the object. At first, the planning + // scene needs to be modified to allow the robot to pick up the object. This stage does not alter the + // robot's position. + 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)); + } /**************************************************** ---- * Close Hand * ***************************************************/ { - // To close the hand, the robot moves from the position of an open hand to a position with a closed hand. - // Similar to moving from the initial pose to the pose with `open hand` as done in the beginning, the MoveTo - // stage is used. - auto stage = std::make_unique("close hand", sampling_planner); + // To close the hand, the robot moves from the position of an open hand to a position with a closed hand. + // Similar to moving from the initial pose to the pose with `open hand` as done in the beginning, the MoveTo + // stage is used. + auto stage = std::make_unique("close hand", sampling_planner); stage->setGroup(hand_group_name_); stage->setGoal(hand_close_pose_); grasp->insert(std::move(stage)); @@ -272,8 +272,8 @@ void PickPlaceTask::init() { /**************************************************** .... * Attach Object * ***************************************************/ - // The next four stages modify the planning scene again by attaching the object to the hand, and lifting the - // object while guaranteeing that the cylinder does not touch the ground. + // The next four stages modify the planning scene again by attaching the object to the hand, and lifting the + // object while guaranteeing that the cylinder does not touch the ground. { auto stage = std::make_unique("attach object"); stage->attachObject(object, hand_frame_); @@ -311,11 +311,11 @@ void PickPlaceTask::init() { /**************************************************** .... * Forbid collision (object support) * ***************************************************/ - { - auto stage = std::make_unique("forbid collision (object,surface)"); - stage->allowCollisions({ object }, support_surfaces_, false); - 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)); + } // Add grasp container to task t.add(std::move(grasp)); @@ -327,10 +327,10 @@ void PickPlaceTask::init() { * * *****************************************************/ { - // Similar to the `move to pick` stage, the `move to place` stage is defined implicitly as a Connect stage: No - // robot movement on its own is defined but the movement bridges the previous `pick object` and the following - // `place object` serial containers. - auto stage = std::make_unique( + // Similar to the `move to pick` stage, the `move to place` stage is defined implicitly as a Connect stage: No + // robot movement on its own is defined but the movement bridges the previous `pick object` and the following + // `place object` serial containers. + 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); @@ -342,10 +342,10 @@ void PickPlaceTask::init() { * Place Object * * * *****************************************************/ - // Placing the object constitutes again a sequence of operations that are combined into a SerialContainer so that - // outside stages only need to communicate with the boundaries of the SerialContainer. The process is very similar - // to the pick object process described above and thus not illustrated. - { + // Placing the object constitutes again a sequence of operations that are combined into a SerialContainer so that + // outside stages only need to communicate with the boundaries of the SerialContainer. The process is very similar + // to the pick object process described above and thus not illustrated. + { auto place = std::make_unique("place object"); t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); From aeb8aa4b156e647ed215bfe1f84c465fb8b0e828 Mon Sep 17 00:00:00 2001 From: FabianSchuetze Date: Wed, 3 Mar 2021 10:24:11 +0100 Subject: [PATCH 3/5] adjust PR based on comments --- demo/src/pick_place_task.cpp | 65 ++++++++++-------------------------- 1 file changed, 18 insertions(+), 47 deletions(-) diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 36b24e5cc..8121eebc2 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -82,20 +82,14 @@ void PickPlaceTask::loadParameters() { rosparam_shortcuts::shutdownIfError(LOGNAME, errors); } -// The required sequences for a successful pick-and-place task are defined below. The entire process is comprised of six -// "high level" movements: Checking if the object to be picked is already attached to the robot arm, opening the robot's -// hand, moving the robot to the pre-grasp position, grasping the object, moving the robot to a pre-place position, -// placing the object, and finally, moving the robot to a standstill position. These six sequences are listed on the -// least indented column of the "Motion Planning Tasks" widget in Rviz. These sequences are not planned or executed but -// defined as part of moveit's task constructor object below. The stages are be described in more detail below. - +// The init function declares all movements for the pick-and-place task. These movements are described below. void PickPlaceTask::init() { ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); const std::string object = object_name_; // Reset ROS introspection before constructing the new object // TODO(henningkayser): verify this is a bug, fix if possible - // The structure of the pick-and-place process is collected inside a moveit::task_constructor::Task. + // Movements are collected inside a Task object. task_.reset(); task_.reset(new moveit::task_constructor::Task()); @@ -103,8 +97,7 @@ void PickPlaceTask::init() { t.stages()->setName(task_name_); t.loadRobotModel(); - // The robot movement is planned by different planners: a cartesian planner, a pipeline planner, or a joint - // interpolation planner. + // 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); @@ -126,7 +119,7 @@ void PickPlaceTask::init() { * Current State * * * ***************************************************/ - // Before beginning with the operation, one needs to verify if the object is not already attached to the robot. + // First, verify that the object is not already attached to the robot. Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator { auto current_state = std::make_unique("current state"); @@ -163,10 +156,8 @@ void PickPlaceTask::init() { * Move to Pick * * * ***************************************************/ - // This is the second major phase of picking the object. This phase moves to the start of the picking location. The - // movement is generated by a stage::Connect object which itself does not declare any movements but acts as a bridge - // between two stages that are precisely defined. - { // Move-to pre-grasp + // Move to a pre-grasp pose. The Connect stage does not declare any movements but connects two stages that do. + { auto stage = std::make_unique( "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); stage->setTimeout(5.0); @@ -181,10 +172,7 @@ void PickPlaceTask::init() { ***************************************************/ Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator { - // The third major stage of the pick-and-place sequence is defined: picking the object. The grasp is defined - // inside a SerialContainer. A SerialContainer consists of a sequence of subordinate tasks. Stages other than - // these subordinate tasks can communicate only with the entire SerialContainer and do not need to know each - // subordinate task. + // A SerialContainer combines subordinate tasks to pick the object. 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" }); @@ -192,11 +180,9 @@ void PickPlaceTask::init() { /**************************************************** ---- * Approach Object * ***************************************************/ + // Subordinate tasks for the SerialContainer are described below: { - // This movement approaches the object and stops right before it through a MoveRelative stage. The stage - // resembles the Connect because it does not define an end or starting point but move across space as defined - // as a geometryMsgs. In contrast to the Connect stage, however, the planner does not have any flexibility in - // the cartesian structure of the movement as this is precisely defined. + // 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_); @@ -215,10 +201,7 @@ void PickPlaceTask::init() { ---- * Generate Grasp Pose * ***************************************************/ { - // The object grasp is defined by combining two stages, a GenerateGraspPose and a ComputeIK pose. The - // generateGraspPose latches several possible target poses onto the object to be grasped. The - // GenerateGraspPose circles in AngleDelta increments along the object creating one target pose in each - // instance. + // 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"); @@ -227,12 +210,8 @@ void PickPlaceTask::init() { stage->setAngleDelta(M_PI / 4); stage->setMonitoredStage(current_state_ptr); // Hook into current state - // The ComputeIK stage defines the grasp pose for the end-effector. The ComputeIK stage attempts to compute - // the appropriate joint parameters from two components: The target posed sampled from the GenerateGraspPose - // stage and the user-defined grasp_frame_transform: The proposed solution is post-multiplied by the - // inverse of the grasp_frame_transform to yield a target frame to be attained by the robot's end-effector. - // If an IK solution is found, the end effectors frame times the rasp_frame_transform equals the frame - // generated in the GenerateGraspPose stage and the object can be picked. + // 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); @@ -246,9 +225,7 @@ void PickPlaceTask::init() { ---- * Allow Collision (hand object) * ***************************************************/ { - // After the grasp frame has been determined, the next step is to pick up the object. At first, the planning - // scene needs to be modified to allow the robot to pick up the object. This stage does not alter the - // robot's position. + // 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(), @@ -260,9 +237,6 @@ void PickPlaceTask::init() { ---- * Close Hand * ***************************************************/ { - // To close the hand, the robot moves from the position of an open hand to a position with a closed hand. - // Similar to moving from the initial pose to the pose with `open hand` as done in the beginning, the MoveTo - // stage is used. auto stage = std::make_unique("close hand", sampling_planner); stage->setGroup(hand_group_name_); stage->setGoal(hand_close_pose_); @@ -272,8 +246,8 @@ void PickPlaceTask::init() { /**************************************************** .... * Attach Object * ***************************************************/ - // The next four stages modify the planning scene again by attaching the object to the hand, and lifting the - // object while guaranteeing that the cylinder does not touch the ground. + // 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_); @@ -327,9 +301,7 @@ void PickPlaceTask::init() { * * *****************************************************/ { - // Similar to the `move to pick` stage, the `move to place` stage is defined implicitly as a Connect stage: No - // robot movement on its own is defined but the movement bridges the previous `pick object` and the following - // `place object` serial containers. + // Fourth, 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); @@ -342,9 +314,8 @@ void PickPlaceTask::init() { * Place Object * * * *****************************************************/ - // Placing the object constitutes again a sequence of operations that are combined into a SerialContainer so that - // outside stages only need to communicate with the boundaries of the SerialContainer. The process is very similar - // to the pick object process described above and thus not illustrated. + // Fifth, placing the object is defined as a SerialContainer. The process is similar to picking the object and thus + // not illustrated. { auto place = std::make_unique("place object"); t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); From b09327045385831a01eec87e9bae8ffbb0a407ea Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 6 Jul 2024 17:47:03 +0200 Subject: [PATCH 4/5] Cleanup comments --- demo/src/pick_place_task.cpp | 55 +++++++++++++++--------------------- 1 file changed, 23 insertions(+), 32 deletions(-) diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index d956e48d4..866ec018b 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -144,22 +144,24 @@ void PickPlaceTask::loadParameters() { rosparam_shortcuts::shutdownIfError(LOGNAME, errors); } -// The init function declares all movements for the pick-and-place task. These movements are described below. +// Initialize the task pipeline 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 - // Movements are collected inside a Task object. 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(); - // Different planners plan robot movement: a cartesian , pipeline, or joint interpolation planner. + /* 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(); sampling_planner->setProperty("goal_joint_tolerance", 1e-5); @@ -203,7 +205,7 @@ bool PickPlaceTask::init() { * * ***************************************************/ Stage* initial_state_ptr = nullptr; - { // Open Hand + { auto stage = std::make_unique("open hand", sampling_planner); stage->setGroup(hand_group_name_); stage->setGoal(hand_open_pose_); @@ -216,7 +218,7 @@ bool PickPlaceTask::init() { * Move to Pick * * * ***************************************************/ - // Move to a pre-grasp pose. The Connect stage does not declare any movements but connects two stages that do. + // Connect initial open-hand state with pre-grasp pose defined in the following { auto stage = std::make_unique( "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); @@ -232,7 +234,7 @@ bool PickPlaceTask::init() { ***************************************************/ Stage* pick_stage_ptr = nullptr; { - // A SerialContainer combines subordinate tasks to pick the object. + // A SerialContainer combines several sub-stages, here for picking the object 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" }); @@ -240,13 +242,12 @@ bool PickPlaceTask::init() { /**************************************************** ---- * Approach Object * ***************************************************/ - // Subordinate tasks for the SerialContainer are described below: { - // The arm moves along the z-dimension and stops right before the object. + // Move the eef link forward along its z-axis by an amount within the given min-max range 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->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 @@ -261,23 +262,23 @@ bool PickPlaceTask::init() { ---- * Generate Grasp Pose * ***************************************************/ { - // Sample grasp pose candidates in angle increments around the z-axis of the object. + // 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->setObject(object); // object to sample grasps for stage->setAngleDelta(M_PI / 12); stage->setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions - // 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. + // Compute IK for sampled grasp poses auto wrapper = std::make_unique("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)); } @@ -285,7 +286,7 @@ bool PickPlaceTask::init() { ---- * Allow Collision (hand object) * ***************************************************/ { - // Modify the planning scene (does not alter the robot's position) to permit picking up the object. + // Modify planning scene (w/o altering the robot's pose) to allow touching the object for picking auto stage = std::make_unique("allow collision (hand,object)"); stage->allowCollisions( object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), @@ -306,11 +307,9 @@ bool PickPlaceTask::init() { /**************************************************** .... * Attach Object * ***************************************************/ - // 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_); + stage->attachObject(object, hand_frame_); // attach object to hand_frame_ grasp->insert(std::move(stage)); } @@ -362,7 +361,7 @@ bool PickPlaceTask::init() { * * *****************************************************/ { - // Fourth, define the `move to place` (as the `move to pick`) stage as a Connect object. + // Connect the grasped state to the pre-place state, i.e. realize the object transport auto stage = std::make_unique( "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); stage->setTimeout(5.0); @@ -375,8 +374,7 @@ bool PickPlaceTask::init() { * Place Object * * * *****************************************************/ - // Fifth, placing the object is defined as a SerialContainer. The process is similar to picking the object and thus - // not illustrated. + // All placing sub-stages are collected within a serial container again { auto place = std::make_unique("place object"); t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); @@ -511,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 - // 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); From c83c99e76ff876ea773bb6bc63bf09a6af4c1161 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 6 Jul 2024 17:49:43 +0200 Subject: [PATCH 5/5] minor change --- demo/src/pick_place_task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 866ec018b..92c4f72e0 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -144,7 +144,7 @@ void PickPlaceTask::loadParameters() { rosparam_shortcuts::shutdownIfError(LOGNAME, errors); } -// Initialize the task pipeline +// Initialize the task pipeline, defining individual movement stages bool PickPlaceTask::init() { ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); const std::string object = object_name_;