机器人抓取 Grasp Pose Detection (GPD) grasp类型转ROS的标准pose

最新推荐文章于 2025-01-21 09:52:49 发布

二分掌柜的

最新推荐文章于 2025-01-21 09:52:49 发布

阅读量2.4k

点赞数 3

CC 4.0 BY-SA版权

分类专栏： # 机器人编程

本文链接：https://blog.csdn.net/flyfish1986/article/details/86245823

机器人编程专栏收录该内容

32 篇文章

订阅专栏

机器人抓取 Grasp Pose Detection (GPD) grasp类型转ROS的标准pose

flyfish

问题：原格式包含众多的Position，Orientation 是三个向量，应该怎么用哪个？

# This message describes a 2-finger grasp configuration by its 6-DOF pose, 
# consisting of a 3-DOF position and 3-DOF orientation, and the opening 
# width of the robot hand.

# Position
geometry_msgs/Point bottom # centered bottom/base of the robot hand)
geometry_msgs/Point top # centered top/fingertip of the robot hand)
geometry_msgs/Point surface # centered position on object surface

# Orientation represented as three axis (R = [approach binormal axis])
geometry_msgs/Vector3 approach # The grasp approach direction
geometry_msgs/Vector3 binormal # The binormal
geometry_msgs/Vector3 axis # The hand axis

geometry_msgs/Point sample # Point at which the grasp was found

std_msgs/Float32 width # Required aperture (opening width) of the robot hand 

std_msgs/Float32 score # Score assigned to the grasp by the classifier

标准的Pose

geometry_msgs/Point position
geometry_msgs/Quaternion orientation

3个向量就是3×3的矩阵，Point使用的top

geometry_msgs::msg::Pose  grasp_to_pose(gpd::GraspConfig & grasp)
{

  geometry_msgs::msg::Pose pose;
  tf2::Matrix3x3 orientation(
    grasp.approach.x, grasp.binormal.x, grasp.axis.x,
    grasp.approach.y, grasp.binormal.y, grasp.axis.y,
    grasp.approach.z, grasp.binormal.z, grasp.axis.z);

  tf2::Quaternion orientation_quat;
  orientation.getRotation(orientation_quat);
  orientation_quat.normalize();
  pose.orientation.x = orientation_quat.x();
  pose.orientation.y = orientation_quat.y();
  pose.orientation.z = orientation_quat.z();
  pose.orientation.w = orientation_quat.w();

  pose.position = grasp.top;

  return pose;
}