├── README.md ├── dexterous_gym ├── __init__.py ├── core │ ├── __init__.py │ ├── two_hand_robot_env.py │ └── two_hands_env.py ├── envs │ ├── __init__.py │ ├── block_catch_overarm.py │ ├── block_catch_underarm.py │ ├── block_catch_underarm_hard.py │ ├── block_hand_over.py │ ├── egg_catch_overarm.py │ ├── egg_catch_underarm.py │ ├── egg_catch_underarm_hard.py │ ├── egg_hand_over.py │ ├── hand │ │ ├── block_catch_overarm.xml │ │ ├── block_catch_underarm.xml │ │ ├── block_catch_underarm_hard.xml │ │ ├── block_handover.xml │ │ ├── egg_catch_overarm.xml │ │ ├── egg_catch_underarm.xml │ │ ├── egg_catch_underarm_hard.xml │ │ ├── egg_handover.xml │ │ ├── manipulate_pen.xml │ │ ├── pen_catch_overarm.xml │ │ ├── pen_catch_underarm.xml │ │ ├── pen_catch_underarm_hard.xml │ │ ├── pen_handover.xml │ │ ├── robot.xml │ │ ├── shared.xml │ │ ├── shared_asset.xml │ │ ├── shared_two.xml │ │ ├── two_block_catch_underarm.xml │ │ ├── two_egg_catch_underarm.xml │ │ └── two_pen_catch_underarm.xml │ ├── pen_catch_overarm.py │ ├── pen_catch_underarm.py │ ├── pen_catch_underarm_hard.py │ ├── pen_hand_over.py │ ├── pen_spin.py │ ├── stls │ │ └── hand │ │ │ ├── F1.stl │ │ │ ├── F2.stl │ │ │ ├── F3.stl │ │ │ ├── TH1_z.stl │ │ │ ├── TH2_z.stl │ │ │ ├── TH3_z.stl │ │ │ ├── forearm_electric.stl │ │ │ ├── forearm_electric_cvx.stl │ │ │ ├── knuckle.stl │ │ │ ├── lfmetacarpal.stl │ │ │ ├── palm.stl │ │ │ └── wrist.stl │ ├── textures │ │ ├── block.png │ │ └── block_hidden.png │ ├── two_block_catch_underarm.py │ ├── two_egg_catch_underarm.py │ └── two_pen_catch_underarm.py └── examples │ ├── __init__.py │ ├── eggcatch.gif │ ├── eggcatchoverarm_still.png │ ├── egghandover.gif │ ├── penspin.gif │ ├── test_all_envs.py │ └── twoeggcatch_error.gif └── setup.py /README.md: -------------------------------------------------------------------------------- 1 | # Dexterous Gym - Challenging Extensions to OpenAI's Gym Hand Manipulation Environments 2 | This repository contains a number of challenging environments based on OpenAI's Gym hand manipulation tasks, all of which are challenging for current RL and trajectory optimisation techniques. Used in our paper: https://arxiv.org/abs/2009.05104 (videos showing our results and comparisons: https://dexterous-manipulation.github.io/) 3 | 4 | ## Installation 5 | Requirements: [Mujoco-py](https://github.com/openai/mujoco-py) and [Gym](https://github.com/openai/gym) 6 | Once these are installed, run `pip install dexterous-gym`. 7 | Alternatively, clone the repository and run `pip install -e .` from within the main directory. 8 | 9 | May require certain versions of Mujoco-Py and Gym - last tested with: 10 | 11 | gym=0.15.3 12 | 13 | dexterous-gym=0.1.7 14 | 15 | mujoco-py=2.0.2.13 16 | 17 | 18 | ## Example usage 19 | ``` 20 | import gym 21 | import dexterous_gym 22 | env = gym.make("EggCatchUnderarm-v0") 23 | ``` 24 | See `dexterous_gym/__init__.py` for full list of registered environments. See `dexterous_gym/examples/test_all_envs.py` to run all available environments with random actions. 25 | 26 | ## Environments 27 | ### Standard RL environments 28 | 29 | We include one "standard" RL environment (i.e. an environment which is not conditioned on a goal), PenSpin. 30 | 31 | \ 32 | Variants: "PenSpin-v0". 33 | 34 | This is a simple modification of the OpenAI gym HandPen environment where all we do is change the observation space to remove any notion of a goal and then define a custom reward function. This reward function encourages the pen to be spun whilst remaining horizontal. 35 | 36 | ### Goal-based environments 37 | A majority of the environments are goal-based, and have a similar API to the openAI Gym manipulation environments (observations are dictionaries with "observation", "achieved_goal", "desired_goal"). In the case of the two object environments "achieved_goal" and "desired_goal" are also dictionaries with entries "object_1" and "object_2". All environments come with a standard dense-reward setting (where the reward is defined in terms of the positional and rotational distance between the achieved goal(s) and the desired goal(s) as well as a sparse setting, where the reward is -1.0 until the goal(s) are achieved (at which point the reward is 0.0). 38 | 39 | #### HandOver Environments 40 | 41 | Variants: "EggHandOver-v0", "BlockHandOver-v0", "PenHandOver-v0", "EggHandOverSparse-v0", "BlockHandOverSparse-v0", "PenHandOverSparse-v0"

42 | These environments involve two fixed-position hands. The hand which starts with the object must find a way to hand it over to the second hand, since the goals are chosen such that only the second hand can move the object to the target position/orientation. The object can be an egg, a block or a pen. 43 | 44 |   45 |   46 | 47 | #### HandCatchUnderarm Environments 48 | 49 | Variants: "EggCatchUnderarm-v0", "BlockCatchUnderarm-v0", "PenCatchUnderArm-v0", "EggCatchUnderarmHard-v0", "BlockCatchUnderarmHard-v0", "PenCatchUnderarmHard-v0", "EggCatchUnderarmSparse-v0", "BlockCatchUnderarmSparse-v0", "PenCatchUnderArmSparse-v0", "EggCatchUnderarmHardSparse-v0", "BlockCatchUnderarmHardSparse-v0", "PenCatchUnderarmHardSparse-v0"

50 | These environments again have two hands, however now they have some additional degrees of freedom that allows them to translate/rotate their centre of masses within some constrained region. The aims of these tasks is to throw the object from one hand to the other, which can then move it to the desired position/rotation. The "Hard" versions involve a greater distance between the two hands, such that the objects must be thrown further. Again the object can be an egg, a block or a pen. 51 | 52 | #### HandCatchOverarm Environments 53 | 54 | Variants: "EggCatchOverarm-v0", "BlockCatchOverArm-v0", "PenCatchOverarm-v0", "EggCatchOverarmSparse-v0", "BlockCatchOverarmSparse-v0", "PenCatchOverarmSparse-v0"

55 | Similar to the HandCatchUnderArm environments but now the two hands are upright, and so the throwing/catching technique that has to be employed is different. TODO: add "harder" (i.e. further separation between hands) version of these environments, and run some tests with my trajectory optimisation algorithm to get some example trajectories. 56 | 57 | #### TwoObjectCatch Environments 58 | 59 | Variants: "TwoEggCatchUnderArm-v0", "TwoBlockCatchUnderArm-v0", "TwoPenCatchUnderArm-v0", "TwoEggCatchUnderArmSparse-v0", "TwoBlockCatchUnderArmSparse-v0", "TwoPenCatchUnderArmSparse-v0"

