def test_mv_quaternion_array():
"""
:rtype: WorldObject
"""
amount = 6
m = np.random.rand(4, amount)
m = m / np.linalg.norm(m, axis=0)
m = m.T
m1 = np.random.rand(3, amount)
m1 = m1.T
res = utils.qv_mult_array(m, m1)
for i in range(m1.shape[0]):
np.testing.assert_array_almost_equal(utils.qv_mult(m[i], m1[i]), res[i])
def pub_goal_marker(self, header, pose):
"""
:param header:
:type header: std_msgs.msg._Header.Header
:param pose:
:type pose: Pose
"""
ma = MarkerArray()
m = Marker()
m.action = Marker.ADD
m.type = Marker.CYLINDER
m.header = header
old_q = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
# x
m.pose = deepcopy(pose)
m.scale.x = 0.05 * MARKER_SCALE
m.scale.y = 0.05 * MARKER_SCALE
m.scale.z = MARKER_SCALE
muh = qv_mult(old_q, [m.scale.z / 2, 0, 0])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.pose.orientation = Quaternion(*quaternion_multiply(
old_q, quaternion_about_axis(np.pi / 2, [0, 1, 0])))
m.color.r = 1
m.color.g = 0
m.color.b = 0
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 0
ma.markers.append(m)
# y
m = deepcopy(m)
m.pose = deepcopy(pose)
muh = qv_mult(old_q, [0, m.scale.z / 2, 0])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.pose.orientation = Quaternion(*quaternion_multiply(
old_q, quaternion_about_axis(-np.pi / 2, [1, 0, 0])))
m.color.r = 0
m.color.g = 1
m.color.b = 0
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 1
ma.markers.append(m)
# z
m = deepcopy(m)
m.pose = deepcopy(pose)
muh = qv_mult(old_q, [0, 0, m.scale.z / 2])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.color.r = 0
m.color.g = 0
m.color.b = 1
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 2
ma.markers.append(m)
self.marker_pub.publish(ma)