1
$\begingroup$

I am working on a pick and place project using ROS & MoveIt!, I wrote my own pick and place functions which generate grasp poses according to the pose of the object and then automatically picks. (all grasps for now are done from the top since I am working with an excavator)

One part of the routine is rotating the gripper to have a <90 deg> angle difference between the gripper frame and the object frame so that the grip is good (instead of having the gripper close at edges)

My approach is once the gripper reaches the object --> transform the object into the gripper's frame and then --> get the rotation around z-axis of the object in the gripper frame (yaw) and then --> rotate the gripper by this value + <90 deg>

The code works fine when the object is originally rotated around z-axis and/or x-axis (of the world frame) but any rotation in y-axis (of the world frame) rotation is not as desired and the gripper collides with the object when moving downwards ? I don't understand why since I am transforming to gripper frame anyway ? any ideas ?

bool akit_pick_place::rotateGripper(moveit_msgs::CollisionObject object_){ //needs adjusting (rotation in y-axis has problems)

  geometry_msgs::PoseStamped object_in_world_frame, object_in_gripper_frame;
  object_in_world_frame.pose = object_.primitive_poses[0];
  object_in_world_frame.header.frame_id = object_.header.frame_id;

  //transform object from world frame to gripper rotator frame
  transform_listener.waitForTransform("gripper_rotator", WORLD_FRAME, ros::Time::now(), ros::Duration(0.1)); //avoid time difference exceptions
  transform_listener.transformPose("gripper_rotator",ros::Time(0), object_in_world_frame, WORLD_FRAME, object_in_gripper_frame);

  //get roll, pitch, yaw between object frame and gripper frame
  tf::Quaternion qq(object_in_gripper_frame.pose.orientation.x, object_in_gripper_frame.pose.orientation.y,
                    object_in_gripper_frame.pose.orientation.z, object_in_gripper_frame.pose.orientation.w);
  tf::Matrix3x3 m(qq);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
  ROS_INFO_STREAM("roll: " << roll << " pitch: " << pitch << "yaw: " << yaw);
  //account for angles in different quadrants 
  if (yaw <= 0.0){
    gripperJointPositions[0] = (M_PI/2) + yaw;
  } else {
    gripperJointPositions[0] =  yaw - (M_PI/2);
  }

  gripperGroup->setJointValueTarget(gripperJointPositions);

  gripperSuccess = (gripperGroup->plan(gripperMotionPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  gripperSuccess = (gripperGroup->execute(gripperMotionPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_STREAM("Gripper Motion Plan: " << (gripperSuccess ? "Rotated gripper" : "FAILED TO ROTATE GRIPPER"));
  return (gripperSuccess ? true : false);
 }
$\endgroup$

Your Answer

By clicking "Post Your Answer", you acknowledge that you have read our updated terms of service, privacy policy and cookie policy, and that your continued use of the website is subject to these policies.

Browse other questions tagged or ask your own question.