Revision history [back]

Please correct me if I am wrong; I am making the following observations based on my experiential results.

MoveIt! Pick and Place Pipeline

Here for opening and closing the gripper, precise joint control is used. Hence if you specify the complete closing position of the gripper upon grasp, the entire pipeline comes to a halt as the final position is never achieved in a setup where you feel the resistance to doing so, like in Gazebo and not in RViz, for instance.

The logical fix would be specifying the closing angles to match the object's width. However, this method is not robust as it would not work for every object and would require knowing the width of the grasping object in advance.

Fix 1 - Hacking the pipeline

If you do not set the gripper width to match the object width on close, the pipeline gets stuck during close. To bypass this, you can set the grasp posture to be open instead of close so that it achieves the trajectory and the retreat distance to be zero. Then you can set orientation constraints and traverse to goal to your desired vector of retreat of choice.

Set gripper to be open when picking instead of closing and failing:
```
openGripperJointAngles(grasps[0].grasp_posture);
```

Set retreat distance to be zero:

grasps[0].post_grasp_retreat.direction.header.frame_id =
  move_group_.getPlanningFrame();
grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
grasps[0].post_grasp_retreat.min_distance = 0.0;
grasps[0].post_grasp_retreat.desired_distance = 0.0;

Call the MoveIt! Pick function:

// This ignores place collision from object and table
move_group_.setSupportSurfaceName("table");

// Start the Pick Pipeline
move_group_.pick("object", grasps);

Close the gripper using GripperCommandActionGoal.

Match the orientation constraint to your retreat direction vector and target orientation:

// Function definition

void setupOrientationConstraints() {
  moveit_msgs::OrientationConstraint ocm;
  ocm.link_name = move_group_.getEndEffectorLink();
  ocm.header.frame_id = move_group_.getPlanningFrame();

  tf2::Quaternion fixed_orientation;
  // example orientation
  fixed_orientation.setRPY(1.5707, 0, 1.5707);
  fixed_orientation.normalize();

  ocm.orientation = tf2::toMsg(fixed_orientation);
  // example tolerances and weight
  ocm.absolute_x_axis_tolerance = 0.075;
  ocm.absolute_y_axis_tolerance = 0.075;
  ocm.absolute_z_axis_tolerance = 0.075;
  ocm.weight = 1.0;

  moveit_msgs::Constraints test_constraints;
  test_constraints.orientation_constraints.push_back(ocm);
  move_group_.setPathConstraints(test_constraints);
}

// Function call
setupOrientationConstraints();

Plan a goal pose matching the earlier retreat grasp position.
Finally clear the orientation constraints:
```
move_group_.clearPathConstraints();
```
There are no changes to the place pipeline as the open gripper posture faces no restrictions.

Fix 2 - Setting up your pipeline

Instinctively, traverse to the pre-grasp and grasp positions.
Close the gripper using the GripperCommandActionGoal Action.
Use the move group's attach function and specify the links that touch the object upon pick up.
```
move_group_.attachObject("object", move_group_.getEndEffectorLink(), touch_links_);
```
Apply orientation constraints to keep the object in the same pose as desired.
Retreat to post-grasp and move to pre-place and the place position.
Open the gripper using the GripperCommandActionGoal Action.
Detach the object from the gripper:
```
move_group_.detachObject("object");
```
Clear all the path constraints as the object is placed.

Disable collisions between the object and support surface, for instance a table:

void PickAndPlace::ignoreCollisions() {
  robot_model_loader::RobotModelLoaderPtr robot_model_loader(
    new robot_model_loader::RobotModelLoader("robot_description"));
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(
    new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader));

  planning_scene_monitor->startSceneMonitor();
  planning_scene_monitor->startWorldGeometryMonitor();
  planning_scene_monitor->startStateMonitor();

  planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor);
  collision_detection::AllowedCollisionMatrix& acm =
    ls->getAllowedCollisionMatrixNonConst();
  acm.setEntry("object", "table", false);  // false disables collisions.

  moveit_msgs::PlanningScene diff_scene;
  diff_scene.robot_state.is_diff = true;
  ls->getPlanningSceneDiffMsg(diff_scene);

  planning_scene_interface_.applyPlanningScene(diff_scene);
}

Retreat to the post-place position after disabling the collision.
Finally, enable collisions between the support surface and the object.

References:

Note: Please let me know if this solution doesn't work out for you. If in case it does, feel free to upvote my answer.