UR5 does not move with object in grasp

edit

Hi,

We have a UR5 robotic arm, and a Robotiq-2f-140 gripper attached to it. We have made a URDF of the arm with the gripper, and used MoveIt Setup Assistant (MSA) to generate a MoveIt configuration package. Gripper links are accounted for when generating self-collision matrix during the MSA wizard. This works fine, and we can plan and execute some simple movements.

For actual grasping, I am running the following command.

roslaunch robotiq_2f_gripper_control robotiq_action_server.launch

Then, within my ROS package, I am using RobotiqActionClient and some APIs to open or close the gripper ( after I have planned moves to the desired locations).

So far, I have been testing this without actually grasping anything, but just checking that movements were ok with open and closing of the gripper working properly.

However, when I tried to actually grasp an object (open gripper close to the object and then close the gripper), then the next move never executes. The robot stays with the object grasped and does not move further.

  GoToHomeJoinState();    // move to joint-goal std::vector<double> homePos{0,-90,90,-90,-90,0};
  ros::Duration(1).sleep();
  GoToFinalJoinState(Convert(75)); // move in joint-space, just moving around base-link by 75 degrees
  ros::Duration(1).sleep();

 // move the gripper down to the target 
 robot_state::RobotStatePtr  kinematic_state = m_MoveGroup->getCurrentState();;
  const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("tool0");
  geometry_msgs::Pose pose;
  tf::poseEigenToMsg(end_effector_state, pose);
  pose.position.z -= 0.06;
  m_MoveGroup->setPoseTarget(pose);
  bool success = (m_MoveGroup->plan(m_MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
  if(!success) {
    return -1;
  }
  m_MoveGroup->move();

   // open gripper
  openGripper();

    ros::Duration(1).sleep();

  // close gripper
  closeGripper();


  ros::Duration(1).sleep();

  // move up the gripper on the z-axis   

  kinematic_state = m_MoveGroup->getCurrentState();
  const Eigen::Affine3d &end_effector_state_01 = kinematic_state->getGlobalLinkTransform("tool0");
  tf::poseEigenToMsg(end_effector_state_01, pose);
  pose.position.z += 0.05;
  m_MoveGroup->setPoseTarget(pose);
  success = (m_MoveGroup->plan(m_MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
  if(!success) {
    return -1;
  }
  m_MoveGroup->move();
  ROS_INFO("completed FOURTH part of the line, moving on to the next .... ");

  // THIS POINT ARM STOPS

  // moving TCP along the x-axis 
  kinematic_state = m_MoveGroup->getCurrentState();
  const Eigen::Affine3d &end_effector_state_02 = kinematic_state->getGlobalLinkTransform("tool0");
  tf::poseEigenToMsg(end_effector_state_02, pose);
  pose.position.x -= 0.5;
  m_MoveGroup->setPoseTarget(pose);
  success = (m_MoveGroup->plan(m_MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
  if(!success) {
    return -1;
  }
  m_MoveGroup->move();

  ROS_INFO("completed FIFTH part of the line, moving on to the next .... ");

  ros::Duration(1).sleep();

Above you can see there is a point in the sequence of movements where the robotic arm just stops and then does not move further. If there was no object grasped, but just a dummy scenario with open and close of gripper, then the whole sequence finished successfully.

thanks, Zahid