def test_identity_transform(self):
wrench_in = geometry_msgs.msg.WrenchStamped()
transform = numpy.identity(4)
wrench_in.wrench.force.x = 0.0
wrench_in.wrench.force.y = 0.0
wrench_in.wrench.force.z = 0.0
wrench_in.wrench.torque.x = 0.0
wrench_in.wrench.torque.y = 0.0
wrench_in.wrench.torque.z = 0.0
w = geometry_transformer_util.transform_wrench(transform, wrench_in)
self.assertEquals(0.0, w.wrench.force.x)
self.assertEquals(0.0, w.wrench.force.y)
self.assertEquals(0.0, w.wrench.force.z)
self.assertEquals(0.0, w.wrench.torque.x)
self.assertEquals(0.0, w.wrench.torque.y)
self.assertEquals(0.0, w.wrench.torque.z)
wrench_in.wrench.force.x = 1.0
wrench_in.wrench.force.y = 2.0
wrench_in.wrench.force.z = 3.0
wrench_in.wrench.torque.x = 4.0
wrench_in.wrench.torque.y = 5.0
wrench_in.wrench.torque.z = 6.0
w = geometry_transformer_util.transform_wrench(transform, wrench_in)
self.assertEquals(1.0, w.wrench.force.x)
self.assertEquals(2.0, w.wrench.force.y)
self.assertEquals(3.0, w.wrench.force.z)
self.assertEquals(4.0, w.wrench.torque.x)
self.assertEquals(5.0, w.wrench.torque.y)
self.assertEquals(6.0, w.wrench.torque.z)
def transformWrench(self, wrench_in, target_frame="base_link"):
'''
Transform the provided wrench to the target frame. Throws an exception
if the transformation failed.
:param wrench_in: The wrench which should be transformed.
:type wrench_in: geometry_msgs.msg.WrenchStamped]
:param target_frame: The frame into which the wrench should be
transformed.
:type target_frame: String
:return: The transformed wrench.
:rtype: geometry_msgs.msg.WrenchStamped
'''
self.listener.waitForTransform(target_frame, wrench_in.header.frame_id,
wrench_in.header.stamp,
rospy.Duration(5.0))
t = self.listener.asMatrix(target_frame, wrench_in.header)
wrench_out = geometry_msgs.msg.WrenchStamped()
wrench_out = geometry_transformer_util.transform_wrench(t, wrench_in)
wrench_out.header.frame_id = target_frame
wrench_out.header.stamp = wrench_in.header.stamp
return wrench_out
def test_translation_transform(self):
wrench_in = geometry_msgs.msg.WrenchStamped()
transform = numpy.identity(4)
transform[0, 3] = 1.0
transform[1, 3] = 2.0
transform[2, 3] = 3.0
wrench_in.wrench.force.x = 0.0
wrench_in.wrench.force.y = 0.0
wrench_in.wrench.force.z = 0.0
wrench_in.wrench.torque.x = 0.0
wrench_in.wrench.torque.y = 0.0
wrench_in.wrench.torque.z = 0.0
w = geometry_transformer_util.transform_wrench(transform, wrench_in)
self.assertEquals(0.0, w.wrench.force.x)
self.assertEquals(0.0, w.wrench.force.y)
self.assertEquals(0.0, w.wrench.force.z)
self.assertEquals(0.0, w.wrench.torque.x)
self.assertEquals(0.0, w.wrench.torque.y)
self.assertEquals(0.0, w.wrench.torque.z)
transform[0, 3] = 1.0
transform[1, 3] = 0.0
transform[2, 3] = 0.0
wrench_in.wrench.force.x = 0.0
wrench_in.wrench.force.y = 1.0
wrench_in.wrench.force.z = 0.0
wrench_in.wrench.torque.x = 0.0
wrench_in.wrench.torque.y = 0.0
wrench_in.wrench.torque.z = 0.0
w = geometry_transformer_util.transform_wrench(transform, wrench_in)
self.assertEquals(0.0, w.wrench.force.x)
self.assertEquals(1.0, w.wrench.force.y)
self.assertEquals(0.0, w.wrench.force.z)
self.assertEquals(0.0, w.wrench.torque.x)
self.assertEquals(0.0, w.wrench.torque.y)
self.assertEquals(1.0, w.wrench.torque.z)
def test_orientation_roll_transform(self):
wrench_in = geometry_msgs.msg.WrenchStamped()
angle = math.pi / 2.0
transform = numpy.array([
[1, 0, 0, 0],
[0, math.cos(angle), -math.sin(angle), 0],
[0, math.sin(angle), math.cos(angle), 0],
[0, 0, 0, 1]])
wrench_in.wrench.force.x = 1.0
wrench_in.wrench.force.y = 0.0
wrench_in.wrench.force.z = 0.0
wrench_in.wrench.torque.x = 1.0
wrench_in.wrench.torque.y = 0.0
wrench_in.wrench.torque.z = 0.0
w = geometry_transformer_util.transform_wrench(transform, wrench_in)
self.assertEquals(1.0, w.wrench.force.x)
self.assertEquals(0.0, w.wrench.force.y)
self.assertEquals(0.0, w.wrench.force.z)
self.assertEquals(1.0, w.wrench.torque.x)
self.assertEquals(0.0, w.wrench.torque.y)
self.assertEquals(0.0, w.wrench.torque.z)