PR2 Gripper is not opening after pick

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

Tawfiq Chowdhury gravatar image

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

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

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 -0500 )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 -0500 )edit