def test_invert_translations(self):
q = [0, 0, 0, 1]
p = PoseStamped()
p.pose.position = Point(*(1, 0, 0))
p.pose.orientation = Quaternion(*q)
inv = geometry_utils.calculate_inverse_pose(p)
result = geometry_utils.transform_pose(p, inv)
quick_fix = PoseStamped()
quick_fix.pose = result
self.assert_pose_almost_equal(quick_fix, (0, 0, 0), (0, 0, 0, 1))
p = PoseStamped()
p.pose.position = Point(*(0, 2, 0))
p.pose.orientation = Quaternion(*q)
inv = geometry_utils.calculate_inverse_pose(p)
result = geometry_utils.transform_pose(p, inv)
quick_fix = PoseStamped()
quick_fix.pose = result
self.assert_pose_almost_equal(quick_fix, (0, 0, 0), (0, 0, 0, 1))
p = PoseStamped()
p.pose.position = Point(*(0, 0, 3))
p.pose.orientation = Quaternion(*q)
inv = geometry_utils.calculate_inverse_pose(p)
result = geometry_utils.transform_pose(p, inv)
quick_fix = PoseStamped()
quick_fix.pose = result
self.assert_pose_almost_equal(quick_fix, (0, 0, 0), (0, 0, 0, 1))
def test_invert_orientations(self):
v = [0, 0, 0]
# Value of qN for N equal non-zero components
q2 = math.sqrt(1 / 2)
q3 = math.sqrt(1 / 3)
q4 = math.sqrt(1 / 4)
p = PoseStamped()
p.pose.position = Point(*v)
p.pose.orientation = Quaternion(*(q2, 0, 0, q2))
inv = geometry_utils.calculate_inverse_pose(p)
result = geometry_utils.transform_pose(p, inv)
quick_fix = PoseStamped()
quick_fix.pose = result
self.assert_pose_almost_equal(quick_fix, (0, 0, 0), (0, 0, 0, 1))
p = PoseStamped()
p.pose.position = Point(*v)
p.pose.orientation = Quaternion(*(0, q2, 0, q2))
inv = geometry_utils.calculate_inverse_pose(p)
result = geometry_utils.transform_pose(p, inv)
quick_fix = PoseStamped()
quick_fix.pose = result
self.assert_pose_almost_equal(quick_fix, (0, 0, 0), (0, 0, 0, 1))
p = PoseStamped()
p.pose.position = Point(*v)
p.pose.orientation = Quaternion(*(0, 0, q2, q2))
inv = geometry_utils.calculate_inverse_pose(p)
result = geometry_utils.transform_pose(p, inv)
quick_fix = PoseStamped()
quick_fix.pose = result
self.assert_pose_almost_equal(quick_fix, (0, 0, 0), (0, 0, 0, 1))
p = PoseStamped()
p.pose.position = Point(*v)
p.pose.orientation = Quaternion(*(q3, q3, q3, 0))
inv = geometry_utils.calculate_inverse_pose(p)
result = geometry_utils.transform_pose(p, inv)
quick_fix = PoseStamped()
quick_fix.pose = result
self.assert_pose_almost_equal(quick_fix, (0, 0, 0), (0, 0, 0, 1))
p = PoseStamped()
p.pose.position = Point(*v)
p.pose.orientation = Quaternion(*(q4, q4, q4, q4))
inv = geometry_utils.calculate_inverse_pose(p)
result = geometry_utils.transform_pose(p, inv)
quick_fix = PoseStamped()
quick_fix.pose = result
self.assert_pose_almost_equal(quick_fix, (0, 0, 0), (0, 0, 0, 1))
def test_invert_identity(self):
p1 = PoseStamped()
v = (0, 0, 0)
q = (0, 0, 0, 1)
p1.pose.position = Point(*v)
p1.pose.orientation = Quaternion(*q)
inv = geometry_utils.calculate_inverse_pose(p1)
self.assert_pose_almost_equal(inv, (0, 0, 0), (0, 0, 0, 1))
def test_invert_a_random_pose(self):
q = [0.1262852, 0.1261165, 0.2100786, 0.9612563]
v = [random.random() * 10 for i in range(0, 3)]
p = PoseStamped()
p.pose.position = Point(*v)
p.pose.orientation = Quaternion(*q)
inv = geometry_utils.calculate_inverse_pose(p)
result = geometry_utils.transform_pose(p, inv)
quick_fix = PoseStamped()
quick_fix.pose = result
self.assert_pose_almost_equal(quick_fix, (0, 0, 0), (0, 0, 0, 1))
def test_invert_many_random_poses(self):
q_base = [0.1262852, 0.1261165, 0.2100786,
0.9612563] # From random converter online
for i in range(0, 5):
random.shuffle(q_base)
q = tuple(q_base)
v = [(random.random() - 0.5) * 10 for i in range(0, 3)]
p = PoseStamped()
p.pose.position = Point(*v)
p.pose.orientation = Quaternion(*q)
inv = geometry_utils.calculate_inverse_pose(p)
result = geometry_utils.transform_pose(p, inv)
quick_fix = PoseStamped()
quick_fix.pose = result
self.assert_pose_almost_equal(quick_fix, (0, 0, 0), (0, 0, 0, 1))