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

昨天想在rviz里用pose表示一个，搞了半天。
Pose的属性，point是起点，是表示由坐标系原点将[1,0,0]这个向量旋转到想要显示的方向所需的旋转的四元数表示。
假如你有空间中的两个点 A(x1,y1,z1) 和 B(x2,y2,z2)，想要表示由A出发指向B的向量，则Pose的Point = A 。
然后计算的四元数，及将向量[1,0,0]旋转到向量AB方向的四元数。
= AB/|AB| //单位向量
r = [1,0,0]X //旋转轴为叉乘方向np.cross()
r = r/|r| //单位向量
Theta = (r*[1,0,0]) //旋转角
四元数公式为

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

# 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 Arhh_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))

四元数（）和旋转 +欧拉角 -- 博客园
【ROS - 用POSE展示方向或向量 vector - pose】/Pose