Merge pull request #38 from dfki-ric/fix/default-values

double check default values

deformable_gym/envs/base_env.py

@@ -9,9 +9,9 @@
 from gymnasium import spaces
 from pybullet_utils import bullet_client as bc
 
-from deformable_gym.envs.bullet_simulation import BulletSimulation
-from deformable_gym.helpers.pybullet_helper import MultibodyPose
-from deformable_gym.robots.bullet_robot import BulletRobot
+from ..envs.bullet_simulation import BulletSimulation
+from ..helpers.pybullet_helper import MultibodyPose
+from ..robots.bullet_robot import BulletRobot
 
 
 class BaseBulletEnv(gym.Env, abc.ABC):
@@ -43,7 +43,7 @@ def __init__(
             horizon: int = 100,
             soft: bool = False,
             verbose: bool = False,
-            time_delta: float = 0.0001,
+            time_delta: float = 0.001,
             verbose_dt: float = 10.00,
             pybullet_options: str = ""):
 
@@ -107,7 +107,8 @@ def reset(self, seed=None, options=None) -> npt.ArrayLike:
         self.step_counter = 0
         self.simulation.timing.reset()
 
-        self.simulation.simulate_time(0.0001)
+        # simulate one time step to have correct initial sensor information
+        self.simulation.simulate_time(self.simulation.time_delta)
 
         observation = self._get_observation()
         info = self._get_info()

deformable_gym/envs/bullet_simulation.py

@@ -2,8 +2,8 @@
 import pybullet_data
 
 from pybullet_utils import bullet_client as bc
-from deformable_gym.robots.bullet_robot import BulletRobot
-from deformable_gym.helpers import pybullet_helper as pbh
+from ..robots.bullet_robot import BulletRobot
+from ..helpers import pybullet_helper as pbh
 
 
 class BulletSimulation:

deformable_gym/envs/floating_mia_grasp_env.py

@@ -2,9 +2,9 @@
 import pybullet as pb
 from gymnasium import spaces
 
-from deformable_gym.envs.grasp_env import GraspEnv
-from deformable_gym.helpers import pybullet_helper as pbh
-from deformable_gym.robots import mia_hand
+from ..envs.grasp_env import GraspEnv
+from ..helpers import pybullet_helper as pbh
+from ..robots import mia_hand
 
 
 class FloatingMiaGraspEnv(GraspEnv):
@@ -34,10 +34,7 @@ class FloatingMiaGraspEnv(GraspEnv):
         object should be sampled from the training or the test set.
     """
 
-    STANDARD_INITIAL_POSE = np.r_[
-        0.03, -0.005, 1.0, pb.getQuaternionFromEuler([-np.pi/8, np.pi, 0])]
-
-    HARD_INITIAL_POSE = np.r_[
+    INITIAL_POSE = np.r_[
         0.03, -0.025, 1.0, pb.getQuaternionFromEuler([-np.pi/8, np.pi, 0])]
 
     _FINGERS_OPEN = {
@@ -72,7 +69,6 @@ def __init__(
             object_name: str = "insole",
             object_scale: float = 1.0,
             observable_object_pos: bool = False,
-            difficulty_mode: str = "hard",
             **kwargs):
 
         self.velocity_commands = False
@@ -83,14 +79,12 @@ def __init__(
             observable_object_pos=observable_object_pos,
             **kwargs)
 
-        self.hand_world_pose = self.STANDARD_INITIAL_POSE
+        self.hand_world_pose = self.INITIAL_POSE
         self.robot = self._create_robot()
 
         limits = pbh.get_limit_array(self.robot.motors.values())
         self.actuated_finger_ids = np.array([0, 1, 5], dtype=int)
 
-        self.set_difficulty_mode(difficulty_mode)
-
         lower_observations = np.concatenate([
             np.array([-2, -2, 0]),
             -np.ones(4),
@@ -157,26 +151,6 @@ def _create_robot(self):
 
         return robot
 
-    def set_difficulty_mode(self, mode: str):
-        """
-        Sets the difficulty of the environment by changing the initial hand
-        position. Can be used for curriculum learning.
-
-        :param mode: String representation of the desired difficulty.
-        """
-
-        if mode == "hard":
-            self.robot.set_initial_joint_positions(self._FINGERS_OPEN)
-            self.hand_world_pose = self.HARD_INITIAL_POSE
-        elif mode == "medium":
-            self.robot.set_initial_joint_positions(self._FINGERS_HALFWAY_CLOSED)
-            self.hand_world_pose = self.STANDARD_INITIAL_POSE
-        elif mode == "easy":
-            self.robot.set_initial_joint_positions(self._FINGERS_CLOSED)
-            self.hand_world_pose = self.STANDARD_INITIAL_POSE
-        else:
-            raise ValueError(f"Received unknown difficulty mode {mode}!")
-
     def _get_observation(self):
         joint_pos = self.robot.get_joint_positions(
             self.robot.actuated_real_joints)

deformable_gym/envs/floating_shadow_grasp_env.py

@@ -2,9 +2,9 @@
 import pybullet as pb
 from gymnasium import spaces
 
-from deformable_gym.envs.grasp_env import GraspEnv
-from deformable_gym.helpers import pybullet_helper as pbh
-from deformable_gym.robots import shadow_hand
+from ..envs.grasp_env import GraspEnv
+from ..helpers import pybullet_helper as pbh
+from ..robots import shadow_hand
 
 
 class FloatingShadowGraspEnv(GraspEnv):
@@ -26,7 +26,7 @@ class FloatingShadowGraspEnv(GraspEnv):
     :param horizon: Number of steps in simulation.
     """
 
