如何设置机械臂末端朝向目标物体

存在的问题

机械臂运动到预采摘位置时，末端朝向与目标物体不一致。

解决方法

RPY全部设置为0。从下图可以看出，Link6和base_link是简单的平移关系，无需旋转。

roll=0, pitch=1.57， yaw=0.0，末端水平放置。

在水平面上与目标物体对齐，计算目标物体在base_link下与x轴的夹角(弧度)，然后转成四元数，赋值给末端的pose。