diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 1428eacc4..92c4f72e0 100644 --- a/demo/src/pick_place_task.cpp +++ b/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(); 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("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( "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("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("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,22 +262,23 @@ 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("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("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)); } @@ -278,6 +286,7 @@ bool PickPlaceTask::init() { ---- * 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("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("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( "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("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 - // 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);