From 8613a3ac18a8469cd37d8d67716205d20053480d Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Thu, 23 May 2024 19:51:22 +0200 Subject: [PATCH 1/8] increase default time_delta --- deformable_gym/envs/base_env.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/deformable_gym/envs/base_env.py b/deformable_gym/envs/base_env.py index cae1b9b..14ec3fc 100644 --- a/deformable_gym/envs/base_env.py +++ b/deformable_gym/envs/base_env.py @@ -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 = ""): From e19d815aadc6bd9464a6bb68b6322704e43bc6f6 Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Mon, 27 May 2024 14:58:11 +0200 Subject: [PATCH 2/8] adapt pre episode simulation time dependent on time delta --- deformable_gym/envs/base_env.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/deformable_gym/envs/base_env.py b/deformable_gym/envs/base_env.py index 14ec3fc..594287c 100644 --- a/deformable_gym/envs/base_env.py +++ b/deformable_gym/envs/base_env.py @@ -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() From 136ec5fb5ec63faccb2510ce936eb5e074823f57 Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Mon, 27 May 2024 15:21:09 +0200 Subject: [PATCH 3/8] use relative imports --- deformable_gym/envs/floating_mia_grasp_env.py | 6 +++--- deformable_gym/envs/floating_shadow_grasp_env.py | 6 +++--- deformable_gym/envs/grasp_env.py | 4 ++-- deformable_gym/envs/ur10_shadow_grasp_env.py | 8 ++++---- deformable_gym/envs/ur5_mia_grasp_env.py | 8 ++++---- 5 files changed, 16 insertions(+), 16 deletions(-) diff --git a/deformable_gym/envs/floating_mia_grasp_env.py b/deformable_gym/envs/floating_mia_grasp_env.py index 68af560..8d6ddfa 100644 --- a/deformable_gym/envs/floating_mia_grasp_env.py +++ b/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): diff --git a/deformable_gym/envs/floating_shadow_grasp_env.py b/deformable_gym/envs/floating_shadow_grasp_env.py index 7405369..d1cab70 100644 --- a/deformable_gym/envs/floating_shadow_grasp_env.py +++ b/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): diff --git a/deformable_gym/envs/grasp_env.py b/deformable_gym/envs/grasp_env.py index 5653fe5..8f38cc2 100644 --- a/deformable_gym/envs/grasp_env.py +++ b/deformable_gym/envs/grasp_env.py @@ -3,8 +3,8 @@ 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 diff --git a/deformable_gym/envs/ur10_shadow_grasp_env.py b/deformable_gym/envs/ur10_shadow_grasp_env.py index 5cc3766..6c0e1b8 100644 --- a/deformable_gym/envs/ur10_shadow_grasp_env.py +++ b/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): diff --git a/deformable_gym/envs/ur5_mia_grasp_env.py b/deformable_gym/envs/ur5_mia_grasp_env.py index 245ff15..977655e 100644 --- a/deformable_gym/envs/ur5_mia_grasp_env.py +++ b/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, From 5e9552e242fb7a2d73d7c35e791c54d224eab709 Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Mon, 27 May 2024 15:24:14 +0200 Subject: [PATCH 4/8] use relative imports --- deformable_gym/envs/base_env.py | 6 +++--- deformable_gym/envs/bullet_simulation.py | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/deformable_gym/envs/base_env.py b/deformable_gym/envs/base_env.py index 594287c..45d5a49 100644 --- a/deformable_gym/envs/base_env.py +++ b/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): diff --git a/deformable_gym/envs/bullet_simulation.py b/deformable_gym/envs/bullet_simulation.py index b173ae6..94a3965 100644 --- a/deformable_gym/envs/bullet_simulation.py +++ b/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: From 57b4097c946007e79b2c081ec937cd4d672e7f37 Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Mon, 27 May 2024 16:42:59 +0200 Subject: [PATCH 5/8] use correct initial pose for floating hands --- deformable_gym/envs/floating_mia_grasp_env.py | 29 ++----------------- .../envs/floating_shadow_grasp_env.py | 4 +-- deformable_gym/envs/grasp_env.py | 4 +-- 3 files changed, 6 insertions(+), 31 deletions(-) diff --git a/deformable_gym/envs/floating_mia_grasp_env.py b/deformable_gym/envs/floating_mia_grasp_env.py index 8d6ddfa..8f39811 100644 --- a/deformable_gym/envs/floating_mia_grasp_env.py +++ b/deformable_gym/envs/floating_mia_grasp_env.py @@ -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 = { @@ -83,14 +80,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 +152,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) diff --git a/deformable_gym/envs/floating_shadow_grasp_env.py b/deformable_gym/envs/floating_shadow_grasp_env.py index d1cab70..29fc934 100644 --- a/deformable_gym/envs/floating_shadow_grasp_env.py +++ b/deformable_gym/envs/floating_shadow_grasp_env.py @@ -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()) diff --git a/deformable_gym/envs/grasp_env.py b/deformable_gym/envs/grasp_env.py index 8f38cc2..9e6c54d 100644 --- a/deformable_gym/envs/grasp_env.py +++ b/deformable_gym/envs/grasp_env.py @@ -11,7 +11,7 @@ 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 From 622bb03d94cc4cd2188db118b28f3bb9aef1e9c4 Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Mon, 27 May 2024 16:44:23 +0200 Subject: [PATCH 6/8] remove difficulty mode option --- deformable_gym/envs/floating_mia_grasp_env.py | 1 - 1 file changed, 1 deletion(-) diff --git a/deformable_gym/envs/floating_mia_grasp_env.py b/deformable_gym/envs/floating_mia_grasp_env.py index 8f39811..16eb50c 100644 --- a/deformable_gym/envs/floating_mia_grasp_env.py +++ b/deformable_gym/envs/floating_mia_grasp_env.py @@ -69,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 From 85c0bc47525e0d4d1e065d21873a6d33f305e70a Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Mon, 27 May 2024 17:06:09 +0200 Subject: [PATCH 7/8] more thorough floating hand env testing --- tests/envs/test_floating_mia_grasp_env.py | 23 +---- tests/envs/test_floating_shadow_grasp_env.py | 92 +++++++++++++------- 2 files changed, 66 insertions(+), 49 deletions(-) diff --git a/tests/envs/test_floating_mia_grasp_env.py b/tests/envs/test_floating_mia_grasp_env.py index 2beeb2b..7f96dca 100644 --- a/tests/envs/test_floating_mia_grasp_env.py +++ b/tests/envs/test_floating_mia_grasp_env.py @@ -14,6 +14,7 @@ def env(): 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 diff --git a/tests/envs/test_floating_shadow_grasp_env.py b/tests/envs/test_floating_shadow_grasp_env.py index 01ec622..18fabaa 100644 --- a/tests/envs/test_floating_shadow_grasp_env.py +++ b/tests/envs/test_floating_shadow_grasp_env.py @@ -4,8 +4,6 @@ from deformable_gym.envs.floating_shadow_grasp_env import \ FloatingShadowGraspEnv -SEED = 42 - @pytest.fixture def env(): @@ -18,39 +16,24 @@ def env(): ) -@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 From 75961666a48de99b364b918ad0fd80d42780747c Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Mon, 27 May 2024 17:07:51 +0200 Subject: [PATCH 8/8] non verbose fixtures --- tests/envs/test_floating_mia_grasp_env.py | 2 +- tests/envs/test_floating_shadow_grasp_env.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/envs/test_floating_mia_grasp_env.py b/tests/envs/test_floating_mia_grasp_env.py index 7f96dca..fbd8020 100644 --- a/tests/envs/test_floating_mia_grasp_env.py +++ b/tests/envs/test_floating_mia_grasp_env.py @@ -8,7 +8,7 @@ def env(): return FloatingMiaGraspEnv( gui=False, - verbose=True, + verbose=False, horizon=10, object_name="insole_on_conveyor_belt/back") diff --git a/tests/envs/test_floating_shadow_grasp_env.py b/tests/envs/test_floating_shadow_grasp_env.py index 18fabaa..2838946 100644 --- a/tests/envs/test_floating_shadow_grasp_env.py +++ b/tests/envs/test_floating_shadow_grasp_env.py @@ -9,7 +9,7 @@ def env(): return FloatingShadowGraspEnv( gui=False, - verbose=True, + verbose=False, horizon=10, object_name="insole", observable_object_pos=True,