待抓取位姿转换到世界坐标系下

抓取位姿变换

位置转换

在机器人抓取中，涉及到抓取点和姿态转换到世界坐标系下，其中用p(x,y,z) 和 r 来表示目标物体的点和姿态。
在转换过程中，点p从相机坐标系转换到世界坐标系，可以直接用ros中tf树可以直接转换，程序如下

• 知道世界坐标系和相机坐标系名称，以及要转换的点（手动解析是: Pb = R*Pa + T, R是旋转矩阵，T是平移矩阵）

# 建立tf监听
buffer = tf2_ros.Buffer()  # 创建缓存对象
sub = tf2_ros.TransformListener(buffer)  # 创建订阅对象，将缓存传入

# 假定cam_point 是相机坐标系下的点
cam_point = tf2_geometry_msgs.PointStamped()  # 组织被转换的坐标点
cam_point.header.stamp = rospy.Time.now()  # 时间戳
cam_point.header.frame_id = "kinect2_rgb_optical_frame"  # 相机坐标系参考点   kinect2_ir_optical_frame
cam_point.point.x = 0.8
cam_point.point.y = 0.8
cam_point.point.z = 0.8

# 将相机坐标下的点转换到世界坐标系下
transformstamped = buffer.lookup_transform("world", "kinect2_ir_optical_frame", rospy.Time(0), rospy.Duration(5))
world_point = buffer.transform(cam_point, "world")
print("world_point: %f %f %f", world_point.point.x, world_point.point.y, world_point.point.z)

姿态转换

• 知道世界坐标系和相机坐标系关系，知道目标物体的姿态求解得到在世界坐标系下的姿态
  等同于点的转换，这里直接用手动转换，相当于就是姿态在 R 下求解变换

# R_res 表示世界坐标系下的姿态
# R_cam_world 表示 相机到世界的转换关系，
# R_obj_cam 表示 目标物体在相机坐标系下的旋转姿态
# 本质就是手动乘法 R_res = R_cam_world * R_obj_cam  （因为用tf自带的矩阵乘法有问题，才自己手算）
R_res = tf.transformations.quaternion_matrix([1,0,0,0])
R_res[0][0] = R_cam_world[0][0] * R_obj_cam[0][0] + R_cam_world[0][1] * R_obj_cam[1][0] + R_cam_world[0][2] * R_obj_cam[2][0]
R_res[0][1] = R_cam_world[0][0] * R_obj_cam[0][1] + R_cam_world[0][1] * R_obj_cam[1][1] + R_cam_world[0][2] * R_obj_cam[2][1]
R_res[0][2] = R_cam_world[0][0] * R_obj_cam[0][2] + R_cam_world[0][1] * R_obj_cam[1][2] + R_cam_world[0][2] * R_obj_cam[2][2]

R_res[1][0] = R_cam_world[1][0] * R_obj_cam[0][0] + R_cam_world[1][1] * R_obj_cam[1][0] + R_cam_world[1][2] * R_obj_cam[2][0]
R_res[1][1] = R_cam_world[1][0] * R_obj_cam[0][1] + R_cam_world[1][1] * R_obj_cam[1][1] + R_cam_world[1][2] * R_obj_cam[2][1]
R_res[1][2] = R_cam_world[1][0] * R_obj_cam[0][2] + R_cam_world[1][1] * R_obj_cam[1][2] + R_cam_world[1][2] * R_obj_cam[2][2]

R_res[2][0] = R_cam_world[2][0] * R_obj_cam[0][0] + R_cam_world[2][1] * R_obj_cam[1][0] + R_cam_world[2][2] * R_obj_cam[2][0]
R_res[2][1] = R_cam_world[2][0] * R_obj_cam[0][1] + R_cam_world[2][1] * R_obj_cam[1][1] + R_cam_world[2][2] * R_obj_cam[2][1]
R_res[2][2] = R_cam_world[2][0] * R_obj_cam[0][2] + R_cam_world[2][1] * R_obj_cam[1][2] + R_cam_world[2][2] * R_obj_cam[2][2]

# 旋转矩阵 R 、 四元素 Q 和 欧拉角相互转换
R_cam_world = tf.transformations.quaternion_matrix(Q_cam_world) # Q convert R
rpy = tf.transformations.euler_from_matrix(R_res)  # R convert rpy
Q = quaternion_from_euler(rpy[0], rpy[1], rpy[2])  # rpy convert Q