Question

我正在使用ROS＆amp; amp; MoveIt !,我编写了自己的拾取和放置功能，根据物体的姿势生成抓握姿势，然后自动拾取。（因为我正在使用挖掘机，所以现在所有的掌握都是从顶部完成的）

该例程的一部分是将夹具旋转至具有<90°的角度。夹具框架和物体框架之间的角度差异，使夹具良好（而不是夹具在边缘处闭合）

我的方法是一旦抓手到达物体 - ＆gt;将对象转换为抓手的框架，然后 - ＆gt;在夹具框架（偏航）中围绕物体的z轴旋转，然后 - >将夹具旋转该值+ <90°

当对象最初围绕z轴和/或x轴（世界框架）旋转但是y轴（世界框架）旋转中的任何旋转都不是所需的并且抓取器时，代码工作正常向下移动时与物体碰撞？我不明白为什么因为我正在转变为抓手框架？有任何想法吗？

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);
 }

ROS :: Gripper和Object之间的方向？

0 个答案: