Merge pull request #23 from dfki-ric/fix/sensor-resetting-bug

Fix issue #16, added corresponding tests

deformable_gym/envs/base_env.py

@@ -93,6 +93,8 @@ def reset(self, seed=None, options=None) -> npt.ArrayLike:
         self.step_counter = 0
         self.simulation.timing.reset()
 
+        self.simulation.simulate_time(0.0001)
+
         observation = self._get_observation()
         info = self._get_info()
 

deformable_gym/envs/floating_mia_grasp_env.py

@@ -122,7 +122,9 @@ def __init__(
                  np.ones(4) * .000005,  # max positive base orn offset
                  limits[1][self.actuated_finger_ids]]  # positive joint limits
 
-        self.action_space = spaces.Box(low=np.concatenate(lower), high=np.concatenate(upper), dtype=np.float64)
+        self.action_space = spaces.Box(low=np.concatenate(lower),
+                                       high=np.concatenate(upper),
+                                       dtype=np.float64)
 
     def _create_robot(self):
         orn_limit = None

tests/envs/test_floating_mia_grasp_env.py

@@ -1,3 +1,4 @@
+from numpy.testing import assert_allclose
 import pytest
 from deformable_gym.envs.floating_mia_grasp_env import FloatingMiaGraspEnv
 
@@ -8,20 +9,19 @@ def env():
         gui=False,
         verbose=True,
         horizon=10,
-        object_name="insole_on_conveyor_belt/back",
-        observable_object_pos=True,
-        difficulty_mode="hard")
+        object_name="insole_on_conveyor_belt/back")
 
 
 action_space_dims_expected = 10
+SEED = 42
 
 
-def test_action_space_dims(env):
+def test_action_space_dims(env: FloatingMiaGraspEnv):
     action_space = env.action_space
     assert action_space.shape[0] == action_space_dims_expected
 
 
-def test_obs_space_dims(env):
+def test_obs_space_dims(env: FloatingMiaGraspEnv):
     if env._observable_object_pos:
         obs_space_dims_expected = 19
     else:
@@ -31,21 +31,39 @@ def test_obs_space_dims(env):
     assert obs_space.shape[0] == obs_space_dims_expected
 
 
-def test_initial_obs(env):
-    obs, info = env.reset()
+def test_initial_obs(env: FloatingMiaGraspEnv):
+    obs, info = env.reset(seed=SEED)
     if env._observable_object_pos:
         obs_space_dims_expected = 19
     else:
         obs_space_dims_expected = 16
     assert len(obs) == obs_space_dims_expected
 
 
-def test_eps_done(env):
+def test_initial_sensor_info(env: FloatingMiaGraspEnv):
+    sensor_readings = []
+    env.action_space.seed(SEED)
+
+    for _ in range(2):
+        observation, _ = env.reset(seed=SEED)
+        sensor_readings.append(observation)
+
+        n_steps = 5
+        for _ in range(n_steps):
+            action = env.action_space.sample()
+            env.step(action)
+
+    assert_allclose(sensor_readings[0], sensor_readings[1])
+
+
+def test_eps_done(env: FloatingMiaGraspEnv):
     if env._observable_object_pos:
         obs_space_dims_expected = 19
     else:
         obs_space_dims_expected = 16
 
+    env.action_space.seed(SEED)
+
     for t in range(9):
         action = env.action_space.sample()
         obs, reward, terminated, truncated, info = env.step(action)

tests/envs/test_floating_shadow_grasp_env.py

@@ -1,5 +1,8 @@
 import pytest
 from deformable_gym.envs.floating_shadow_grasp_env import FloatingShadowGraspEnv
+from numpy.testing import assert_allclose
+
+SEED = 42
 
 
 @pytest.fixture
@@ -47,3 +50,18 @@ def test_ep_termination(env):
 
     assert terminated
 
+
+def test_initial_sensor_info(env: FloatingShadowGraspEnv):
+    sensor_readings = []
+    env.action_space.seed(SEED)
+
+    for _ in range(2):
+        observation, _ = env.reset(seed=SEED)
+        sensor_readings.append(observation)
+
+        n_steps = 5
+        for _ in range(n_steps):
+            action = env.action_space.sample()
+            env.step(action)
+
+    assert_allclose(sensor_readings[0], sensor_readings[1])

tests/envs/test_ur5_mia_grasp_env.py

@@ -9,7 +9,6 @@ def env():
         verbose=True,
         horizon=10,
         object_name="insole",
-        #observable_object_pos=True,
     )
 
 
@@ -18,6 +17,7 @@ def test_action_space_dims(env):
     action_space = env.action_space
     assert action_space.shape[0] == 10
 
+
 @pytest.mark.skip("TODO")
 def test_obs_space_dims(env):
     obs_space = env.observation_space