-    STANDARD_INITIAL_POSE = np.r_[
+    INITIAL_POSE = np.r_[
         0, -0.5, 1, pb.getQuaternionFromEuler([-np.pi/2, np.pi, 0])]
 
     def __init__(
@@ -42,7 +42,7 @@ def __init__(
                          observable_object_pos=observable_object_pos,
                          **kwargs)
 
-        self.hand_world_pose = self.STANDARD_INITIAL_POSE
+        self.hand_world_pose = self.INITIAL_POSE
         self.robot = self._create_robot()
 
         limits = pbh.get_limit_array(self.robot.motors.values())

deformable_gym/envs/grasp_env.py

@@ -3,15 +3,15 @@
 import numpy as np
 import pybullet as pb
 
-from deformable_gym.envs.base_env import BaseBulletEnv, GraspDeformableMixin
-from deformable_gym.envs.sampler import Sampler, FixedSampler
+from ..envs.base_env import BaseBulletEnv, GraspDeformableMixin
+from ..envs.sampler import Sampler, FixedSampler
 from typing import Optional
 
 from ..objects.bullet_object import ObjectFactory
 
 
 class GraspEnv(BaseBulletEnv, GraspDeformableMixin, ABC):
-    HARD_INITIAL_POSE = np.r_[
+    INITIAL_POSE = np.r_[
         0.03, -0.025, 1.0, pb.getQuaternionFromEuler([-np.pi / 8, np.pi, 0])]
 
     def __init__(
@@ -26,7 +26,7 @@ def __init__(
         self.object_scale = object_scale
         self._observable_object_pos = observable_object_pos
         if initial_state_sampler is None:
-            self.initial_pose_sampler = FixedSampler(self.HARD_INITIAL_POSE)
+            self.initial_pose_sampler = FixedSampler(self.INITIAL_POSE)
         else:
             self.initial_pose_sampler = initial_state_sampler
 

deformable_gym/envs/ur10_shadow_grasp_env.py

@@ -2,10 +2,10 @@
 import pytransform3d.transformations as pt
 from gymnasium import spaces
 
-from deformable_gym.envs.base_env import BaseBulletEnv, GraspDeformableMixin
-from deformable_gym.helpers import pybullet_helper as pbh
-from deformable_gym.objects.bullet_object import ObjectFactory
-from deformable_gym.robots import ur10_shadow
+from ..envs.base_env import BaseBulletEnv, GraspDeformableMixin
+from ..helpers import pybullet_helper as pbh
+from ..objects.bullet_object import ObjectFactory
+from ..robots import ur10_shadow
 
 
 class UR10ShadowGraspEnv(GraspDeformableMixin, BaseBulletEnv):

deformable_gym/envs/ur5_mia_grasp_env.py

@@ -2,10 +2,10 @@
 import pytransform3d.transformations as pt
 from gymnasium import spaces
 
-from deformable_gym.envs.base_env import BaseBulletEnv, GraspDeformableMixin
-from deformable_gym.helpers import pybullet_helper as pbh
-from deformable_gym.objects.bullet_object import ObjectFactory
-from deformable_gym.robots import ur5_mia
+from ..envs.base_env import BaseBulletEnv, GraspDeformableMixin
+from ..helpers import pybullet_helper as pbh
+from ..objects.bullet_object import ObjectFactory
+from ..robots import ur5_mia
 
 INITIAL_JOINT_ANGLES = {
     "ur5_shoulder_pan_joint": 2.44388798,

tests/envs/test_floating_mia_grasp_env.py

@@ -8,12 +8,13 @@
 def env():
     return FloatingMiaGraspEnv(
         gui=False,
-        verbose=True,
+        verbose=False,
         horizon=10,
         object_name="insole_on_conveyor_belt/back")
 
 
 action_space_dims_expected = 10
+observation_space_dims_expected = 16
 SEED = 42
 
 
@@ -24,23 +25,14 @@ def test_action_space_dims(env: FloatingMiaGraspEnv):
 
 def test_obs_space_dims(env: FloatingMiaGraspEnv):
     if env._observable_object_pos:
-        obs_space_dims_expected = 19
+        obs_space_dims_expected = observation_space_dims_expected + 3
     else:
-        obs_space_dims_expected = 16
+        obs_space_dims_expected = observation_space_dims_expected
 
     obs_space = env.observation_space
     assert obs_space.shape[0] == obs_space_dims_expected
 
 
-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_initial_sensor_info(env: FloatingMiaGraspEnv):
     sensor_readings = []
     env.action_space.seed(SEED)
@@ -90,20 +82,13 @@ def test_episode_reproducibility():
     assert_allclose(termination_flags[0], termination_flags[1])
 
 
-def test_eps_done(env: FloatingMiaGraspEnv):
-    if env._observable_object_pos:
-        obs_space_dims_expected = 19
-    else:
-        obs_space_dims_expected = 16
-
+def test_episode_termination(env: FloatingMiaGraspEnv):
     env.action_space.seed(SEED)
 
     for t in range(9):
         action = env.action_space.sample()
         obs, reward, terminated, truncated, info = env.step(action)
 
-        assert len(obs) == obs_space_dims_expected
-        assert isinstance(reward, float)
         assert isinstance(terminated, bool)
         assert not terminated
 

tests/envs/test_floating_shadow_grasp_env.py

@@ -4,53 +4,36 @@
 from deformable_gym.envs.floating_shadow_grasp_env import \
     FloatingShadowGraspEnv
 
-SEED = 42
-
 
 @pytest.fixture
 def env():
     return FloatingShadowGraspEnv(
         gui=False,
-        verbose=True,
+        verbose=False,
         horizon=10,
         object_name="insole",
         observable_object_pos=True,
     )
 
 
-@pytest.mark.skip("TODO")
-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
-    assert obs_space.shape[0] == 28
-
-
-@pytest.mark.skip("TODO")
-def test_initial_obs(env):
-    obs, info = env.reset()
-    assert len(obs) == 18
+observation_space_dims_expected = 28
+action_space_dims_expected = 32
+SEED = 42
 
 
-def test_ep_termination(env):
-    env.reset()
-    for t in range(9):
-        action = env.action_space.sample()
-        obs, reward, terminated, truncated, info = env.step(action)
+def test_action_space_dims(env):
+    action_space = env.action_space
+    assert action_space.shape[0] == action_space_dims_expected
 
-        assert len(obs) == 18
-        assert isinstance(reward, float)
-        assert isinstance(terminated, bool)
-        assert not terminated
 
-    action = env.action_space.sample()
-    obs, reward, terminated, truncated, info = env.step(action)
+def test_obs_space_dims(env: FloatingShadowGraspEnv):
+    if env._observable_object_pos:
+        obs_space_dims_expected = observation_space_dims_expected + 3
+    else:
+        obs_space_dims_expected = observation_space_dims_expected
 
-    assert terminated
+    obs_space = env.observation_space
+    assert obs_space.shape[0] == obs_space_dims_expected
 
 
 def test_initial_sensor_info(env: FloatingShadowGraspEnv):
@@ -67,3 +50,52 @@ def test_initial_sensor_info(env: FloatingShadowGraspEnv):
             env.step(action)
 
     assert_allclose(sensor_readings[0], sensor_readings[1])
+
+
+def test_episode_reproducibility():
+    observations = []
+    termination_flags = []
+    actions = []
+
+    env = FloatingShadowGraspEnv(
+        verbose=False,
+        horizon=3,
+        gui=False,
+        object_name="insole_on_conveyor_belt/back",
+    )
+
+    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])
+
+
+def test_episode_termination(env: FloatingShadowGraspEnv):
+    env.action_space.seed(SEED)
+
+    for t in range(9):
+        action = env.action_space.sample()
+        obs, reward, terminated, truncated, info = env.step(action)
+
+        assert isinstance(terminated, bool)
+        assert not terminated
+
+    action = env.action_space.sample()
+    obs, reward, terminated, truncated, info = env.step(action)
+
+    assert terminated