From 926e31b23ff895db0818359c823a0e1d24076d39 Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Sun, 11 Feb 2024 02:56:27 +0100 Subject: [PATCH] adapt tests for use of bullet client wrapper --- tests/envs/test_floating_mia_grasp_env.py | 13 +++++++------ tests/envs/test_parallel_envs.py | 14 +++++++++++--- tests/objects/test_box.py | 3 ++- tests/objects/test_capsule.py | 3 ++- tests/objects/test_cylinder.py | 3 ++- tests/objects/test_insole.py | 3 ++- tests/objects/test_insole_on_conveyor.py | 2 +- tests/objects/test_pillow.py | 2 +- tests/objects/test_sphere.py | 5 ++++- tests/objects/test_urdf_object.py | 8 ++++++-- tests/robots/test_mia_hand_position.py | 2 +- tests/robots/test_mia_hand_velocity.py | 2 +- tests/robots/test_shadow_hand_position.py | 2 +- tests/robots/test_shadow_hand_velocity.py | 2 +- tests/robots/test_ur10_shadow_position.py | 2 +- tests/robots/test_ur10_shadow_velocity.py | 2 +- tests/robots/test_ur5_mia_position.py | 2 +- tests/robots/test_ur5_mia_velocity.py | 2 +- 18 files changed, 46 insertions(+), 26 deletions(-) diff --git a/tests/envs/test_floating_mia_grasp_env.py b/tests/envs/test_floating_mia_grasp_env.py index 7c9a911..663c303 100644 --- a/tests/envs/test_floating_mia_grasp_env.py +++ b/tests/envs/test_floating_mia_grasp_env.py @@ -61,31 +61,32 @@ def test_initial_sensor_info(env: FloatingMiaGraspEnv): def test_episode_reproducibility(): observations = [] termination_flags = [] + actions = [] env = FloatingMiaGraspEnv( verbose=False, - horizon=10, + horizon=3, gui=False, - observable_object_pos=True, object_name="insole_on_conveyor_belt/back", - difficulty_mode="hard", ) - env = RescaleAction(env, 0., 1.) - - env.action_space.seed(SEED) for _ in range(2): observation, _ = env.reset(seed=SEED) + env.action_space.seed(SEED) + observations.append([observation]) terminated = False termination_flags.append([terminated]) + actions.append([]) while not terminated: action = env.action_space.sample() + actions[-1].append(action) observation, reward, terminated, truncated, info = env.step(action) observations[-1].append(observation) termination_flags[-1].append(terminated) + assert_allclose(actions[0], actions[1]) assert_allclose(observations[0], observations[1]) assert_allclose(termination_flags[0], termination_flags[1]) diff --git a/tests/envs/test_parallel_envs.py b/tests/envs/test_parallel_envs.py index 25c790f..76837b6 100644 --- a/tests/envs/test_parallel_envs.py +++ b/tests/envs/test_parallel_envs.py @@ -4,8 +4,16 @@ def test_parallel_envs(): - env = gymnasium.make("FloatingMiaGraspInsole-v0", gui=False) - env2 = gymnasium.make("FloatingMiaGraspInsole-v0", gui=False) + env = gymnasium.make( + "FloatingMiaGraspInsole-v0", + gui=False, + horizon=10 + ) + env2 = gymnasium.make( + "FloatingMiaGraspInsole-v0", + gui=False, + horizon=10 + ) obs, info = env.reset(seed=SEED) num_steps = 0 @@ -26,7 +34,7 @@ def test_parallel_envs(): obs, reward, terminated, truncated, _ = env.step(action) num_steps += 1 - obs2, reward2, terminated2, truncated2, _ = env.step(action2) + obs2, reward2, terminated2, truncated2, _ = env2.step(action2) num_steps2 += 1 episode_return += reward episode_return2 += reward2 diff --git a/tests/objects/test_box.py b/tests/objects/test_box.py index 2505ee4..c628ca5 100644 --- a/tests/objects/test_box.py +++ b/tests/objects/test_box.py @@ -11,7 +11,8 @@ def test_box(simulation): - obj, _, _ = ObjectFactory().create("box", object_position=TEST_POS, object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create( + "box", object_position=TEST_POS, object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0])) diff --git a/tests/objects/test_capsule.py b/tests/objects/test_capsule.py index aafb6a1..062d87d 100644 --- a/tests/objects/test_capsule.py +++ b/tests/objects/test_capsule.py @@ -8,7 +8,8 @@ def test_capsule_creation(simulation): - obj, _, _ = ObjectFactory().create("capsule", object_position=TEST_POS, object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create( + "capsule", object_position=TEST_POS, object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0])) diff --git a/tests/objects/test_cylinder.py b/tests/objects/test_cylinder.py index 1a6851c..f7a4a7c 100644 --- a/tests/objects/test_cylinder.py +++ b/tests/objects/test_cylinder.py @@ -8,7 +8,8 @@ def test_cylinder_creation(simulation): - obj, _, _ = ObjectFactory().create("cylinder", object_position=TEST_POS, object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create( + "cylinder", object_position=TEST_POS, object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0])) diff --git a/tests/objects/test_insole.py b/tests/objects/test_insole.py index 43cb55c..aa4c56a 100644 --- a/tests/objects/test_insole.py +++ b/tests/objects/test_insole.py @@ -11,7 +11,8 @@ def test_insole_creation(simulation): - obj, _, _ = ObjectFactory().create("insole", object_position=TEST_POS, object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create( + "insole", object_position=TEST_POS, object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0]), decimal=1) diff --git a/tests/objects/test_insole_on_conveyor.py b/tests/objects/test_insole_on_conveyor.py index c662e24..7f82884 100644 --- a/tests/objects/test_insole_on_conveyor.py +++ b/tests/objects/test_insole_on_conveyor.py @@ -10,7 +10,7 @@ @pytest.mark.skip("TODO") def test_insole_ob_conveyor_creation(simulation): - obj, _, _ = ObjectFactory().create( + obj, _, _ = ObjectFactory(simulation.pb_client).create( "insole_on_conveyor_belt/back", object_position=TEST_POS, object_orientation=TEST_ORN) diff --git a/tests/objects/test_pillow.py b/tests/objects/test_pillow.py index d73d857..542fbf3 100644 --- a/tests/objects/test_pillow.py +++ b/tests/objects/test_pillow.py @@ -11,7 +11,7 @@ def test_pillow(simulation): - obj, _, _ = ObjectFactory().create("pillow_small", object_position=TEST_POS, object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create("pillow_small", object_position=TEST_POS, object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0]), decimal=1) diff --git a/tests/objects/test_sphere.py b/tests/objects/test_sphere.py index 4f8ca0f..e4c6233 100644 --- a/tests/objects/test_sphere.py +++ b/tests/objects/test_sphere.py @@ -11,7 +11,10 @@ def test_sphere(simulation): - obj, _, _ = ObjectFactory().create("sphere", object_position=TEST_POS, object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create( + "sphere", + object_position=TEST_POS, + object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0])) diff --git a/tests/objects/test_urdf_object.py b/tests/objects/test_urdf_object.py index 179abc2..216c130 100644 --- a/tests/objects/test_urdf_object.py +++ b/tests/objects/test_urdf_object.py @@ -11,8 +11,12 @@ def test_urdf_object_creation(simulation): - obj = UrdfObject("plane.urdf", world_pos=TEST_POS, world_orn=TEST_ORN, fixed=True, - client_id=simulation.get_physics_client_id()) + obj = UrdfObject( + "plane.urdf", + simulation.pb_client, + world_pos=TEST_POS, + world_orn=TEST_ORN, + fixed=True) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0])) diff --git a/tests/robots/test_mia_hand_position.py b/tests/robots/test_mia_hand_position.py index 31e1f85..c3beffa 100644 --- a/tests/robots/test_mia_hand_position.py +++ b/tests/robots/test_mia_hand_position.py @@ -12,7 +12,7 @@ @pytest.fixture def robot(simulation): robot = MiaHandPosition( - pb_client_id=simulation.get_physics_client_id(), + pb_client=simulation.pb_client, world_pos=TEST_POS, world_orn=TEST_ORN, base_commands=True) diff --git a/tests/robots/test_mia_hand_velocity.py b/tests/robots/test_mia_hand_velocity.py index c24d24b..6b6672d 100644 --- a/tests/robots/test_mia_hand_velocity.py +++ b/tests/robots/test_mia_hand_velocity.py @@ -12,7 +12,7 @@ @pytest.fixture def robot(simulation): robot = MiaHandVelocity( - pb_client_id=simulation.get_physics_client_id(), + pb_client=simulation.pb_client, world_pos=TEST_POS, world_orn=TEST_ORN, base_commands=True) diff --git a/tests/robots/test_shadow_hand_position.py b/tests/robots/test_shadow_hand_position.py index 1370de3..9fe7edc 100644 --- a/tests/robots/test_shadow_hand_position.py +++ b/tests/robots/test_shadow_hand_position.py @@ -12,7 +12,7 @@ @pytest.fixture def robot(simulation): robot = ShadowHandPosition( - pb_client_id=simulation.get_physics_client_id(), + pb_client=simulation.pb_client, world_pos=TEST_POS, world_orn=TEST_ORN, base_commands=True) diff --git a/tests/robots/test_shadow_hand_velocity.py b/tests/robots/test_shadow_hand_velocity.py index 7ea92d1..bed9aff 100644 --- a/tests/robots/test_shadow_hand_velocity.py +++ b/tests/robots/test_shadow_hand_velocity.py @@ -12,7 +12,7 @@ @pytest.fixture def robot(simulation): robot = ShadowHandVelocity( - pb_client_id=simulation.get_physics_client_id(), + pb_client=simulation.pb_client, world_pos=TEST_POS, world_orn=TEST_ORN, base_commands=True) diff --git a/tests/robots/test_ur10_shadow_position.py b/tests/robots/test_ur10_shadow_position.py index 6d910b8..d493156 100644 --- a/tests/robots/test_ur10_shadow_position.py +++ b/tests/robots/test_ur10_shadow_position.py @@ -7,7 +7,7 @@ @pytest.fixture def robot(simulation): - robot = UR10ShadowPosition(pb_client_id=simulation.get_physics_client_id()) + robot = UR10ShadowPosition(pb_client=simulation.pb_client) return robot diff --git a/tests/robots/test_ur10_shadow_velocity.py b/tests/robots/test_ur10_shadow_velocity.py index 9ef5d4a..ad8cdac 100644 --- a/tests/robots/test_ur10_shadow_velocity.py +++ b/tests/robots/test_ur10_shadow_velocity.py @@ -7,7 +7,7 @@ @pytest.fixture def robot(simulation): - robot = UR10ShadowVelocity(pb_client_id=simulation.get_physics_client_id()) + robot = UR10ShadowVelocity(pb_client=simulation.pb_client) simulation.add_robot(robot) return robot diff --git a/tests/robots/test_ur5_mia_position.py b/tests/robots/test_ur5_mia_position.py index 1b7e7be..d8b7eba 100644 --- a/tests/robots/test_ur5_mia_position.py +++ b/tests/robots/test_ur5_mia_position.py @@ -7,7 +7,7 @@ @pytest.fixture def robot(simulation): - robot = UR5MiaPosition(pb_client_id=simulation.get_physics_client_id()) + robot = UR5MiaPosition(pb_client=simulation.pb_client) return robot diff --git a/tests/robots/test_ur5_mia_velocity.py b/tests/robots/test_ur5_mia_velocity.py index 24b7b7b..b40b356 100644 --- a/tests/robots/test_ur5_mia_velocity.py +++ b/tests/robots/test_ur5_mia_velocity.py @@ -7,7 +7,7 @@ @pytest.fixture def robot(simulation): - robot = UR5MiaVelocity(simulation.get_physics_client_id()) + robot = UR5MiaVelocity(simulation.pb_client) return robot