Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

documenting instructions for the pick-and-place task #238

Merged
merged 7 commits into from
Jul 6, 2024
Merged

documenting instructions for the pick-and-place task #238

Changes from 1 commit
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
2b18369
documenting instructions for the pick place task
FabianSchuetze Feb 23, 2021
162f66c
apply clang-format
FabianSchuetze Feb 23, 2021
f1d0f8b
Merge branch 'master' into illustrate_pick_place_task
FabianSchuetze Mar 1, 2021
aeb8aa4
adjust PR based on comments
FabianSchuetze Mar 3, 2021
2086b04
Merge branch 'master' into illustrate_pick_place_task
rhaschke Jul 6, 2024
b093270
Cleanup comments
rhaschke Jul 6, 2024
c83c99e
minor change
rhaschke Jul 6, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
97 changes: 70 additions & 27 deletions demo/src/pick_place_task.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,20 +82,29 @@ 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
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

"least indented" --> "top-level", although since technically that's not true either I'd skip it altogether. "These sequences are shown in the widget..."

// defined as part of moveit's task constructor object below. The stages are be described in more detail below.
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'd call it a Task object.


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());

Task& t = *task_;
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<solvers::PipelinePlanner>();
sampling_planner->setProperty("goal_joint_tolerance", 1e-5);

Expand All @@ -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.
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

  1. I would try to keep it shorter, in general. E.g. "We verify that the object is not already attached to the robot."
  2. Does every task need to start with a state? Would help to say something about that here instead (otherwise there are 2 (3) comments within 10 lines about the verification).

Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator
{
auto current_state = std::make_unique<stages::CurrentState>("current state");
Expand Down Expand Up @@ -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
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would keep it shorter here, too. E.g. "Moves to a pre-grasp pose from where the gripper approaches the object. Generated by a stage::Connect object which does not plan a movement, but connects two stages that do."

Also, remove the comment in the next line

// 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<stages::Connect>(
"move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
Expand All @@ -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<SerialContainer>("pick object");
t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
Expand All @@ -176,7 +193,11 @@ void PickPlaceTask::init() {
---- * Approach Object *
***************************************************/
{
auto stage = std::make_unique<stages::MoveRelative>("approach object", cartesian_planner);
// This movement approaches the object and stops right before it through a MoveRelative stage. The stage
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Misses a comment that this stage is inserted into the grasp/PickObject SerialContainer.

// 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
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

geometry_msgs::Pose?

// the cartesian structure of the movement as this is precisely defined.
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure what you mean by "cartesian structure of the movement"

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" });
Expand All @@ -194,16 +215,24 @@ void PickPlaceTask::init() {
---- * 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
// The object grasp is defined by combining two stages, a GenerateGraspPose and a ComputeIK pose. The
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

"latches onto the object" implies a connection that I don't really see. "connects" seems to make more sense.

"The generateGraspPose" should be "The generateGraspPose stage" or "...container". The phrase "a ComputeIK pose" seems to be used to mean "a ComputeIKpose stage". This is confusing.

GenerateGraspPose is written both with and without capital first letter.

// 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<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 / 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<stages::ComputeIK>("grasp pose IK", std::move(stage));
wrapper->setMaxIKSolutions(8);
wrapper->setMinSolutionDistance(1.0);
Expand All @@ -216,19 +245,25 @@ void PickPlaceTask::init() {
/****************************************************
---- * 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));
}
{
// 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<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);
// 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<stages::MoveTo>("close hand", sampling_planner);
stage->setGroup(hand_group_name_);
stage->setGoal(hand_close_pose_);
grasp->insert(std::move(stage));
Expand All @@ -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<stages::ModifyPlanningScene>("attach object");
stage->attachObject(object, hand_frame_);
Expand Down Expand Up @@ -274,11 +311,11 @@ void PickPlaceTask::init() {
/****************************************************
.... * 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));
}
{
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));
Expand All @@ -290,7 +327,10 @@ void PickPlaceTask::init() {
* *
*****************************************************/
{
auto stage = std::make_unique<stages::Connect>(
// 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<stages::Connect>(
"move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(Stage::PARENT);
Expand All @@ -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<SerialContainer>("place object");
t.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" });
Expand Down