From 108041c2a6984bd900ed932feeff27a8763a64c4 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 5 Nov 2024 10:01:01 +0100 Subject: [PATCH] [MultiverseTest] Test attach with robot with and without coincide. --- test/test_multiverse.py | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 9eab647bf..26c54a09a 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -356,18 +356,26 @@ def test_detach_object(self): self.multiverse.conf.position_tolerance) self.tearDown() - def test_attach_with_robot(self): - milk = self.spawn_milk([-1, -1, 0.1]) + def test_attach_with_robot_with_coincide(self): + self.generic_attach_with_robot(True) + + def test_attach_with_robot_without_coincide(self): + self.generic_attach_with_robot(False) + + def generic_attach_with_robot(self, coincide: bool): + milk = self.spawn_milk([-1, -1, 2]) robot = self.spawn_robot() + joint_name = "arm_right_2_joint" + robot.set_joint_position(joint_name, 0) ee_link = self.multiverse.get_arm_tool_frame_link(Arms.RIGHT) # Get position of milk relative to robot end effector - robot.attach(milk, ee_link.name) + robot.attach(milk, ee_link.name, coincide_the_objects=coincide) self.assertTrue(robot in milk.attachments) milk_initial_pose = milk.root_link.get_pose_wrt_link(ee_link) - robot_position = 1.57 - robot.set_joint_position("arm_right_2_joint", robot_position) - milk_pose = milk.root_link.get_pose_wrt_link(ee_link) - self.assert_poses_are_equal(milk_initial_pose, milk_pose) + for joint_pos in [0, 0.5, 1]: + robot.set_joint_position(joint_name, joint_pos) + milk_pose = milk.root_link.get_pose_wrt_link(ee_link) + self.assert_poses_are_equal(milk_initial_pose, milk_pose) def test_get_object_contact_points(self): for i in range(10):