renaming to fancy_gym

2022-07-13 15:10:43 +02:00
parent d412fb229c
commit 8d1c1b44bf
179 changed files with 332 additions and 361 deletions
@@ -0,0 +1,14 @@
+# OpenAI Gym Wrappers
+
+These are the Environment Wrappers for selected [OpenAI Gym](https://gym.openai.com/) environments to use
+the Motion Primitive gym interface for them.
+
+## MP Environments
+These environments are wrapped-versions of their OpenAI-gym counterparts.
+
+|Name| Description|Trajectory Horizon|Action Dimension|Context Dimension
+|---|---|---|---|---|
+|`ContinuousMountainCarProMP-v0`| A ProMP wrapped version of the ContinuousMountainCar-v0 environment. | 100 | 1
+|`ReacherProMP-v2`| A ProMP wrapped version of the Reacher-v2 environment. | 50 | 2
+|`FetchSlideDenseProMP-v1`| A ProMP wrapped version of the FetchSlideDense-v1 environment. | 50 | 4 
+|`FetchReachDenseProMP-v1`| A ProMP wrapped version of the FetchReachDense-v1 environment. | 50 | 4
@@ -0,0 +1,118 @@
+from copy import deepcopy
+
+from gym import register
+
+from . import mujoco
+from .deprecated_needs_gym_robotics import robotics
+
+ALL_GYM_MOVEMENT_PRIMITIVE_ENVIRONMENTS = {"DMP": [], "ProMP": []}
+
+DEFAULT_BB_DICT_ProMP = {
+    "name": 'EnvName',
+    "wrappers": [],
+    "trajectory_generator_kwargs": {
+        'trajectory_generator_type': 'promp'
+    },
+    "phase_generator_kwargs": {
+        'phase_generator_type': 'linear'
+    },
+    "controller_kwargs": {
+        'controller_type': 'motor',
+        "p_gains": 1.0,
+        "d_gains": 0.1,
+    },
+    "basis_generator_kwargs": {
+        'basis_generator_type': 'zero_rbf',
+        'num_basis': 5,
+        'num_basis_zero_start': 1
+    }
+}
+
+kwargs_dict_reacher_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
+kwargs_dict_reacher_promp['controller_kwargs']['p_gains'] = 0.6
+kwargs_dict_reacher_promp['controller_kwargs']['d_gains'] = 0.075
+kwargs_dict_reacher_promp['basis_generator_kwargs']['num_basis'] = 6
+kwargs_dict_reacher_promp['name'] = "Reacher-v2"
+kwargs_dict_reacher_promp['wrappers'].append(mujoco.reacher_v2.MPWrapper)
+register(
+    id='ReacherProMP-v2',
+    entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
+    kwargs=kwargs_dict_reacher_promp
+)
+ALL_GYM_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append("ReacherProMP-v2")
+"""
+The Fetch environments are not supported by gym anymore. A new repository (gym_robotics) is supporting the environments.
+However, the usage and so on needs to be checked
+
+register(
+    id='FetchSlideDenseProMP-v1',
+    entry_point='fancy_gym.utils.make_env_helpers:make_promp_env_helper',
+    kwargs={
+        "name": "gym.envs.robotics:FetchSlideDense-v1",
+        "wrappers": [FlattenObservation, robotics.fetch.MPWrapper],
+        "traj_gen_kwargs": {
+            "num_dof": 4,
+            "num_basis": 5,
+            "duration": 2,
+            "post_traj_time": 0,
+            "zero_start": True,
+            "policy_type": "position"
+        }
+    }
+)
+ALL_GYM_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append("FetchSlideDenseProMP-v1")
+
+register(
+    id='FetchSlideProMP-v1',
+    entry_point='fancy_gym.utils.make_env_helpers:make_promp_env_helper',
+    kwargs={
+        "name": "gym.envs.robotics:FetchSlide-v1",
+        "wrappers": [FlattenObservation, robotics.fetch.MPWrapper],
+        "traj_gen_kwargs": {
+            "num_dof": 4,
+            "num_basis": 5,
+            "duration": 2,
+            "post_traj_time": 0,
+            "zero_start": True,
+            "policy_type": "position"
+        }
+    }
+)
+ALL_GYM_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append("FetchSlideProMP-v1")
+
+register(
+    id='FetchReachDenseProMP-v1',
+    entry_point='fancy_gym.utils.make_env_helpers:make_promp_env_helper',
+    kwargs={
+        "name": "gym.envs.robotics:FetchReachDense-v1",
+        "wrappers": [FlattenObservation, robotics.fetch.MPWrapper],
+        "traj_gen_kwargs": {
+            "num_dof": 4,
+            "num_basis": 5,
+            "duration": 2,
+            "post_traj_time": 0,
+            "zero_start": True,
+            "policy_type": "position"
+        }
+    }
+)
+ALL_GYM_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append("FetchReachDenseProMP-v1")
+
+register(
+    id='FetchReachProMP-v1',
+    entry_point='fancy_gym.utils.make_env_helpers:make_promp_env_helper',
+    kwargs={
+        "name": "gym.envs.robotics:FetchReach-v1",
+        "wrappers": [FlattenObservation, robotics.fetch.MPWrapper],
+        "traj_gen_kwargs": {
+            "num_dof": 4,
+            "num_basis": 5,
+            "duration": 2,
+            "post_traj_time": 0,
+            "zero_start": True,
+            "policy_type": "position"
+        }
+    }
+)
+ALL_GYM_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append("FetchReachProMP-v1")
+"""
@@ -0,0 +1 @@
+from . import fetch
@@ -0,0 +1 @@
+from .mp_wrapper import MPWrapper
@@ -0,0 +1,49 @@
+from typing import Union
+
+import numpy as np
+
+from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
+
+
+class MPWrapper(RawInterfaceWrapper):
+
+    @property
+    def active_obs(self):
+        return np.hstack([
+            [False] * 3,  # achieved goal
+            [True] * 3,  # desired/true goal
+            [False] * 3,  # grip pos
+            [True, True, False] * int(self.has_object),  # object position
+            [True, True, False] * int(self.has_object),  # object relative position
+            [False] * 2,  # gripper state
+            [False] * 3 * int(self.has_object),  # object rotation
+            [False] * 3 * int(self.has_object),  # object velocity position
+            [False] * 3 * int(self.has_object),  # object velocity rotation
+            [False] * 3,  # grip velocity position
+            [False] * 2,  # gripper velocity
+        ]).astype(bool)
+
+    @property
+    def current_vel(self) -> Union[float, int, np.ndarray]:
+        dt = self.sim.nsubsteps * self.sim.model.opt.timestep
+        grip_velp = self.sim.data.get_site_xvelp("robot0:grip") * dt
+        # gripper state should be symmetric for left and right.
+        # They are controlled with only one action for both gripper joints
+        gripper_state = self.sim.data.get_joint_qvel('robot0:r_gripper_finger_joint') * dt
+        return np.hstack([grip_velp, gripper_state])
+
+    @property
+    def current_pos(self) -> Union[float, int, np.ndarray]:
+        grip_pos = self.sim.data.get_site_xpos("robot0:grip")
+        # gripper state should be symmetric for left and right.
+        # They are controlled with only one action for both gripper joints
+        gripper_state = self.sim.data.get_joint_qpos('robot0:r_gripper_finger_joint')
+        return np.hstack([grip_pos, gripper_state])
+
+    @property
+    def goal_pos(self):
+        raise ValueError("Goal position is not available and has to be learnt based on the environment.")
+
+    @property
+    def dt(self) -> Union[float, int]:
+        return self.env.dt
@@ -0,0 +1 @@
+from . import reacher_v2
@@ -0,0 +1 @@
+from .mp_wrapper import MPWrapper
@@ -0,0 +1,26 @@
+from typing import Union
+
+import numpy as np
+
+from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
+
+
+class MPWrapper(RawInterfaceWrapper):
+
+    @property
+    def current_vel(self) -> Union[float, int, np.ndarray]:
+        return self.sim.data.qvel[:2]
+
+    @property
+    def current_pos(self) -> Union[float, int, np.ndarray]:
+        return self.sim.data.qpos[:2]
+
+    @property
+    def context_mask(self):
+        return np.concatenate([
+            [False] * 2,  # cos of two links
+            [False] * 2,  # sin of two links
+            [True] * 2,  # goal position
+            [False] * 2,  # angular velocity
+            [False] * 3,  # goal distance
+        ])