ROS - 用POSE展示方向或向量 vector - pose

昨天想在rviz里用pose表示一个vector，搞了半天。

Pose的属性，point是起点，orientation是表示由坐标系原点将[1,0,0]这个向量旋转到想要显示的方向所需的旋转的四元数表示。

假如你有空间中的两个点 A(x1,y1,z1) 和 B(x2,y2,z2)，想要表示由A出发指向B的向量，则Pose的Point = A。

然后计算orientation的四元数，及将向量[1,0,0]旋转到向量AB方向的四元数。

target_v = AB/|AB| //单位向量

r = [1,0,0] X target_v X //旋转轴为叉乘方向np.cross()

r = r/|r| //单位向量

Theta = arccos(r*[1,0,0]) //旋转角

四元数公式为

 其中(x,y,z)为旋转轴方向上的单位向量。代入即可得到Pose的orientation。

# A(x,y,z) B(x1,y1,z1)
direction.position = Point(x,y,z)
rhh_v = np.asarray([x-x1,y-y1,z-z1]) //determine A to B or B to A
rhh_v = rhh_v/np.linalg.norm(rhh_v)
rhh_n = np.cross(np.asarray([1,0,0]),rhh_v)
rhh_n = rhh_n/np.linalg.norm(rhh_n)
rhh_tht = np.arccos(rhh_v.dot(np.asarray([1,0,0])))
direction.orientation = Quaternion(x=np.sin(rhh_tht/2)*rhh_n[0], y=np.sin(rhh_tht/2)*rhh_n[1], z=np.sin(rhh_tht/2)*rhh_n[2], w=np.cos(rhh_tht/2))

references

四元数（Quaternion）和旋转 +欧拉角 - Jerry_Jin - 博客园

geometry_msgs/Pose Documentation