昨天想在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
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)