PR2 Gripper is not opening after pick

asked 2019-08-07 15:18:11 -0600

Tawfiq Chowdhury
137 ●31 ●41 ●46

updated 2019-08-07 20:54:46 -0600

I am trying to open the gripper of the PR2 after a pick is completed, I tried to do a place, it did not work, I tried only to open the gripper, that did not work either. Here is my code where I am trying to open the gripper after a pick is completed. I am getting the grasp pose from a service call.

#include <ros/ros.h>
#include "pr2_package/PoseSend.h"

// MoveIt!
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/move_group_interface/move_group.h>
#include <geometric_shapes/solid_primitive_dims.h>

static const std::string ROBOT_DESCRIPTION="robot_description";

void openGripper(trajectory_msgs::JointTrajectory& posture){
  posture.joint_names.resize(6);
  posture.joint_names[0] = "r_gripper_joint";
  posture.joint_names[1] = "r_gripper_motor_screw_joint";
  posture.joint_names[2] = "r_gripper_l_finger_joint";
  posture.joint_names[3] = "r_gripper_r_finger_joint";
  posture.joint_names[4] = "r_gripper_r_finger_tip_joint";
  posture.joint_names[5] = "r_gripper_l_finger_tip_joint";

  posture.points.resize(1);
  posture.points[0].positions.resize(6);
  posture.points[0].positions[0] = 1;
  posture.points[0].positions[1] = 1.0;
  posture.points[0].positions[2] = 0.477;
  posture.points[0].positions[3] = 0.477;
  posture.points[0].positions[4] = 0.477;
  posture.points[0].positions[5] = 0.477;
}

void closedGripper(trajectory_msgs::JointTrajectory& posture){
  posture.joint_names.resize(6);
  posture.joint_names[0] = "r_gripper_joint";
  posture.joint_names[1] = "r_gripper_motor_screw_joint";
  posture.joint_names[2] = "r_gripper_l_finger_joint";
  posture.joint_names[3] = "r_gripper_r_finger_joint";
  posture.joint_names[4] = "r_gripper_r_finger_tip_joint";
  posture.joint_names[5] = "r_gripper_l_finger_tip_joint";

  posture.points.resize(1);
  posture.points[0].positions.resize(6);
  posture.points[0].positions[0] = 0;
  posture.points[0].positions[1] = 0;
  posture.points[0].positions[2] = 0.002;
  posture.points[0].positions[3] = 0.002;
  posture.points[0].positions[4] = 0.002;
  posture.points[0].positions[5] = 0.002;
}



void pick(moveit::planning_interface::MoveGroup &group, geometry_msgs::PoseStamped p)
{
  std::vector<moveit_msgs::Grasp> grasps;
  moveit_msgs::Grasp g;

  g.pre_grasp_approach.direction.vector.x = 1.0;
  g.pre_grasp_approach.direction.header.frame_id = "r_wrist_roll_link";
  g.pre_grasp_approach.min_distance = 0.2;
  g.pre_grasp_approach.desired_distance = 0.4;

  g.post_grasp_retreat.direction.header.frame_id = "base_footprint";
  g.post_grasp_retreat.direction.vector.z = 1.0;
  g.post_grasp_retreat.min_distance = 0.1;
  g.post_grasp_retreat.desired_distance = 0.25;
  g.grasp_pose = p;

  openGripper(g.pre_grasp_posture);

  closedGripper(g.grasp_posture);
  //openGripper(g.pre_grasp_posture);

  grasps.push_back(g);
  //openGripper(g.pre_grasp_posture);
  group.pick("part", grasps);

  ros::WallDuration(1.0).sleep();

  openGripper(g.pre_grasp_posture);

}


bool serv(pr2_package::PoseSend::Request &req, 
  pr2_package::PoseSend::Response &res){
  ROS_INFO("Ready for service call");
  ros::WallDuration(1.0).sleep();
  moveit::planning_interface::MoveGroup group("right_arm");
  group.setPlanningTime(45.0);
  ros::WallDuration(1.0).sleep();
  geometry_msgs::PoseStamped p;
  p.header.frame_id = "odom_combined";
  p.pose.position.x = (float)req.a;
  p.pose.position.y = (float)req.b;
  p.pose.position.z = (float)req.c;
  p.pose.orientation.x = (float)req.d;
  p.pose.orientation.y = (float)req.e;
  p.pose.orientation.z = (float)req.f;
  p.pose.orientation.w = (float)req.g;
  pick(group, p);

  return true;
}

int main(int argc, char **argv)
{
  ros::init (argc, argv, "right_arm_pick_place");
  //ros::AsyncSpinner spinner(1);
  //spinner.start();

  ros::NodeHandle nh;

  //ros::WallDuration(1.0).sleep();
  ros::ServiceServer service = nh.advertiseService("Pose_Send", serv);
  //ros::WallDuration(1.0).sleep();

  ROS_INFO("Ready For Client Call");

  //ros::waitForShutdown();
  ros::spin();
  return 0;
}

Any suggestions?

Comments

Hi, did you resolved your issue ? I am also trying to move the gripper with moveit at a joint level. I can control the gripper with GripperCommand value (position, max effort) but can't manage to control it with joint. Joint control of gripper is necessary to be able to use Pick place pipeline but also move it task constructor which make grasping easier.

Madcreator ( 2020-02-07 02:56:30 -0600 )edit

@Madcreator, it will work as the way pick function is defined, you cannot just open/close it by calling the open/close function.

Tawfiq Chowdhury ( 2020-02-14 12:47:28 -0600 )edit

add a comment

answered 2022-12-01 11:30:30 -0600

LSD

98 ●6 ●8 https://github.com/lea...

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 ...

(more)

edit flag offensive delete link

add a comment

PR2 Gripper is not opening after pick

Comments

1 Answer

MoveIt! Pick and Place Pipeline

Fix 1 - Hacking the pipeline

Fix 2 - Setting up your pipeline

Question Tools

Stats

Related questions

PR2 Gripper is not opening after pick edit

Comments

1 Answer

MoveIt! Pick and Place Pipeline

Fix 1 - Hacking the pipeline

Fix 2 - Setting up your pipeline

Question Tools

Stats

Related questions

PR2 Gripper is not opening after pick