60 | These environments involve coordination between the two hands so as to throw the two objects between hands (i.e. swapping them). This is necessary since each object's goal can only be reached by the other hand. TODO: introduce "harder" and overarm versions of these environments. Also, note that the examples shown were generated when there was an error with the environment - need to re-run experiment now this is fixed. 61 | -------------------------------------------------------------------------------- /dexterous_gym/__init__.py: -------------------------------------------------------------------------------- 1 | from gym.envs.registration import register 2 | 3 | """ 4 | HandOver environments 5 | """ 6 | register( 7 | id='EggHandOver-v0', 8 | entry_point='dexterous_gym.envs.egg_hand_over:EggHandOver', 9 | max_episode_steps=75, 10 | kwargs={'reward_type': 'dense'} 11 | ) 12 | 13 | register( 14 | id='BlockHandOver-v0', 15 | entry_point='dexterous_gym.envs.block_hand_over:BlockHandOver', 16 | max_episode_steps=75, 17 | kwargs={'reward_type': 'dense'} 18 | ) 19 | 20 | register( 21 | id='PenHandOver-v0', 22 | entry_point='dexterous_gym.envs.pen_hand_over:PenHandOver', 23 | max_episode_steps=75, 24 | kwargs={'reward_type': 'dense'} 25 | ) 26 | 27 | register( 28 | id='EggHandOverSparse-v0', 29 | entry_point='dexterous_gym.envs.egg_hand_over:EggHandOver', 30 | max_episode_steps=75, 31 | kwargs={'reward_type': 'sparse'} 32 | ) 33 | 34 | register( 35 | id='BlockHandOverSparse-v0', 36 | entry_point='dexterous_gym.envs.block_hand_over:BlockHandOver', 37 | max_episode_steps=75, 38 | kwargs={'reward_type': 'sparse'} 39 | ) 40 | 41 | register( 42 | id='PenHandOverSparse-v0', 43 | entry_point='dexterous_gym.envs.pen_hand_over:PenHandOver', 44 | max_episode_steps=75, 45 | kwargs={'reward_type': 'sparse'} 46 | ) 47 | 48 | 49 | """ 50 | Underarm HandCatch envs 51 | """ 52 | 53 | register( 54 | id='EggCatchUnderarm-v0', 55 | entry_point='dexterous_gym.envs.egg_catch_underarm:EggCatchUnderarm', 56 | max_episode_steps=75, 57 | kwargs={'reward_type': 'dense'} 58 | ) 59 | 60 | register( 61 | id='BlockCatchUnderarm-v0', 62 | entry_point='dexterous_gym.envs.block_catch_underarm:BlockCatchUnderarm', 63 | max_episode_steps=75, 64 | kwargs={'reward_type': 'dense'} 65 | ) 66 | 67 | register( 68 | id='PenCatchUnderarm-v0', 69 | entry_point='dexterous_gym.envs.pen_catch_underarm:PenCatchUnderarm', 70 | max_episode_steps=75, 71 | kwargs={'reward_type': 'dense'} 72 | ) 73 | 74 | register( 75 | id='EggCatchUnderarmHard-v0', 76 | entry_point='dexterous_gym.envs.egg_catch_underarm_hard:EggCatchUnderarmHard', 77 | max_episode_steps=75, 78 | kwargs={'reward_type': 'dense'} 79 | ) 80 | 81 | register( 82 | id='BlockCatchUnderarmHard-v0', 83 | entry_point='dexterous_gym.envs.block_catch_underarm_hard:BlockCatchUnderarmHard', 84 | max_episode_steps=75, 85 | kwargs={'reward_type': 'dense'} 86 | ) 87 | 88 | register( 89 | id='PenCatchUnderarmHard-v0', 90 | entry_point='dexterous_gym.envs.pen_catch_underarm_hard:PenCatchUnderarmHard', 91 | max_episode_steps=75, 92 | kwargs={'reward_type': 'dense'} 93 | ) 94 | 95 | register( 96 | id='EggCatchUnderarmSparse-v0', 97 | entry_point='dexterous_gym.envs.egg_catch_underarm:EggCatchUnderarm', 98 | max_episode_steps=75, 99 | kwargs={'reward_type': 'sparse'} 100 | ) 101 | 102 | register( 103 | id='BlockCatchUnderarmSparse-v0', 104 | entry_point='dexterous_gym.envs.block_catch_underarm:BlockCatchUnderarm', 105 | max_episode_steps=75, 106 | kwargs={'reward_type': 'sparse'} 107 | ) 108 | 109 | register( 110 | id='PenCatchUnderarmSparse-v0', 111 | entry_point='dexterous_gym.envs.pen_catch_underarm:PenCatchUnderarm', 112 | max_episode_steps=75, 113 | kwargs={'reward_type': 'sparse'} 114 | ) 115 | 116 | register( 117 | id='EggCatchUnderarmHardSparse-v0', 118 | entry_point='dexterous_gym.envs.egg_catch_underarm_hard:EggCatchUnderarmHard', 119 | max_episode_steps=75, 120 | kwargs={'reward_type': 'sparse'} 121 | ) 122 | 123 | register( 124 | id='BlockCatchUnderarmHardSparse-v0', 125 | entry_point='dexterous_gym.envs.block_catch_underarm_hard:BlockCatchUnderarmHard', 126 | max_episode_steps=75, 127 | kwargs={'reward_type': 'sparse'} 128 | ) 129 | 130 | register( 131 | id='PenCatchUnderarmHardSparse-v0', 132 | entry_point='dexterous_gym.envs.pen_catch_underarm_hard:PenCatchUnderarmHard', 133 | max_episode_steps=75, 134 | kwargs={'reward_type': 'sparse'} 135 | ) 136 | 137 | register( 138 | id='TwoEggCatchUnderArm-v0', 139 | entry_point='dexterous_gym.envs.two_egg_catch_underarm:TwoEggCatchUnderarm', 140 | max_episode_steps=75, 141 | kwargs={'reward_type': 'dense'} 142 | ) 143 | 144 | register( 145 | id='TwoBlockCatchUnderArm-v0', 146 | entry_point='dexterous_gym.envs.two_block_catch_underarm:TwoBlockCatchUnderarm', 147 | max_episode_steps=75, 148 | kwargs={'reward_type': 'dense'} 149 | ) 150 | 151 | register( 152 | id='TwoPenCatchUnderArm-v0', 153 | entry_point='dexterous_gym.envs.two_pen_catch_underarm:TwoPenCatchUnderarm', 154 | max_episode_steps=75, 155 | kwargs={'reward_type': 'dense'} 156 | ) 157 | 158 | register( 159 | id='TwoEggCatchUnderArmSparse-v0', 160 | entry_point='dexterous_gym.envs.two_egg_catch_underarm:TwoEggCatchUnderarm', 161 | max_episode_steps=75, 162 | kwargs={'reward_type': 'sparse'} 163 | ) 164 | 165 | register( 166 | id='TwoBlockCatchUnderArmSparse-v0', 167 | entry_point='dexterous_gym.envs.two_block_catch_underarm:TwoBlockCatchUnderarm', 168 | max_episode_steps=75, 169 | kwargs={'reward_type': 'sparse'} 170 | ) 171 | 172 | register( 173 | id='TwoPenCatchUnderArmSparse-v0', 174 | entry_point='dexterous_gym.envs.two_pen_catch_underarm:TwoPenCatchUnderarm', 175 | max_episode_steps=75, 176 | kwargs={'reward_type': 'sparse'} 177 | ) 178 | 179 | 180 | """ 181 | overarm HandCatch envs 182 | """ 183 | 184 | register( 185 | id='EggCatchOverarm-v0', 186 | entry_point='dexterous_gym.envs.egg_catch_overarm:EggCatchOverarm', 187 | max_episode_steps=75, 188 | kwargs={'reward_type': 'dense'} 189 | ) 190 | 191 | register( 192 | id='BlockCatchOverarm-v0', 193 | entry_point='dexterous_gym.envs.block_catch_overarm:BlockCatchOverarm', 194 | max_episode_steps=75, 195 | kwargs={'reward_type': 'dense'} 196 | ) 197 | 198 | register( 199 | id='PenCatchOverarm-v0', 200 | entry_point='dexterous_gym.envs.pen_catch_overarm:PenCatchOverarm', 201 | max_episode_steps=75, 202 | kwargs={'reward_type': 'dense'} 203 | ) 204 | 205 | register( 206 | id='EggCatchOverarmSparse-v0', 207 | entry_point='dexterous_gym.envs.egg_catch_overarm:EggCatchOverarm', 208 | max_episode_steps=75, 209 | kwargs={'reward_type': 'sparse'} 210 | ) 211 | 212 | register( 213 | id='BlockCatchOverarmSparse-v0', 214 | entry_point='dexterous_gym.envs.block_catch_overarm:BlockCatchOverarm', 215 | max_episode_steps=75, 216 | kwargs={'reward_type': 'sparse'} 217 | ) 218 | 219 | register( 220 | id='PenCatchOverarmSparse-v0', 221 | entry_point='dexterous_gym.envs.pen_catch_overarm:PenCatchOverarm', 222 | max_episode_steps=75, 223 | kwargs={'reward_type': 'sparse'} 224 | ) 225 | 226 | 227 | """ 228 | PenSpin 229 | """ 230 | register( 231 | id='PenSpin-v0', 232 | entry_point='dexterous_gym.envs.pen_spin:PenSpin', 233 | max_episode_steps=250, 234 | ) 235 | -------------------------------------------------------------------------------- /dexterous_gym/core/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/core/__init__.py -------------------------------------------------------------------------------- /dexterous_gym/core/two_hand_robot_env.py: -------------------------------------------------------------------------------- 1 | """ 2 | Almost identical to the gym file, but had to play with the observation space declaration 3 | """ 4 | 5 | import os 6 | import copy 7 | import numpy as np 8 | 9 | import gym 10 | from gym import error, spaces 11 | from gym.utils import seeding 12 | 13 | try: 14 | import mujoco_py 15 | except ImportError as e: 16 | raise error.DependencyNotInstalled("{}. (HINT: you need to install mujoco_py, and also perform the setup instructions here: https://github.com/openai/mujoco-py/.)".format(e)) 17 | 18 | DEFAULT_SIZE = 500 19 | 20 | class RobotEnv(gym.GoalEnv): 21 | def __init__(self, model_path, initial_qpos, n_actions, n_substeps): 22 | if model_path.startswith('/'): 23 | fullpath = model_path 24 | else: 25 | fullpath = os.path.join(os.path.dirname(__file__), 'assets', model_path) 26 | if not os.path.exists(fullpath): 27 | raise IOError('File {} does not exist'.format(fullpath)) 28 | 29 | model = mujoco_py.load_model_from_path(fullpath) 30 | self.sim = mujoco_py.MjSim(model, nsubsteps=n_substeps) 31 | self.viewer = None 32 | self._viewers = {} 33 | 34 | self.metadata = { 35 | 'render.modes': ['human', 'rgb_array'], 36 | 'video.frames_per_second': int(np.round(1.0 / self.dt)) 37 | } 38 | 39 | self.seed() 40 | self._env_setup(initial_qpos=initial_qpos) 41 | self.initial_state = copy.deepcopy(self.sim.get_state()) 42 | 43 | self.goal = self._sample_goal() 44 | obs = self._get_obs() 45 | self.action_space = spaces.Box(-1., 1., shape=(n_actions,), dtype='float32') 46 | if isinstance(obs['achieved_goal'], dict): 47 | self.observation_space = spaces.Dict(dict( 48 | desired_goal = spaces.Dict(dict( 49 | object_1=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal']['object_1'].shape, dtype='float32'), 50 | object_2=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal']['object_2'].shape, dtype='float32') 51 | )), 52 | achieved_goal = spaces.Dict(dict( 53 | object_1=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal']['object_1'].shape, dtype='float32'), 54 | object_2=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal']['object_2'].shape, dtype='float32') 55 | )), 56 | observation=spaces.Box(-np.inf, np.inf, shape=obs['observation'].shape, dtype='float32'), 57 | )) 58 | else: 59 | self.observation_space = spaces.Dict(dict( 60 | desired_goal=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal'].shape, dtype='float32'), 61 | achieved_goal=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal'].shape, dtype='float32'), 62 | observation=spaces.Box(-np.inf, np.inf, shape=obs['observation'].shape, dtype='float32'), 63 | )) 64 | 65 | @property 66 | def dt(self): 67 | return self.sim.model.opt.timestep * self.sim.nsubsteps 68 | 69 | # Env methods 70 | # ---------------------------- 71 | 72 | def seed(self, seed=None): 73 | self.np_random, seed = seeding.np_random(seed) 74 | return [seed] 75 | 76 | def step(self, action): 77 | action = np.clip(action, self.action_space.low, self.action_space.high) 78 | self._set_action(action) 79 | self.sim.step() 80 | self._step_callback() 81 | obs = self._get_obs() 82 | 83 | done = False 84 | info = { 85 | 'is_success': self._is_success(obs['achieved_goal'], self.goal), 86 | } 87 | reward = self.compute_reward(obs['achieved_goal'], self.goal, info) 88 | return obs, reward, done, info 89 | 90 | def reset(self): 91 | # Attempt to reset the simulator. Since we randomize initial conditions, it 92 | # is possible to get into a state with numerical issues (e.g. due to penetration or 93 | # Gimbel lock) or we may not achieve an initial condition (e.g. an object is within the hand). 94 | # In this case, we just keep randomizing until we eventually achieve a valid initial 95 | # configuration. 96 | super(RobotEnv, self).reset() 97 | did_reset_sim = False 98 | while not did_reset_sim: 99 | did_reset_sim = self._reset_sim() 100 | self.goal = self._sample_goal().copy() 101 | obs = self._get_obs() 102 | return obs 103 | 104 | def close(self): 105 | if self.viewer is not None: 106 | # self.viewer.finish() 107 | self.viewer = None 108 | self._viewers = {} 109 | 110 | def render(self, mode='human', width=DEFAULT_SIZE, height=DEFAULT_SIZE): 111 | self._render_callback() 112 | if mode == 'rgb_array': 113 | self._get_viewer(mode).render(width, height) 114 | # window size used for old mujoco-py: 115 | data = self._get_viewer(mode).read_pixels(width, height, depth=False) 116 | # original image is upside-down, so flip it 117 | return data[::-1, :, :] 118 | elif mode == 'human': 119 | self._get_viewer(mode).render() 120 | 121 | def _get_viewer(self, mode): 122 | self.viewer = self._viewers.get(mode) 123 | if self.viewer is None: 124 | if mode == 'human': 125 | self.viewer = mujoco_py.MjViewer(self.sim) 126 | elif mode == 'rgb_array': 127 | self.viewer = mujoco_py.MjRenderContextOffscreen(self.sim, device_id=-1) 128 | self._viewer_setup() 129 | self._viewers[mode] = self.viewer 130 | return self.viewer 131 | 132 | # Extension methods 133 | # ---------------------------- 134 | 135 | def _reset_sim(self): 136 | """Resets a simulation and indicates whether or not it was successful. 137 | If a reset was unsuccessful (e.g. if a randomized state caused an error in the 138 | simulation), this method should indicate such a failure by returning False. 139 | In such a case, this method will be called again to attempt a the reset again. 140 | """ 141 | self.sim.set_state(self.initial_state) 142 | self.sim.forward() 143 | return True 144 | 145 | def _get_obs(self): 146 | """Returns the observation. 147 | """ 148 | raise NotImplementedError() 149 | 150 | def _set_action(self, action): 151 | """Applies the given action to the simulation. 152 | """ 153 | raise NotImplementedError() 154 | 155 | def _is_success(self, achieved_goal, desired_goal): 156 | """Indicates whether or not the achieved goal successfully achieved the desired goal. 157 | """ 158 | raise NotImplementedError() 159 | 160 | def _sample_goal(self): 161 | """Samples a new goal and returns it. 162 | """ 163 | raise NotImplementedError() 164 | 165 | def _env_setup(self, initial_qpos): 166 | """Initial configuration of the environment. Can be used to configure initial state 167 | and extract information from the simulation. 168 | """ 169 | pass 170 | 171 | def _viewer_setup(self): 172 | """Initial configuration of the viewer. Can be used to set the camera position, 173 | for example. 174 | """ 175 | pass 176 | 177 | def _render_callback(self): 178 | """A custom callback that is called before rendering. Can be used 179 | to implement custom visualizations. 180 | """ 181 | pass 182 | 183 | def _step_callback(self): 184 | """A custom callback that is called after stepping the simulation. Can be used 185 | to enforce additional constraints on the simulation state. 186 | """ 187 | pass 188 | -------------------------------------------------------------------------------- /dexterous_gym/core/two_hands_env.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from gym.envs.robotics import rotations 3 | from gym import error 4 | 5 | from dexterous_gym.core.two_hand_robot_env import RobotEnv 6 | try: 7 | import mujoco_py 8 | except ImportError as e: 9 | raise error.DependencyNotInstalled("{}. (HINT: you need to install mujoco_py, and also perform the setup instructions here: https://github.com/openai/mujoco-py/.)".format(e)) 10 | 11 | def quat_from_angle_and_axis(angle, axis): 12 | assert axis.shape == (3,) 13 | axis /= np.linalg.norm(axis) 14 | quat = np.concatenate([[np.cos(angle / 2.)], np.sin(angle / 2.) * axis]) 15 | quat /= np.linalg.norm(quat) 16 | return quat 17 | 18 | class TwoHandsEnv(RobotEnv): 19 | def __init__(self, model_path, initial_qpos, target_rotation, target_position_range_1, target_position_range_2, 20 | reward_type="dense", n_substeps=20, distance_threshold=0.01, rotation_threshold=0.1, 21 | ignore_z_target_rotation=False, break_up_obs=False, moving_hands=False, two_objects=False, 22 | randomise_initial_rotation=True): 23 | """ 24 | :param model_path (string): path to environment XML file 25 | :param initial_qpos (dict): define initial configuration 26 | :param target_rotation (string): 27 | - "ignore": target rotation ignored. 28 | - "xyz": fully randomised target rotation 29 | - "z": randomised target rotation around Z axis 30 | :param target_position_range_1 (np.array (3,2)): valid target positions for hand 1 31 | :param target_position_range_2 (np.array (3,2)): valid target positions for hand 2 32 | :param reward_type (string): "dense" or "sparse" 33 | :param n_substeps (int): number of simulation steps per call to step 34 | :param distance_threshold (float, metres): threshold at which position is "achieved" 35 | :param rotation_threshold (float, radians): threshold at which rotation is "achieved" 36 | :param ignore_z_target_rotation (bool): whether or not the Z axis of the target rotation is ignored 37 | :param break_up_obs (bool): if true, obs returns a dictionary with different entries for each hand, 38 | {"hand_1", "hand_2", "object", "achieved_goal", "desired_goal"} as opposed to {"observation", "achieved_goal", "desired_goal"} 39 | :param moving_hands (bool): whether or not the hands are able to move and rotate 40 | :param two_objects (bool): whether or not there are one or two objects 41 | """ 42 | self.target_position_1 = target_position_range_1 43 | self.target_position_2 = target_position_range_2 44 | self.reward_type = reward_type 45 | self.target_rotation = target_rotation 46 | self.ignore_z_target_rotation = ignore_z_target_rotation 47 | self.distance_threshold = distance_threshold 48 | self.rotation_threshold = rotation_threshold 49 | self.break_up_obs = break_up_obs 50 | self.moving_hands = moving_hands 51 | self.two_objects = two_objects 52 | self.randomise_initial_rotation = randomise_initial_rotation 53 | 54 | if moving_hands: 55 | self.n_actions = 52 56 | else: 57 | self.n_actions = 40 58 | 59 | assert self.target_rotation in ['ignore', 'xyz', 'z'] 60 | assert self.reward_type in ['dense', 'sparse'] 61 | initial_qpos = initial_qpos or {} 62 | 63 | super(TwoHandsEnv, self).__init__( 64 | model_path=model_path, n_substeps=n_substeps, n_actions=self.n_actions, initial_qpos=initial_qpos 65 | ) 66 | 67 | def _set_action(self, action): 68 | assert action.shape == (self.n_actions,) 69 | ctrlrange = self.sim.model.actuator_ctrlrange 70 | actuation_range = (ctrlrange[:, 1] - ctrlrange[:, 0]) / 2.0 71 | actuation_centre = (ctrlrange[:, 1] + ctrlrange[:, 0]) / 2.0 72 | self.sim.data.ctrl[:] = actuation_centre + action*actuation_range 73 | self.sim.data.ctrl[:] = np.clip(self.sim.data.ctrl, ctrlrange[:, 0], ctrlrange[:, 1]) 74 | 75 | def _get_achieved_goal(self): 76 | object_1_qpos = self.sim.data.get_joint_qpos('object:joint') 77 | assert object_1_qpos.shape == (7,) 78 | if self.two_objects: 79 | object_2_qpos = self.sim.data.get_joint_qpos('object_2:joint') 80 | assert object_2_qpos.shape == (7,) 81 | return {"object_1": object_1_qpos.copy(), "object_2": object_2_qpos.copy()} 82 | return object_1_qpos 83 | 84 | def _goal_distance_single(self, goal_a, goal_b): 85 | assert goal_a.shape == goal_b.shape 86 | assert goal_a.shape[-1] == 7 87 | 88 | d_rot = np.zeros_like(goal_b[..., 0]) 89 | d_pos = np.linalg.norm(goal_a[..., :3] - goal_b[..., :3], axis=-1) 90 | 91 | if self.target_rotation != 'ignore': 92 | quat_a, quat_b = goal_a[..., 3:], goal_b[..., 3:] 93 | if self.ignore_z_target_rotation: 94 | euler_a = rotations.quat2euler(quat_a) 95 | euler_b = rotations.quat2euler(quat_b) 96 | euler_a[2] = euler_b[2] 97 | quat_a = rotations.euler2quat(euler_a) 98 | quat_diff = rotations.quat_mul(quat_a, rotations.quat_conjugate(quat_b)) 99 | angle_diff = 2 * np.arccos(np.clip(quat_diff[..., 0], -1., 1.)) 100 | d_rot = angle_diff 101 | return d_pos, d_rot 102 | 103 | def _goal_distance(self, goal_a, goal_b): 104 | if self.two_objects: 105 | d_pos_1, d_rot_1 = self._goal_distance_single(goal_a["object_1"], goal_b["object_1"]) 106 | d_pos_2, d_rot_2 = self._goal_distance_single(goal_a["object_2"], goal_b["object_2"]) 107 | return (d_pos_1, d_rot_1), (d_pos_2, d_rot_2) 108 | else: 109 | return self._goal_distance_single(goal_a, goal_b) 110 | 111 | def compute_reward(self, achieved_goal, desired_goal, info): 112 | raise NotImplementedError #each environment will implement this individually. 113 | 114 | def _is_success(self, achieved_goal, desired_goal, array=False): 115 | if self.two_objects: 116 | (d_pos_1, d_rot_1), (d_pos_2, d_rot_2) = self._goal_distance(achieved_goal, desired_goal) 117 | a_pos_1 = (d_pos_1 < self.distance_threshold).astype(np.float32) 118 | a_rot_1 = (d_rot_1 < self.rotation_threshold).astype(np.float32) 119 | a_1 = a_pos_1 * a_rot_1 120 | a_pos_2 = (d_pos_2 < self.distance_threshold).astype(np.float32) 121 | a_rot_2 = (d_rot_2 < self.rotation_threshold).astype(np.float32) 122 | a_2 = a_pos_2 * a_rot_2 123 | if array: 124 | return np.array([a_1, a_2]) 125 | else: 126 | return a_1*a_2 127 | else: 128 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 129 | a_pos = (d_pos < self.distance_threshold).astype(np.float32) 130 | a_rot = (d_rot < self.rotation_threshold).astype(np.float32) 131 | return a_pos * a_rot 132 | 133 | def _reset_sim(self): 134 | self.sim.set_state(self.initial_state) 135 | self.sim.forward() 136 | 137 | if self.randomise_initial_rotation: 138 | initial_qpos = self.sim.data.get_joint_qpos('object:joint').copy() 139 | initial_pos, initial_quat = initial_qpos[:3], initial_qpos[3:] 140 | angle = self.np_random.uniform(-np.pi, np.pi) 141 | axis = self.np_random.uniform(-1., 1., size=3) 142 | offset_quat = quat_from_angle_and_axis(angle, axis) 143 | initial_quat = rotations.quat_mul(initial_quat, offset_quat) 144 | initial_quat /= np.linalg.norm(initial_quat) 145 | initial_qpos = np.concatenate([initial_pos, initial_quat]) 146 | self.sim.data.set_joint_qpos('object:joint', initial_qpos) 147 | if self.two_objects: 148 | initial_qpos_2 = self.sim.data.get_joint_qpos('object_2:joint').copy() 149 | initial_pos_2, initial_quat_2 = initial_qpos_2[:3], initial_qpos_2[3:] 150 | angle = self.np_random.uniform(-np.pi, np.pi) 151 | axis = self.np_random.uniform(-1., 1., size=3) 152 | offset_quat_2 = quat_from_angle_and_axis(angle, axis) 153 | initial_quat_2= rotations.quat_mul(initial_quat_2, offset_quat_2) 154 | initial_quat_2 /= np.linalg.norm(initial_quat_2) 155 | initial_qpos_2 = np.concatenate([initial_pos_2, initial_quat_2]) 156 | self.sim.data.set_joint_qpos('object_2:joint', initial_qpos_2) 157 | 158 | 159 | for _ in range(10): 160 | self._set_action(np.zeros(self.n_actions)) 161 | try: 162 | self.sim.step() 163 | except mujoco_py.MujocoException: 164 | return False 165 | return True 166 | 167 | def _sample_goal(self, goal_side=None): 168 | """ 169 | :param goal_side: which hand (1 or 2) is the target near. If None we automatically choose the side furthest away. 170 | """ 171 | if self.two_objects: 172 | obj_state = self._get_achieved_goal() 173 | pos_1 = np.mean(self.target_position_1, axis=-1) 174 | pos_2 = np.mean(self.target_position_2, axis=-1) 175 | dist_1 = np.linalg.norm(obj_state["object_1"][:3]-pos_1) 176 | dist_2 = np.linalg.norm(obj_state["object_1"][:3] - pos_2) 177 | if dist_1 > dist_2: 178 | target_pos_1 = self.np_random.uniform(self.target_position_1[:, 0], self.target_position_1[:, 1]) 179 | target_pos_2 = self.np_random.uniform(self.target_position_2[:, 0], self.target_position_2[:, 1]) 180 | else: 181 | target_pos_1 = self.np_random.uniform(self.target_position_2[:, 0], self.target_position_2[:, 1]) 182 | target_pos_2 = self.np_random.uniform(self.target_position_1[:, 0], self.target_position_1[:, 1]) 183 | else: 184 | if goal_side is None: 185 | obj_state = self._get_achieved_goal() 186 | pos_1 = np.mean(self.target_position_1, axis=-1) 187 | pos_2 = np.mean(self.target_position_2, axis=-1) 188 | dist_1 = np.linalg.norm(obj_state[:3]-pos_1) 189 | dist_2 = np.linalg.norm(obj_state[:3]-pos_2) 190 | if dist_1 > dist_2: 191 | goal_side = 1 192 | else: 193 | goal_side = 2 194 | if goal_side == 1: 195 | target_pos_1 = self.np_random.uniform(self.target_position_1[:, 0], self.target_position_1[:, 1]) 196 | else: 197 | target_pos_1 = self.np_random.uniform(self.target_position_2[:, 0], self.target_position_2[:, 1]) 198 | 199 | if self.target_rotation == 'z': 200 | angle = self.np_random.uniform(-np.pi, np.pi) 201 | axis = np.array([0.0, 0.0, 1.0]) 202 | target_quat_1 = quat_from_angle_and_axis(angle, axis) 203 | if self.two_objects: 204 | angle_2 = self.np_random.uniform(-np.pi, np.pi) 205 | target_quat_2 = quat_from_angle_and_axis(angle_2, axis) 206 | elif self.target_rotation == 'xyz': 207 | angle = self.np_random.uniform(-np.pi, np.pi) 208 | axis = self.np_random.uniform(-1.0, 1.0, size=3) 209 | target_quat_1 = quat_from_angle_and_axis(angle, axis) 210 | if self.two_objects: 211 | angle_2 = self.np_random.uniform(-np.pi, np.pi) 212 | axis_2 = self.np_random.uniform(-1.0, 1.0, size=3) 213 | target_quat_2 = quat_from_angle_and_axis(angle_2, axis_2) 214 | elif self.target_rotation == 'ignore': 215 | target_quat_1 = np.zeros((4,))+0.1 216 | if self.two_objects: 217 | target_quat_2 = np.zeros((4,))+0.1 218 | else: 219 | raise error.Error('Unknown target_rotation option "{}".'.format(self.target_rotation)) 220 | target_quat_1 /= np.linalg.norm(target_quat_1) 221 | if self.two_objects: 222 | target_quat_2 /= np.linalg.norm(target_quat_2) 223 | return {"object_1": np.concatenate([target_pos_1, target_quat_1]), 224 | "object_2": np.concatenate([target_pos_2, target_quat_2])} 225 | else: 226 | return np.concatenate([target_pos_1, target_quat_1]) 227 | 228 | def _get_obs(self): 229 | achieved_goal = self._get_achieved_goal() 230 | if self.sim.data.qpos is not None and self.sim.model.joint_names: 231 | names_1 = [n for n in self.sim.model.joint_names if n.startswith('robot') and not n.endswith('_2')] 232 | hand_qpos_1 = np.array([self.sim.data.get_joint_qpos(name) for name in names_1]) 233 | hand_qvel_1 = np.array([self.sim.data.get_joint_qvel(name) for name in names_1]) 234 | object_qvel_1 = self.sim.data.get_joint_qvel('object:joint') 235 | hand_1 = np.concatenate((hand_qpos_1, hand_qvel_1)) 236 | names_2 = [n for n in self.sim.model.joint_names if n.startswith('robot') and n.endswith('_2')] 237 | hand_qpos_2 = np.array([self.sim.data.get_joint_qpos(name) for name in names_2]) 238 | hand_qvel_2 = np.array([self.sim.data.get_joint_qvel(name) for name in names_2]) 239 | hand_2 = np.concatenate((hand_qpos_2, hand_qvel_2)) 240 | if self.two_objects: 241 | object_qvel_2 = self.sim.data.get_joint_qvel('object_2:joint') 242 | obj_1 = np.concatenate((achieved_goal["object_1"], object_qvel_1)) 243 | obj_2 = np.concatenate((achieved_goal["object_2"], object_qvel_2)) 244 | if self.break_up_obs: 245 | return { 246 | "hand_1": hand_1, "hand_2": hand_2, "obj_1": obj_1, "obj_2": obj_2, "achieved_goal": achieved_goal, 247 | "desired_goal": self.goal.copy() 248 | } 249 | else: 250 | obs = np.concatenate((hand_1, hand_2, obj_1, obj_2)) 251 | return { 252 | "observation": obs, "achieved_goal": achieved_goal, "desired_goal": self.goal.copy() 253 | } 254 | else: 255 | obj_1 = np.concatenate((achieved_goal, object_qvel_1)) 256 | if self.break_up_obs: 257 | return { 258 | "hand_1": hand_1, "hand_2": hand_2, "obj": obj_1, "achieved_goal": achieved_goal, "desired_goal": self.goal.copy() 259 | } 260 | else: 261 | return { 262 | "observation": np.concatenate((hand_1, hand_2, obj_1)), "achieved_goal": achieved_goal, "desired_goal": self.goal.copy() 263 | } 264 | return np.zeros(0), np.zeros(0) 265 | 266 | def _env_setup(self, initial_qpos): 267 | for name, value in initial_qpos.items(): 268 | self.sim.data.set_joint_qpos(name, value) 269 | self.sim.forward() 270 | 271 | def _viewer_setup(self): #each environment defines this itself. 272 | raise NotImplementedError 273 | 274 | def _render_callback(self): 275 | if self.two_objects: 276 | goal_1 = self.goal["object_1"] 277 | goal_2 = self.goal["object_2"] 278 | self.sim.data.set_joint_qpos('target_2:joint', goal_2) 279 | self.sim.data.set_joint_qvel('target_2:joint', np.zeros((6,))) 280 | if 'object_hidden_2' in self.sim.model.geom_names: 281 | hidden_id = self.sim.model.geom_name2id('object_hidden_2') 282 | self.sim.model.geom_rgba[hidden_id, 3] = 1.0 283 | else: 284 | goal_1 = self.goal.copy() 285 | 286 | self.sim.data.set_joint_qpos('target:joint', goal_1) 287 | self.sim.data.set_joint_qvel('target:joint', np.zeros((6,))) 288 | if 'object_hidden' in self.sim.model.geom_names: 289 | hidden_id = self.sim.model.geom_name2id('object_hidden') 290 | self.sim.model.geom_rgba[hidden_id, 3] = 1.0 291 | self.sim.forward() 292 | 293 | def render(self, mode='human', width=500, height=500): 294 | return super(TwoHandsEnv, self).render(mode, width, height) -------------------------------------------------------------------------------- /dexterous_gym/envs/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/__init__.py -------------------------------------------------------------------------------- /dexterous_gym/envs/block_catch_overarm.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/block_catch_overarm.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.9, 1.1], [-0.05, 0.05], [0.4, 0.5]]) 9 | DEFAULT_RANGE_2 = np.array([[0.9, 1.1], [1.25, 1.35], [0.4, 0.5]]) 10 | 11 | init_qpos = np.array([ 12 | -1.69092e-16, 1.28969e-13, -0.0983158, 0.00671436, 8.81387e-05, -6.18241e-07, 0.00953895, 0.00197672, 0.000177327, 13 | 0.00967983, 0.00965064, 0.0203301, 0.000178503, 0.0096801, 0.00965076, 0.0203529, 0.000178503, 0.00968019, 0.00965077, 14 | 0.0203259, 0.00969491, 0.000176805, 0.00967987, 0.00965127, 0.021756, -0.000176283, 0.00969183, 0.000157871, 0.00720181, 15 | -0.00959593, 1.52536e-12, 2.07967e-12, -0.0983158, -0.438026, 6.86275e-06, 3.1689e-06, -0.111061, -0.688919, -0.00154806, 16 | 0.0115532, 0.746703, 0.994549, -0.0971119, 0.201292, 0.777997, 0.885088, -0.0269409, 0.0686252, 0.903907, 1.11351, 17 | 0.00921815, -0.00116265, 0.00948305, 1.32187, 1.56106, -0.322045, 0.199435, 0.0400855, -0.51436, -1.41563, 1, -0.2, 18 | 0.40267, 1, 0, 0, 0, 1, 0.7, -24370.4, 1, 0, 0, 0 19 | ]) 20 | 21 | class BlockCatchOverarm(TwoHandsEnv, utils.EzPickle): 22 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 23 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 24 | rotation_threshold=0.1, dist_multiplier=50.0): 25 | self.dist_multiplier = dist_multiplier #scale of pos distance vs rot distance for reward. 26 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 27 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_rotation=target_rotation, 28 | target_position_range_1=target_range_1, target_position_range_2=target_range_2, 29 | reward_type=reward_type, distance_threshold=distance_threshold, moving_hands=True, 30 | rotation_threshold=rotation_threshold, break_up_obs=break_up_obs, two_objects=False) 31 | 32 | def compute_reward(self, achieved_goal, desired_goal, info): 33 | if self.reward_type == 'sparse': 34 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 35 | return (success - 1.0) 36 | else: 37 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 38 | dist = self.dist_multiplier*d_pos + d_rot 39 | return np.exp(-0.2*dist) 40 | 41 | def _viewer_setup(self): 42 | # body_id = self.sim.model.body_name2id('robot0:palm') 43 | middle_id = self.sim.model.site_name2id('centre-point') 44 | # lookat = self.sim.data.body_xpos[body_id] 45 | lookat = self.sim.data.site_xpos[middle_id] 46 | for idx, value in enumerate(lookat): 47 | self.viewer.cam.lookat[idx] = value 48 | self.viewer.cam.distance = 1.5 49 | self.viewer.cam.azimuth = 180.0 50 | self.viewer.cam.elevation = -15.0 51 | 52 | def _reset_sim(self): 53 | for i in range(len(init_qpos)): 54 | self.initial_state.qpos[i] = init_qpos[i] 55 | self.sim.set_state(self.initial_state) 56 | self.sim.forward() 57 | return True -------------------------------------------------------------------------------- /dexterous_gym/envs/block_catch_underarm.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/block_catch_underarm.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.9, 1.1], [0.33, 0.5], [0.1, 0.4]]) 9 | DEFAULT_RANGE_2 = np.array([[0.9, 1.1], [0.85, 1.02], [0.1, 0.4]]) 10 | 11 | class BlockCatchUnderarm(TwoHandsEnv, utils.EzPickle): 12 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 13 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 14 | rotation_threshold=0.1, dist_multiplier=50.0): 15 | self.dist_multiplier = dist_multiplier #scale of pos distance vs rot distance for reward. 16 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 17 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_rotation=target_rotation, 18 | target_position_range_1=target_range_1, target_position_range_2=target_range_2, 19 | reward_type=reward_type, distance_threshold=distance_threshold, moving_hands=True, 20 | rotation_threshold=rotation_threshold, break_up_obs=break_up_obs, two_objects=False) 21 | 22 | def compute_reward(self, achieved_goal, desired_goal, info): 23 | if self.reward_type == 'sparse': 24 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 25 | return (success - 1.0) 26 | else: 27 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 28 | dist = self.dist_multiplier*d_pos + d_rot 29 | return np.exp(-0.2*dist) 30 | 31 | def _viewer_setup(self): 32 | # body_id = self.sim.model.body_name2id('robot0:palm') 33 | middle_id = self.sim.model.site_name2id('centre-point') 34 | # lookat = self.sim.data.body_xpos[body_id] 35 | lookat = self.sim.data.site_xpos[middle_id] 36 | for idx, value in enumerate(lookat): 37 | self.viewer.cam.lookat[idx] = value 38 | self.viewer.cam.distance = 1.5 39 | self.viewer.cam.azimuth = 180.0 40 | self.viewer.cam.elevation = -55.0 -------------------------------------------------------------------------------- /dexterous_gym/envs/block_catch_underarm_hard.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/block_catch_underarm_hard.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.97, 1.03], [0.08, 0.15], [0.15, 0.35]]) 9 | DEFAULT_RANGE_2 = np.array([[0.97, 1.03], [1.20, 1.27], [0.15, 0.35]]) 10 | 11 | class BlockCatchUnderarmHard(TwoHandsEnv, utils.EzPickle): 12 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 13 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 14 | rotation_threshold=0.1, dist_multiplier=50.0): 15 | self.dist_multiplier = dist_multiplier #scale of pos distance vs rot distance for reward. 16 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 17 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_rotation=target_rotation, 18 | target_position_range_1=target_range_1, target_position_range_2=target_range_2, 19 | reward_type=reward_type, distance_threshold=distance_threshold, moving_hands=True, 20 | rotation_threshold=rotation_threshold, break_up_obs=break_up_obs, two_objects=False) 21 | 22 | def compute_reward(self, achieved_goal, desired_goal, info): 23 | if self.reward_type == 'sparse': 24 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 25 | return (success - 1.0) 26 | else: 27 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 28 | dist = self.dist_multiplier*d_pos + d_rot 29 | return np.exp(-0.2*dist) 30 | 31 | def _viewer_setup(self): 32 | # body_id = self.sim.model.body_name2id('robot0:palm') 33 | middle_id = self.sim.model.site_name2id('centre-point') 34 | # lookat = self.sim.data.body_xpos[body_id] 35 | lookat = self.sim.data.site_xpos[middle_id] 36 | for idx, value in enumerate(lookat): 37 | self.viewer.cam.lookat[idx] = value 38 | self.viewer.cam.distance = 2.0 39 | self.viewer.cam.azimuth = 180.0 40 | self.viewer.cam.elevation = -55.0 -------------------------------------------------------------------------------- /dexterous_gym/envs/block_hand_over.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/block_handover.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.96, 1.04], [0.81, 0.89], [0.2, 0.26]]) 9 | DEFAULT_RANGE_2 = np.array([[0.96, 1.04], [0.60, 0.68], [0.2, 0.26]]) 10 | 11 | class BlockHandOver(TwoHandsEnv, utils.EzPickle): 12 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 13 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 14 | rotation_threshold=0.1, dist_multiplier=20.0): 15 | self.dist_multiplier = dist_multiplier 16 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 17 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_position_range_1=target_range_1, 18 | target_position_range_2=target_range_2, reward_type=reward_type, target_rotation=target_rotation, 19 | distance_threshold=distance_threshold, moving_hands=False, rotation_threshold=rotation_threshold, 20 | break_up_obs=break_up_obs, two_objects=False) 21 | 22 | def compute_reward(self, achieved_goal, desired_goal, info): 23 | if self.reward_type == 'sparse': 24 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 25 | return (success - 1.0) 26 | else: 27 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 28 | dist = self.dist_multiplier*d_pos + d_rot 29 | return np.exp(-1.0*dist) 30 | 31 | def _viewer_setup(self): 32 | middle_id = self.sim.model.site_name2id('centre-point') 33 | lookat = self.sim.data.site_xpos[middle_id] 34 | for idx, value in enumerate(lookat): 35 | self.viewer.cam.lookat[idx] = value 36 | self.viewer.cam.distance = 1.2 37 | self.viewer.cam.azimuth = 180.0 38 | self.viewer.cam.elevation = -55.0 -------------------------------------------------------------------------------- /dexterous_gym/envs/egg_catch_overarm.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/egg_catch_overarm.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.9, 1.1], [-0.05, 0.05], [0.4, 0.5]]) 9 | DEFAULT_RANGE_2 = np.array([[0.9, 1.1], [1.25, 1.35], [0.4, 0.5]]) 10 | 11 | init_qpos = np.array([ 12 | -1.69092e-16, 1.28969e-13, -0.0983158, 0.00671436, 8.81387e-05, -6.18241e-07, 0.00953895, 0.00197672, 0.000177327, 13 | 0.00967983, 0.00965064, 0.0203301, 0.000178503, 0.0096801, 0.00965076, 0.0203529, 0.000178503, 0.00968019, 0.00965077, 14 | 0.0203259, 0.00969491, 0.000176805, 0.00967987, 0.00965127, 0.021756, -0.000176283, 0.00969183, 0.000157871, 0.00720181, 15 | -0.00959593, 1.52536e-12, 2.07967e-12, -0.0983158, -0.438026, 6.86275e-06, 3.1689e-06, -0.111061, -0.688919, -0.00154806, 16 | 0.0115532, 0.746703, 0.994549, -0.0971119, 0.201292, 0.777997, 0.885088, -0.0269409, 0.0686252, 0.903907, 1.11351, 17 | 0.00921815, -0.00116265, 0.00948305, 1.32187, 1.56106, -0.322045, 0.199435, 0.0400855, -0.51436, -1.41563, 1, -0.2, 18 | 0.40267, 1, 0, 0, 0, 1, 0.7, -24370.4, 1, 0, 0, 0 19 | ]) 20 | 21 | class EggCatchOverarm(TwoHandsEnv, utils.EzPickle): 22 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 23 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 24 | rotation_threshold=0.1, dist_multiplier=50.0): 25 | self.dist_multiplier = dist_multiplier #scale of pos distance vs rot distance for reward. 26 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 27 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_rotation=target_rotation, 28 | target_position_range_1=target_range_1, target_position_range_2=target_range_2, 29 | reward_type=reward_type, distance_threshold=distance_threshold, moving_hands=True, 30 | rotation_threshold=rotation_threshold, break_up_obs=break_up_obs, two_objects=False) 31 | 32 | def compute_reward(self, achieved_goal, desired_goal, info): 33 | if self.reward_type == 'sparse': 34 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 35 | return (success - 1.0) 36 | else: 37 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 38 | dist = self.dist_multiplier*d_pos + d_rot 39 | return np.exp(-0.2*dist) 40 | 41 | def _viewer_setup(self): 42 | # body_id = self.sim.model.body_name2id('robot0:palm') 43 | middle_id = self.sim.model.site_name2id('centre-point') 44 | # lookat = self.sim.data.body_xpos[body_id] 45 | lookat = self.sim.data.site_xpos[middle_id] 46 | for idx, value in enumerate(lookat): 47 | self.viewer.cam.lookat[idx] = value 48 | self.viewer.cam.distance = 1.5 49 | self.viewer.cam.azimuth = 180.0 50 | self.viewer.cam.elevation = -15.0 51 | 52 | def _reset_sim(self): 53 | for i in range(len(init_qpos)): 54 | self.initial_state.qpos[i] = init_qpos[i] 55 | self.sim.set_state(self.initial_state) 56 | self.sim.forward() 57 | return True -------------------------------------------------------------------------------- /dexterous_gym/envs/egg_catch_underarm.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/egg_catch_underarm.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.9, 1.1], [0.33, 0.5], [0.1, 0.4]]) 9 | DEFAULT_RANGE_2 = np.array([[0.9, 1.1], [0.85, 1.02], [0.1, 0.4]]) 10 | 11 | class EggCatchUnderarm(TwoHandsEnv, utils.EzPickle): 12 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 13 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 14 | rotation_threshold=0.1, dist_multiplier=50.0): 15 | self.dist_multiplier = dist_multiplier #scale of pos distance vs rot distance for reward. 16 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 17 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_rotation=target_rotation, 18 | target_position_range_1=target_range_1, target_position_range_2=target_range_2, 19 | reward_type=reward_type, distance_threshold=distance_threshold, moving_hands=True, 20 | rotation_threshold=rotation_threshold, break_up_obs=break_up_obs, two_objects=False) 21 | 22 | def compute_reward(self, achieved_goal, desired_goal, info): 23 | if self.reward_type == 'sparse': 24 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 25 | return (success - 1.0) 26 | else: 27 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 28 | dist = self.dist_multiplier*d_pos + d_rot 29 | return np.exp(-0.2*dist) 30 | 31 | def _viewer_setup(self): 32 | # body_id = self.sim.model.body_name2id('robot0:palm') 33 | middle_id = self.sim.model.site_name2id('centre-point') 34 | # lookat = self.sim.data.body_xpos[body_id] 35 | lookat = self.sim.data.site_xpos[middle_id] 36 | for idx, value in enumerate(lookat): 37 | self.viewer.cam.lookat[idx] = value 38 | self.viewer.cam.distance = 1.5 39 | self.viewer.cam.azimuth = 180.0 40 | self.viewer.cam.elevation = -55.0 -------------------------------------------------------------------------------- /dexterous_gym/envs/egg_catch_underarm_hard.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/egg_catch_underarm_hard.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.97, 1.03], [0.08, 0.15], [0.15, 0.35]]) 9 | DEFAULT_RANGE_2 = np.array([[0.97, 1.03], [1.20, 1.27], [0.15, 0.35]]) 10 | 11 | class EggCatchUnderarmHard(TwoHandsEnv, utils.EzPickle): 12 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 13 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 14 | rotation_threshold=0.1, dist_multiplier=50.0): 15 | self.dist_multiplier = dist_multiplier #scale of pos distance vs rot distance for reward. 16 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 17 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_rotation=target_rotation, 18 | target_position_range_1=target_range_1, target_position_range_2=target_range_2, 19 | reward_type=reward_type, distance_threshold=distance_threshold, moving_hands=True, 20 | rotation_threshold=rotation_threshold, break_up_obs=break_up_obs, two_objects=False) 21 | 22 | def compute_reward(self, achieved_goal, desired_goal, info): 23 | if self.reward_type == 'sparse': 24 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 25 | return (success - 1.0) 26 | else: 27 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 28 | dist = self.dist_multiplier*d_pos + d_rot 29 | return np.exp(-0.2*dist) 30 | 31 | def _viewer_setup(self): 32 | # body_id = self.sim.model.body_name2id('robot0:palm') 33 | middle_id = self.sim.model.site_name2id('centre-point') 34 | # lookat = self.sim.data.body_xpos[body_id] 35 | lookat = self.sim.data.site_xpos[middle_id] 36 | for idx, value in enumerate(lookat): 37 | self.viewer.cam.lookat[idx] = value 38 | self.viewer.cam.distance = 2.0 39 | self.viewer.cam.azimuth = 180.0 40 | self.viewer.cam.elevation = -55.0 -------------------------------------------------------------------------------- /dexterous_gym/envs/egg_hand_over.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/egg_handover.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.96, 1.04], [0.81, 0.89], [0.2, 0.26]]) 9 | DEFAULT_RANGE_2 = np.array([[0.96, 1.04], [0.60, 0.68], [0.2, 0.26]]) 10 | 11 | class EggHandOver(TwoHandsEnv, utils.EzPickle): 12 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 13 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 14 | rotation_threshold=0.1, dist_multiplier=20.0): 15 | self.dist_multiplier = dist_multiplier 16 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 17 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_position_range_1=target_range_1, 18 | target_position_range_2=target_range_2, reward_type=reward_type, target_rotation=target_rotation, 19 | distance_threshold=distance_threshold, moving_hands=False, rotation_threshold=rotation_threshold, 20 | break_up_obs=break_up_obs, two_objects=False) 21 | 22 | def compute_reward(self, achieved_goal, desired_goal, info): 23 | if self.reward_type == 'sparse': 24 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 25 | return (success - 1.0) 26 | else: 27 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 28 | dist = self.dist_multiplier*d_pos + d_rot 29 | return np.exp(-1.0*dist) 30 | 31 | def _viewer_setup(self): 32 | middle_id = self.sim.model.site_name2id('centre-point') 33 | lookat = self.sim.data.site_xpos[middle_id] 34 | for idx, value in enumerate(lookat): 35 | self.viewer.cam.lookat[idx] = value 36 | self.viewer.cam.distance = 1.2 37 | self.viewer.cam.azimuth = 180.0 38 | self.viewer.cam.elevation = -55.0 -------------------------------------------------------------------------------- /dexterous_gym/envs/hand/block_handover.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | -------------------------------------------------------------------------------- /dexterous_gym/envs/hand/egg_handover.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | -------------------------------------------------------------------------------- /dexterous_gym/envs/hand/manipulate_pen.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /dexterous_gym/envs/hand/robot.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | -------------------------------------------------------------------------------- /dexterous_gym/envs/hand/shared.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | -------------------------------------------------------------------------------- /dexterous_gym/envs/hand/shared_asset.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /dexterous_gym/envs/hand/shared_two.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | -------------------------------------------------------------------------------- /dexterous_gym/envs/pen_catch_overarm.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/pen_catch_overarm.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.9, 1.1], [-0.05, 0.05], [0.4, 0.5]]) 9 | DEFAULT_RANGE_2 = np.array([[0.9, 1.1], [1.25, 1.35], [0.4, 0.5]]) 10 | 11 | init_qpos = np.array([ 12 | -1.69092e-16, 1.28969e-13, -0.0983158, 0.00671436, 8.81387e-05, -6.18241e-07, 0.00953895, 0.00197672, 0.000177327, 13 | 0.00967983, 0.00965064, 0.0203301, 0.000178503, 0.0096801, 0.00965076, 0.0203529, 0.000178503, 0.00968019, 0.00965077, 14 | 0.0203259, 0.00969491, 0.000176805, 0.00967987, 0.00965127, 0.021756, -0.000176283, 0.00969183, 0.000157871, 0.00720181, 15 | -0.00959593, 1.52536e-12, 2.07967e-12, -0.0983158, -0.438026, 6.86275e-06, 3.1689e-06, -0.111061, -0.688919, -0.00154806, 16 | 0.0115532, 0.746703, 0.994549, -0.0971119, 0.201292, 0.777997, 0.885088, -0.0269409, 0.0686252, 0.903907, 1.11351, 17 | 0.00921815, -0.00116265, 0.00948305, 1.32187, 1.56106, -0.322045, 0.199435, 0.0400855, -0.51436, -1.41563, 1, -0.2, 18 | 0.40267, 1, 1, 1, 1, 1, 0.7, -24370.4, 1, 0, 0, 0 19 | ]) 20 | 21 | class PenCatchOverarm(TwoHandsEnv, utils.EzPickle): 22 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 23 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 24 | rotation_threshold=0.1, dist_multiplier=50.0): 25 | self.dist_multiplier = dist_multiplier #scale of pos distance vs rot distance for reward. 26 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 27 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_rotation=target_rotation, 28 | target_position_range_1=target_range_1, target_position_range_2=target_range_2, 29 | reward_type=reward_type, distance_threshold=distance_threshold, moving_hands=True, 30 | rotation_threshold=rotation_threshold, break_up_obs=break_up_obs, two_objects=False, 31 | randomise_initial_rotation=False) 32 | 33 | def compute_reward(self, achieved_goal, desired_goal, info): 34 | if self.reward_type == 'sparse': 35 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 36 | return (success - 1.0) 37 | else: 38 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 39 | dist = self.dist_multiplier*d_pos + d_rot 40 | return np.exp(-0.2*dist) 41 | 42 | def _viewer_setup(self): 43 | # body_id = self.sim.model.body_name2id('robot0:palm') 44 | middle_id = self.sim.model.site_name2id('centre-point') 45 | # lookat = self.sim.data.body_xpos[body_id] 46 | lookat = self.sim.data.site_xpos[middle_id] 47 | for idx, value in enumerate(lookat): 48 | self.viewer.cam.lookat[idx] = value 49 | self.viewer.cam.distance = 1.5 50 | self.viewer.cam.azimuth = 180.0 51 | self.viewer.cam.elevation = -15.0 52 | 53 | def _reset_sim(self): 54 | for i in range(len(init_qpos)): 55 | self.initial_state.qpos[i] = init_qpos[i] 56 | self.sim.set_state(self.initial_state) 57 | self.sim.forward() 58 | return True -------------------------------------------------------------------------------- /dexterous_gym/envs/pen_catch_underarm.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/pen_catch_underarm.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.9, 1.1], [0.33, 0.5], [0.1, 0.4]]) 9 | DEFAULT_RANGE_2 = np.array([[0.9, 1.1], [0.85, 1.02], [0.1, 0.4]]) 10 | 11 | class PenCatchUnderarm(TwoHandsEnv, utils.EzPickle): 12 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 13 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 14 | rotation_threshold=0.1, dist_multiplier=50.0): 15 | self.dist_multiplier = dist_multiplier #scale of pos distance vs rot distance for reward. 16 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 17 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_rotation=target_rotation, 18 | target_position_range_1=target_range_1, target_position_range_2=target_range_2, 19 | reward_type=reward_type, distance_threshold=distance_threshold, moving_hands=True, 20 | rotation_threshold=rotation_threshold, break_up_obs=break_up_obs, two_objects=False, 21 | randomise_initial_rotation=False) 22 | 23 | def compute_reward(self, achieved_goal, desired_goal, info): 24 | if self.reward_type == 'sparse': 25 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 26 | return (success - 1.0) 27 | else: 28 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 29 | dist = self.dist_multiplier*d_pos + d_rot 30 | return np.exp(-0.2*dist) 31 | 32 | def _viewer_setup(self): 33 | # body_id = self.sim.model.body_name2id('robot0:palm') 34 | middle_id = self.sim.model.site_name2id('centre-point') 35 | # lookat = self.sim.data.body_xpos[body_id] 36 | lookat = self.sim.data.site_xpos[middle_id] 37 | for idx, value in enumerate(lookat): 38 | self.viewer.cam.lookat[idx] = value 39 | self.viewer.cam.distance = 1.5 40 | self.viewer.cam.azimuth = 180.0 41 | self.viewer.cam.elevation = -55.0 -------------------------------------------------------------------------------- /dexterous_gym/envs/pen_catch_underarm_hard.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/pen_catch_underarm_hard.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.97, 1.03], [0.08, 0.15], [0.15, 0.35]]) 9 | DEFAULT_RANGE_2 = np.array([[0.97, 1.03], [1.20, 1.27], [0.15, 0.35]]) 10 | 11 | class PenCatchUnderarmHard(TwoHandsEnv, utils.EzPickle): 12 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 13 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 14 | rotation_threshold=0.1, dist_multiplier=50.0): 15 | self.dist_multiplier = dist_multiplier #scale of pos distance vs rot distance for reward. 16 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 17 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_rotation=target_rotation, 18 | target_position_range_1=target_range_1, target_position_range_2=target_range_2, 19 | reward_type=reward_type, distance_threshold=distance_threshold, moving_hands=True, 20 | rotation_threshold=rotation_threshold, break_up_obs=break_up_obs, two_objects=False, 21 | randomise_initial_rotation=False) 22 | 23 | def compute_reward(self, achieved_goal, desired_goal, info): 24 | if self.reward_type == 'sparse': 25 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 26 | return (success - 1.0) 27 | else: 28 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 29 | dist = self.dist_multiplier*d_pos + d_rot 30 | return np.exp(-0.2*dist) 31 | 32 | def _viewer_setup(self): 33 | # body_id = self.sim.model.body_name2id('robot0:palm') 34 | middle_id = self.sim.model.site_name2id('centre-point') 35 | # lookat = self.sim.data.body_xpos[body_id] 36 | lookat = self.sim.data.site_xpos[middle_id] 37 | for idx, value in enumerate(lookat): 38 | self.viewer.cam.lookat[idx] = value 39 | self.viewer.cam.distance = 2.0 40 | self.viewer.cam.azimuth = 180.0 41 | self.viewer.cam.elevation = -55.0 -------------------------------------------------------------------------------- /dexterous_gym/envs/pen_hand_over.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/pen_handover.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.96, 1.04], [0.81, 0.89], [0.2, 0.26]]) 9 | DEFAULT_RANGE_2 = np.array([[0.96, 1.04], [0.60, 0.68], [0.2, 0.26]]) 10 | 11 | class PenHandOver(TwoHandsEnv, utils.EzPickle): 12 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 13 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 14 | rotation_threshold=0.1, dist_multiplier=20.0): 15 | self.dist_multiplier = dist_multiplier 16 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 17 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_position_range_1=target_range_1, 18 | target_position_range_2=target_range_2, reward_type=reward_type, target_rotation=target_rotation, 19 | distance_threshold=distance_threshold, moving_hands=False, rotation_threshold=rotation_threshold, 20 | break_up_obs=break_up_obs, two_objects=False, randomise_initial_rotation=False) 21 | 22 | def compute_reward(self, achieved_goal, desired_goal, info): 23 | if self.reward_type == 'sparse': 24 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 25 | return (success - 1.0) 26 | else: 27 | d_pos, d_rot = self._goal_distance(achieved_goal, desired_goal) 28 | dist = self.dist_multiplier*d_pos + d_rot 29 | return np.exp(-1.0*dist) 30 | 31 | def _viewer_setup(self): 32 | middle_id = self.sim.model.site_name2id('centre-point') 33 | lookat = self.sim.data.site_xpos[middle_id] 34 | for idx, value in enumerate(lookat): 35 | self.viewer.cam.lookat[idx] = value 36 | self.viewer.cam.distance = 1.2 37 | self.viewer.cam.azimuth = 180.0 38 | self.viewer.cam.elevation = -55.0 -------------------------------------------------------------------------------- /dexterous_gym/envs/pen_spin.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from gym.envs.robotics.hand.manipulate import HandPenEnv 3 | 4 | class PenSpin(HandPenEnv): 5 | def __init__(self, direction=1, alpha=1.0): 6 | self.direction = direction #-1 or 1 7 | self.alpha = alpha 8 | super(PenSpin, self).__init__() 9 | self.bottom_id = self.sim.model.site_name2id("object:bottom") 10 | self.top_id = self.sim.model.site_name2id("object:top") 11 | self._max_episode_steps = 250 12 | self.observation_space = self.observation_space["observation"] 13 | 14 | def step(self, action): 15 | action = np.clip(action, self.action_space.low, self.action_space.high) 16 | self._set_action(action) 17 | self.sim.step() 18 | self._step_callback() 19 | obs = self._get_obs() 20 | done = False 21 | info = {} 22 | reward = self.compute_reward() 23 | return obs["observation"], reward, done, info 24 | 25 | def compute_reward(self): 26 | bottom_z = self.sim.data.site_xpos[self.bottom_id].ravel()[-1] 27 | top_z = self.sim.data.site_xpos[self.top_id].ravel()[-1] 28 | reward_1 = -15 * np.abs(bottom_z - top_z) # want pen to be in x-y plane 29 | #if top_z < 0.12: 30 | # reward_1 -= 10.0 31 | #if bottom_z < 0.12: 32 | # reward_1 -= 10.0 33 | qvel = self.sim.data.get_joint_qvel('object:joint') 34 | reward_2 = self.direction * qvel[3] 35 | return self.alpha * reward_2 + reward_1 36 | 37 | def reset(self): 38 | did_reset_sim = False 39 | while not did_reset_sim: 40 | did_reset_sim = self._reset_sim() 41 | self.goal = np.array([10000,10000,10000,0,0,0,0]) #hide offscreen 42 | obs = self._get_obs()["observation"] 43 | return obs 44 | -------------------------------------------------------------------------------- /dexterous_gym/envs/stls/hand/F1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/stls/hand/F1.stl -------------------------------------------------------------------------------- /dexterous_gym/envs/stls/hand/F2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/stls/hand/F2.stl -------------------------------------------------------------------------------- /dexterous_gym/envs/stls/hand/F3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/stls/hand/F3.stl -------------------------------------------------------------------------------- /dexterous_gym/envs/stls/hand/TH1_z.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/stls/hand/TH1_z.stl -------------------------------------------------------------------------------- /dexterous_gym/envs/stls/hand/TH2_z.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/stls/hand/TH2_z.stl -------------------------------------------------------------------------------- /dexterous_gym/envs/stls/hand/TH3_z.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/stls/hand/TH3_z.stl -------------------------------------------------------------------------------- /dexterous_gym/envs/stls/hand/forearm_electric.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/stls/hand/forearm_electric.stl -------------------------------------------------------------------------------- /dexterous_gym/envs/stls/hand/forearm_electric_cvx.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/stls/hand/forearm_electric_cvx.stl -------------------------------------------------------------------------------- /dexterous_gym/envs/stls/hand/knuckle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/stls/hand/knuckle.stl -------------------------------------------------------------------------------- /dexterous_gym/envs/stls/hand/lfmetacarpal.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/stls/hand/lfmetacarpal.stl -------------------------------------------------------------------------------- /dexterous_gym/envs/stls/hand/palm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/stls/hand/palm.stl -------------------------------------------------------------------------------- /dexterous_gym/envs/stls/hand/wrist.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/stls/hand/wrist.stl -------------------------------------------------------------------------------- /dexterous_gym/envs/textures/block.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/textures/block.png -------------------------------------------------------------------------------- /dexterous_gym/envs/textures/block_hidden.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/envs/textures/block_hidden.png -------------------------------------------------------------------------------- /dexterous_gym/envs/two_block_catch_underarm.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/two_block_catch_underarm.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.97, 1.03], [0.38, 0.45], [0.15, 0.35]]) 9 | DEFAULT_RANGE_2 = np.array([[0.97, 1.03], [0.90, 0.97], [0.15, 0.35]]) 10 | 11 | class TwoBlockCatchUnderarm(TwoHandsEnv, utils.EzPickle): 12 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 13 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 14 | rotation_threshold=0.1, dist_multiplier=50.0): 15 | self.dist_multiplier = dist_multiplier 16 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 17 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_rotation=target_rotation, 18 | target_position_range_1=target_range_1, target_position_range_2=target_range_2, 19 | reward_type=reward_type, distance_threshold=distance_threshold, moving_hands=True, 20 | rotation_threshold=rotation_threshold, break_up_obs=break_up_obs, two_objects=True) 21 | 22 | def compute_reward(self, achieved_goal, desired_goal, info): 23 | if self.reward_type == 'sparse': 24 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 25 | return (success - 1.0) 26 | else: 27 | (d_pos_1, d_rot_1), (d_pos_2, d_rot_2) = self._goal_distance(achieved_goal, desired_goal) 28 | dist_1 = self.dist_multiplier*d_pos_1 + d_rot_1 29 | dist_2 = self.dist_multiplier*d_pos_2 + d_rot_2 30 | return np.exp(-0.2*dist_1) + np.exp(-0.2*dist_2) 31 | 32 | def _viewer_setup(self): 33 | # body_id = self.sim.model.body_name2id('robot0:palm') 34 | middle_id = self.sim.model.site_name2id('centre-point') 35 | # lookat = self.sim.data.body_xpos[body_id] 36 | lookat = self.sim.data.site_xpos[middle_id] 37 | for idx, value in enumerate(lookat): 38 | self.viewer.cam.lookat[idx] = value 39 | self.viewer.cam.distance = 1.5 40 | self.viewer.cam.azimuth = 180.0 41 | self.viewer.cam.elevation = -55.0 42 | -------------------------------------------------------------------------------- /dexterous_gym/envs/two_egg_catch_underarm.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/two_egg_catch_underarm.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.97, 1.03], [0.38, 0.45], [0.15, 0.35]]) 9 | DEFAULT_RANGE_2 = np.array([[0.97, 1.03], [0.90, 0.97], [0.15, 0.35]]) 10 | 11 | class TwoEggCatchUnderarm(TwoHandsEnv, utils.EzPickle): 12 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 13 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 14 | rotation_threshold=0.1, dist_multiplier=50.0): 15 | self.dist_multiplier = dist_multiplier 16 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 17 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_rotation=target_rotation, 18 | target_position_range_1=target_range_1, target_position_range_2=target_range_2, 19 | reward_type=reward_type, distance_threshold=distance_threshold, moving_hands=True, 20 | rotation_threshold=rotation_threshold, break_up_obs=break_up_obs, two_objects=True) 21 | 22 | def compute_reward(self, achieved_goal, desired_goal, info): 23 | if self.reward_type == 'sparse': 24 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 25 | return (success - 1.0) 26 | else: 27 | (d_pos_1, d_rot_1), (d_pos_2, d_rot_2) = self._goal_distance(achieved_goal, desired_goal) 28 | dist_1 = self.dist_multiplier*d_pos_1 + d_rot_1 29 | dist_2 = self.dist_multiplier*d_pos_2 + d_rot_2 30 | return np.exp(-0.2*dist_1) + np.exp(-0.2*dist_2) 31 | 32 | def _viewer_setup(self): 33 | # body_id = self.sim.model.body_name2id('robot0:palm') 34 | middle_id = self.sim.model.site_name2id('centre-point') 35 | # lookat = self.sim.data.body_xpos[body_id] 36 | lookat = self.sim.data.site_xpos[middle_id] 37 | for idx, value in enumerate(lookat): 38 | self.viewer.cam.lookat[idx] = value 39 | self.viewer.cam.distance = 1.5 40 | self.viewer.cam.azimuth = 180.0 41 | self.viewer.cam.elevation = -55.0 42 | -------------------------------------------------------------------------------- /dexterous_gym/envs/two_pen_catch_underarm.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from gym import utils 4 | from dexterous_gym.core.two_hands_env import TwoHandsEnv 5 | 6 | dir_path = os.path.dirname(os.path.realpath(__file__)) 7 | MODEL_XML = dir_path+"/hand/two_pen_catch_underarm.xml" 8 | DEFAULT_RANGE_1 = np.array([[0.97, 1.03], [0.38, 0.45], [0.15, 0.35]]) 9 | DEFAULT_RANGE_2 = np.array([[0.97, 1.03], [0.90, 0.97], [0.15, 0.35]]) 10 | 11 | class TwoPenCatchUnderarm(TwoHandsEnv, utils.EzPickle): 12 | def __init__(self, target_rotation='xyz', reward_type='dense', target_range_1=DEFAULT_RANGE_1, 13 | target_range_2=DEFAULT_RANGE_2, break_up_obs=False, distance_threshold=0.01, 14 | rotation_threshold=0.1, dist_multiplier=50.0): 15 | self.dist_multiplier = dist_multiplier 16 | utils.EzPickle.__init__(self, 'random', target_rotation, reward_type) 17 | TwoHandsEnv.__init__(self, model_path=MODEL_XML, initial_qpos=None, target_rotation=target_rotation, 18 | target_position_range_1=target_range_1, target_position_range_2=target_range_2, 19 | reward_type=reward_type, distance_threshold=distance_threshold, moving_hands=True, 20 | rotation_threshold=rotation_threshold, break_up_obs=break_up_obs, two_objects=True, 21 | randomise_initial_rotation=False) 22 | 23 | def compute_reward(self, achieved_goal, desired_goal, info): 24 | if self.reward_type == 'sparse': 25 | success = self._is_success(achieved_goal, desired_goal).astype(np.float32) 26 | return (success - 1.0) 27 | else: 28 | (d_pos_1, d_rot_1), (d_pos_2, d_rot_2) = self._goal_distance(achieved_goal, desired_goal) 29 | dist_1 = self.dist_multiplier*d_pos_1 + d_rot_1 30 | dist_2 = self.dist_multiplier*d_pos_2 + d_rot_2 31 | return np.exp(-0.2*dist_1) + np.exp(-0.2*dist_2) 32 | 33 | def _viewer_setup(self): 34 | # body_id = self.sim.model.body_name2id('robot0:palm') 35 | middle_id = self.sim.model.site_name2id('centre-point') 36 | # lookat = self.sim.data.body_xpos[body_id] 37 | lookat = self.sim.data.site_xpos[middle_id] 38 | for idx, value in enumerate(lookat): 39 | self.viewer.cam.lookat[idx] = value 40 | self.viewer.cam.distance = 1.5 41 | self.viewer.cam.azimuth = 180.0 42 | self.viewer.cam.elevation = -55.0 43 | -------------------------------------------------------------------------------- /dexterous_gym/examples/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/examples/__init__.py -------------------------------------------------------------------------------- /dexterous_gym/examples/eggcatch.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/examples/eggcatch.gif -------------------------------------------------------------------------------- /dexterous_gym/examples/eggcatchoverarm_still.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/examples/eggcatchoverarm_still.png -------------------------------------------------------------------------------- /dexterous_gym/examples/egghandover.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/examples/egghandover.gif -------------------------------------------------------------------------------- /dexterous_gym/examples/penspin.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/examples/penspin.gif -------------------------------------------------------------------------------- /dexterous_gym/examples/test_all_envs.py: -------------------------------------------------------------------------------- 1 | import gym 2 | import time 3 | import dexterous_gym 4 | 5 | envs = [ 6 | 'EggHandOver-v0', 7 | 'BlockHandOver-v0', 8 | 'PenHandOver-v0', 9 | 'EggHandOverSparse-v0', 10 | 'BlockHandOverSparse-v0', 11 | 'PenHandOverSparse-v0', 12 | 'EggCatchUnderarm-v0', 13 | 'BlockCatchUnderarm-v0', 14 | 'PenCatchUnderarm-v0', 15 | 'EggCatchUnderarmHard-v0', 16 | 'BlockCatchUnderarmHard-v0', 17 | 'PenCatchUnderarmHard-v0', 18 | 'EggCatchUnderarmSparse-v0', 19 | 'BlockCatchUnderarmSparse-v0', 20 | 'PenCatchUnderarmSparse-v0', 21 | 'EggCatchUnderarmHardSparse-v0', 22 | 'BlockCatchUnderarmHardSparse-v0', 23 | 'PenCatchUnderarmHardSparse-v0', 24 | 'TwoEggCatchUnderArm-v0', 25 | 'TwoBlockCatchUnderArm-v0', 26 | 'TwoPenCatchUnderArm-v0', 27 | 'TwoEggCatchUnderArmSparse-v0', 28 | 'TwoBlockCatchUnderArmSparse-v0', 29 | 'TwoPenCatchUnderArmSparse-v0', 30 | 'EggCatchOverarm-v0', 31 | 'BlockCatchOverarm-v0', 32 | 'PenCatchOverarm-v0', 33 | 'EggCatchOverarmSparse-v0', 34 | 'BlockCatchOverarmSparse-v0', 35 | 'PenCatchOverarmSparse-v0', 36 | 'PenSpin-v0' 37 | ] 38 | 39 | for name in envs: 40 | 41 | env = gym.make(name) 42 | print(name) 43 | 44 | for j in range(1): 45 | obs = env.reset() 46 | env.render() 47 | r_sum = 0.0 48 | for i in range(50): 49 | obs, r, d, info = env.step(env.action_space.sample()) 50 | r_sum += r 51 | time.sleep(0.07) 52 | env.render() 53 | print(r_sum) 54 | env.close() -------------------------------------------------------------------------------- /dexterous_gym/examples/twoeggcatch_error.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrycharlesworth/dexterous-gym/dd047ad7f47ba78e2a4e2642e39bc0c0b778e594/dexterous_gym/examples/twoeggcatch_error.gif -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | import setuptools 3 | 4 | setup(name='dexterous_gym', 5 | version='0.1.6', 6 | description='Challenging extensions to openAI Gyms hand manipulation environments', 7 | url='http://github.com/henrycharlesworth/dexterous_gym', 8 | author='Henry Charlesworth', 9 | author_email='H.Charlesworth@warwick.ac.uk', 10 | packages=setuptools.find_packages(), 11 | package_data={'dexterous_gym.envs': [ 12 | 'hand/*', 13 | 'stls/hand/*', 14 | 'textures/*' 15 | ]}, 16 | zip_safe=False) 17 | --------------------------------------------------------------------------------