ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

PR2 Gripper is not opening after pick

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

Tawfiq Chowdhury gravatar image

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?

edit retag flag offensive close merge delete

Comments

1

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 gravatar image 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 gravatar image Tawfiq Chowdhury  ( 2020-02-14 12:47:28 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

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

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 more

Question Tools

3 followers

Stats

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

Seen: 159 times

Last updated: Dec 01 '22