ROS :: Ориентация между захватом и объектом? - PullRequest
Новая
       101

ROS :: Ориентация между захватом и объектом?

0 голосов
/ 23 мая 2018

Я работаю над проектом выбора и размещения, используя ROS & MoveIt !, я написал свои собственные функции выбора и размещения, которые генерируют позы захвата в соответствии с позицией объекта, а затем автоматически выбирают.(все захваты на данный момент выполняются сверху, так как я работаю с экскаватором)

Одной частью процедуры является вращение захвата для получения разницы углов <90 градусов> между рамой захвата и рамой объектатак что сцепление хорошее (вместо того, чтобы захват был близко к краям)

Мой подход - когда захват достигает объекта -> трансформировать объект в рамку захвата, а затем -> получить вращение вокругось z объекта в рамке захвата (рыскание), а затем -> повернуть захват на это значение + <90 град>

Код работает нормально, когда объект первоначально вращается вокруг оси z и/ или ось х (мировой рамы), но любое вращение по оси у (мировой рамы) не является желательным, и захват движется с объектом при движении вниз?Я не понимаю, почему, так как я все равно превращаюсь в рамку захвата?есть идеи ?

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