diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml index 64f60f82..15806af6 100644 --- a/.github/workflows/python-app.yml +++ b/.github/workflows/python-app.yml @@ -5,9 +5,9 @@ name: Python application on: push: - branches: [ "main" ] + branches: [ "main" , "dev" ] pull_request: - branches: [ "main" ] + branches: [ "main" , "dev" ] permissions: contents: read @@ -15,6 +15,9 @@ permissions: jobs: build: + env: + DISPLAY: :0 + ROBOHIVE_TEST: LITE runs-on: ubuntu-latest @@ -33,6 +36,14 @@ jobs: libosmesa6-dev \ software-properties-common sudo apt-get install -y patchelf + sudo apt-get update -y -qq + sudo apt-get install -y xvfb x11-xserver-utils + sudo /usr/bin/Xvfb $DISPLAY -screen 0 1280x1024x24 & + + - name: Install ffmpeg + run: | + sudo apt-get install --no-install-recommends ffmpeg && pip3 install ffmpeg scikit-video + - name: Checkout submodules run: git submodule update --init --recursive - name: Install dependencies @@ -41,6 +52,10 @@ jobs: pip install -e ".[mujoco,encoder]" pip install 'r3m@git+https://github.com/facebookresearch/r3m.git' pip install 'vc_models@git+https://github.com/facebookresearch/eai-vc.git@9958b278666bcbde193d665cc0df9ccddcdb8a5a#egg=vc_models&subdirectory=vc_models' + + - name: Run MuJoCo Rendering test + run: python3 -m mujoco.render_test + - name: Test with pytest run: | python robohive/tests/test_all.py diff --git a/.gitignore b/.gitignore index 5eeada76..735c5ce0 100644 --- a/.gitignore +++ b/.gitignore @@ -49,6 +49,7 @@ coverage.xml *.cover .hypothesis/ .pytest_cache/ +*.nbconvert.ipynb # Translations *.mo @@ -75,6 +76,7 @@ target/ # Jupyter Notebook .ipynb_checkpoints +*.nbconvert.ipynb # pyenv .python-version diff --git a/README.md b/README.md index 79d66695..a5aaa0f1 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,7 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use th [![Downloads](https://pepy.tech/badge/robohive)](https://pepy.tech/project/robohive) [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/drive/1rdSgnsfUaE-eFLjAkFHeqfUWzAK8ruTs?usp=sharing) [![Slack](https://img.shields.io/badge/Slack-4A154B?style=for-the-badge&logo=slack&logoColor=white)](https://robohiveworkspace.slack.com) +[![Documentation](https://img.shields.io/static/v1?label=Wiki&message=Documentation&color= + diff --git a/robohive/envs/arms/franka/assets/franka_reach_v0.xml b/robohive/envs/arms/franka/assets/franka_reach_v0.xml index 1daee8ff..650509c2 100644 --- a/robohive/envs/arms/franka/assets/franka_reach_v0.xml +++ b/robohive/envs/arms/franka/assets/franka_reach_v0.xml @@ -21,7 +21,7 @@ - + diff --git a/robohive/envs/arms/franka/assets/franka_ycb_v0.xml b/robohive/envs/arms/franka/assets/franka_ycb_v0.xml index d18ca51a..ae2b1ce7 100644 --- a/robohive/envs/arms/franka/assets/franka_ycb_v0.xml +++ b/robohive/envs/arms/franka/assets/franka_ycb_v0.xml @@ -26,6 +26,7 @@ + diff --git a/robohive/envs/arms/pick_place_v0.py b/robohive/envs/arms/pick_place_v0.py index 3b7d8227..298dc187 100644 --- a/robohive/envs/arms/pick_place_v0.py +++ b/robohive/envs/arms/pick_place_v0.py @@ -15,7 +15,10 @@ class PickPlaceV0(env_base.MujocoEnv): DEFAULT_OBS_KEYS = [ - 'qp', 'qv', 'object_err', 'target_err' + 'qp_robot', 'qp_object', 'qv_robot', 'qv_object', 'object_err', 'target_err' + ] + DEFAULT_PROPRIO_KEYS = [ + 'qp_robot', 'qp_object', 'qv_robot', 'qv_object' ] DEFAULT_RWD_KEYS_AND_WEIGHTS = { "object_dist": -1.0, @@ -44,6 +47,7 @@ def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs): def _setup(self, + robot_ndof, robot_site_name, object_site_name, target_site_name, @@ -57,6 +61,7 @@ def _setup(self, **kwargs, ): + self.robot_ndof = robot_ndof # ids self.grasp_sid = self.sim.model.site_name2id(robot_site_name) self.object_sid = self.sim.model.site_name2id(object_site_name) @@ -76,8 +81,10 @@ def _setup(self, def get_obs_dict(self, sim): obs_dict = {} obs_dict['time'] = np.array([self.sim.data.time]) - obs_dict['qp'] = sim.data.qpos.copy() - obs_dict['qv'] = sim.data.qvel.copy() + obs_dict['qp_robot'] = sim.data.qpos[:self.robot_ndof].copy() + obs_dict['qv_robot'] = sim.data.qvel[:self.robot_ndof].copy() + obs_dict['qp_object'] = sim.data.qpos[self.robot_ndof:].copy() + obs_dict['qv_object'] = sim.data.qvel[self.robot_ndof:].copy() obs_dict['object_err'] = sim.data.site_xpos[self.object_sid]-sim.data.site_xpos[self.grasp_sid] obs_dict['target_err'] = sim.data.site_xpos[self.target_sid]-sim.data.site_xpos[self.object_sid] return obs_dict diff --git a/robohive/envs/arms/push_base_v0.py b/robohive/envs/arms/push_base_v0.py index 56f9985b..fe0ba699 100644 --- a/robohive/envs/arms/push_base_v0.py +++ b/robohive/envs/arms/push_base_v0.py @@ -14,7 +14,10 @@ class PushBaseV0(env_base.MujocoEnv): DEFAULT_OBS_KEYS = [ - 'qp', 'qv', 'object_err', 'target_err' + 'qp_robot', 'qp_object', 'qv_robot', 'qv_object', 'object_err', 'target_err' + ] + DEFAULT_PROPRIO_KEYS = [ + 'qp_robot', 'qp_object', 'qv_robot', 'qv_object' ] DEFAULT_RWD_KEYS_AND_WEIGHTS = { "object_dist": -1.0, @@ -43,6 +46,7 @@ def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs): def _setup(self, + robot_ndof, robot_site_name, object_site_name, target_site_name, @@ -50,10 +54,11 @@ def _setup(self, frame_skip=40, reward_mode="dense", obs_keys=DEFAULT_OBS_KEYS, + proprio_keys=DEFAULT_PROPRIO_KEYS, weighted_reward_keys=DEFAULT_RWD_KEYS_AND_WEIGHTS, **kwargs, ): - + self.robot_ndof = robot_ndof # ids self.grasp_sid = self.sim.model.site_name2id(robot_site_name) self.object_sid = self.sim.model.site_name2id(object_site_name) @@ -61,6 +66,7 @@ def _setup(self, self.target_xyz_range = target_xyz_range super()._setup(obs_keys=obs_keys, + proprio_keys=proprio_keys, weighted_reward_keys=weighted_reward_keys, reward_mode=reward_mode, frame_skip=frame_skip, @@ -69,8 +75,10 @@ def _setup(self, def get_obs_dict(self, sim): obs_dict = {} obs_dict['time'] = np.array([self.sim.data.time]) - obs_dict['qp'] = sim.data.qpos.copy() - obs_dict['qv'] = sim.data.qvel.copy() + obs_dict['qp_robot'] = sim.data.qpos[:self.robot_ndof].copy() + obs_dict['qv_robot'] = sim.data.qvel[:self.robot_ndof].copy() + obs_dict['qp_object'] = sim.data.qpos[self.robot_ndof:].copy() + obs_dict['qv_object'] = sim.data.qvel[self.robot_ndof:].copy() obs_dict['object_err'] = sim.data.site_xpos[self.object_sid]-sim.data.site_xpos[self.grasp_sid] obs_dict['target_err'] = sim.data.site_xpos[self.target_sid]-sim.data.site_xpos[self.object_sid] return obs_dict diff --git a/robohive/envs/arms/reach_base_v0.py b/robohive/envs/arms/reach_base_v0.py index 22547c14..6a78e031 100644 --- a/robohive/envs/arms/reach_base_v0.py +++ b/robohive/envs/arms/reach_base_v0.py @@ -15,7 +15,10 @@ class ReachBaseV0(env_base.MujocoEnv): DEFAULT_OBS_KEYS = [ - 'qp', 'qv', 'reach_err' + 'qp_robot', 'qv_robot', 'reach_err' + ] + DEFAULT_PROPRIO_KEYS = [ + 'qp_robot', 'qv_robot' ] DEFAULT_RWD_KEYS_AND_WEIGHTS = { "reach": -1.0, @@ -50,6 +53,7 @@ def _setup(self, frame_skip = 40, reward_mode = "dense", obs_keys=DEFAULT_OBS_KEYS, + proprio_keys=DEFAULT_PROPRIO_KEYS, weighted_reward_keys=DEFAULT_RWD_KEYS_AND_WEIGHTS, **kwargs, ): @@ -60,6 +64,7 @@ def _setup(self, self.target_xyz_range = target_xyz_range super()._setup(obs_keys=obs_keys, + proprio_keys=proprio_keys, weighted_reward_keys=weighted_reward_keys, reward_mode=reward_mode, frame_skip=frame_skip, @@ -69,8 +74,8 @@ def _setup(self, def get_obs_dict(self, sim): obs_dict = {} obs_dict['time'] = np.array([self.sim.data.time]) - obs_dict['qp'] = sim.data.qpos.copy() - obs_dict['qv'] = sim.data.qvel.copy() + obs_dict['qp_robot'] = sim.data.qpos.copy() + obs_dict['qv_robot'] = sim.data.qvel.copy() obs_dict['reach_err'] = sim.data.site_xpos[self.target_sid]-sim.data.site_xpos[self.grasp_sid] return obs_dict diff --git a/robohive/envs/claws/__init__.py b/robohive/envs/claws/__init__.py index b46a04e7..1b78c421 100644 --- a/robohive/envs/claws/__init__.py +++ b/robohive/envs/claws/__init__.py @@ -49,11 +49,12 @@ def register_visual_envs(env_id, encoder_type): env_id=env_id, variant_id=env_id[:-3]+'_v'+encoder_type+env_id[-3:], variants={'obs_keys': - ['qp', 'qv', - "rgb:center_cam:224x224:{}".format(encoder_type)] + ['qp', 'qv'], + 'visual_keys':[ + "rgb:center_cam:224x224:{}".format(encoder_type),] }, silent=True ) for env in ['TrifingerReachFixed-v0', 'TrifingerReachRandom-v0']: - for enc in ["r3m18", "r3m34", "r3m50", "rrl18", "rrl34", "rrl50", "flat"]: + for enc in ["r3m18", "r3m34", "r3m50", "rrl18", "rrl34", "rrl50", "2d"]: register_visual_envs(env, enc) diff --git a/robohive/envs/env_base.py b/robohive/envs/env_base.py index 0834f7ff..0c29f980 100644 --- a/robohive/envs/env_base.py +++ b/robohive/envs/env_base.py @@ -10,7 +10,7 @@ import os import time as timer -from robohive.envs.obj_vec_dict import ObsVecDict +from robohive.envs.obs_vec_dict import ObsVecDict from robohive.utils import tensor_utils from robohive.robot.robot import Robot from robohive.utils.prompt_utils import prompt, Prompt @@ -48,7 +48,7 @@ def __init__(self, model_path, obsd_model_path=None, seed=None, env_credits=DEF """ prompt("RoboHive:> For environment credits, please cite -") - prompt(env_credits, color="cyan", type=Prompt.INFO) + prompt(env_credits, color="cyan", type=Prompt.ONCE) # Seed and initialize the random number generator self.seed(seed) @@ -134,8 +134,7 @@ def _setup(self, # Question: Should we replace above with following? Its specially helpful for hardware as it forces a env reset before continuing, without which the hardware will make a big jump from its position to the position asked by step. # observation = self.reset() assert not done, "Check initialization. Simulation starts in a done state." - self.obs_dim = observation.size - self.observation_space = gym.spaces.Box(obs_range[0]*np.ones(self.obs_dim), obs_range[1]*np.ones(self.obs_dim), dtype=np.float32) + self.observation_space = gym.spaces.Box(obs_range[0]*np.ones(observation.size), obs_range[1]*np.ones(observation.size), dtype=np.float32) return @@ -352,9 +351,22 @@ def get_visuals(self, sim=None, visual_keys:list=None, device_id:int=None)->dict visual_dict['time'] = np.array([self.sim.data.time]) for key in visual_keys: if key.startswith('rgb'): - _, cam, wxh, rgb_encoder_id = key.split(':') + + # _, cam, wxh, rgb_encoder_id = key.split(':') # faces issues if camera names have : in them + + # get encoder + key_payload = key[4:] + rgb_encoder_id = key_payload.split(':')[-1] + + # get image resolution + key_payload = key_payload[:-(len(rgb_encoder_id)+1)] + wxh = key_payload.split(':')[-1] height = int(wxh.split('x')[0]) width = int(wxh.split('x')[1]) + + # get camera name + cam = key_payload[:-(len(wxh)+1)] + # render images ==> returns (ncams, height, width, 3) img, dpt = self.robot.get_visual_sensors( height=height, @@ -386,7 +398,7 @@ def get_visuals(self, sim=None, visual_keys:list=None, device_id:int=None)->dict visual_dict.update({key:rgb_encoded}) # add depth observations if requested in the keys (assumption d will always be accompanied by rgb keys) d_key = 'd:'+key[4:] - if d_key in self.obs_keys: + if d_key in visual_keys: visual_dict.update({d_key:dpt}) return visual_dict @@ -629,7 +641,7 @@ def mj_render(self): self.sim.renderer.render_to_window() - def viewer_setup(self, distance=2.5, azimuth=90, elevation=-30, lookat=None): + def viewer_setup(self, distance=2.5, azimuth=90, elevation=-30, lookat=None, render_actuator=None, render_tendon=None): """ Setup the default camera """ @@ -639,6 +651,10 @@ def viewer_setup(self, distance=2.5, azimuth=90, elevation=-30, lookat=None): elevation=elevation, lookat=lookat ) + self.sim.renderer.set_viewer_settings( + render_actuator=render_actuator, + render_tendon=render_tendon + ) def examine_policy(self, @@ -833,7 +849,7 @@ def examine_policy_new(self, skvideo.io.vwrite(file_name, np.asarray(frames),outputdict={"-pix_fmt": "yuv420p"}) else: skvideo.io.vwrite(file_name, np.asarray(frames)) - prompt("saved: "+file_name, type=Prompt.INFO) + prompt("saved: "+file_name, type=Prompt.ALWAYS) self.mujoco_render_frames = False prompt("Total time taken = %f"% (timer.time()-exp_t0), type=Prompt.INFO) diff --git a/robohive/envs/hands/__init__.py b/robohive/envs/hands/__init__.py index 427453a6..11001702 100644 --- a/robohive/envs/hands/__init__.py +++ b/robohive/envs/hands/__init__.py @@ -14,7 +14,7 @@ # ================================================================================== # V1 envs: -# - env_base class independet of mjrl, making it self contained +# - env_base class independent of mjrl, making it self contained # - Updated obs such that rwd are recoverable from obs # - Vectorized obs and rwd calculations # ================================================================================== @@ -52,7 +52,7 @@ ) from robohive.envs.hands.pen_v1 import PenEnvV1 -# Relcoate an object to the target +# Relocate an object to the target register( id='relocate-v1', entry_point='robohive.envs.hands:RelocateEnvV1', @@ -90,44 +90,34 @@ def register_visual_envs(env_name, encoder_type): # ================================================================================== -# V0: Old Baoding ball +# Baoding ball # ================================================================================== - -# V0: Old Baoding ball -# register( -# id='baoding-v0', -# entry_point='robohive.envs.hands:BaodingEnvV0', -# max_episode_steps=200, -# ) -# from robohive.envs.hands.baoding_v0 import BaodingEnvV0 - -# V0: baoding balls new register( id='baoding-v1', entry_point='robohive.envs.hands:BaodingFixedEnvV1', max_episode_steps=200, - kwargs={ - 'model_path': curr_dir+'/assets/baoding_v1.mjb', - } + kwargs={ + 'model_path': curr_dir+'/assets/PDDM_baoding.xml', + } ) from robohive.envs.hands.baoding_v1 import BaodingFixedEnvV1 register( id='baoding4th-v1', entry_point='robohive.envs.hands:BaodingFixedEnvV1', max_episode_steps=200, - kwargs={ - 'model_path': curr_dir+'/assets/baoding_v1.mjb', + kwargs={ + 'model_path': curr_dir+'/assets/PDDM_baoding.xml', 'n_shifts_per_period':4, - } + } ) register( id='baoding8th-v1', entry_point='robohive.envs.hands:BaodingFixedEnvV1', max_episode_steps=200, - kwargs={ - 'model_path': curr_dir+'/assets/baoding_v1.mjb', + kwargs={ + 'model_path': curr_dir+'/assets/PDDM_baoding.xml', 'n_shifts_per_period':8, - } + } ) from robohive.envs.hands.baoding_v1 import BaodingFixedEnvV1 diff --git a/robohive/envs/hands/assets/DAPG_assets.xml b/robohive/envs/hands/assets/DAPG_assets.xml index 6ee0baff..82bba9ba 100644 --- a/robohive/envs/hands/assets/DAPG_assets.xml +++ b/robohive/envs/hands/assets/DAPG_assets.xml @@ -69,8 +69,8 @@ - - + + diff --git a/robohive/envs/hands/assets/PDDM_baoding.xml b/robohive/envs/hands/assets/PDDM_baoding.xml new file mode 100644 index 00000000..95b35c97 --- /dev/null +++ b/robohive/envs/hands/assets/PDDM_baoding.xml @@ -0,0 +1,96 @@ + + + + + + + \ No newline at end of file diff --git a/robohive/envs/hands/assets/baoding_v1.mjb b/robohive/envs/hands/assets/baoding_v1.mjb deleted file mode 100644 index de3b44fe..00000000 Binary files a/robohive/envs/hands/assets/baoding_v1.mjb and /dev/null differ diff --git a/robohive/envs/hands/baoding_v1.py b/robohive/envs/hands/baoding_v1.py index 932abf1f..1e643916 100644 --- a/robohive/envs/hands/baoding_v1.py +++ b/robohive/envs/hands/baoding_v1.py @@ -11,6 +11,7 @@ import numpy as np from robohive.envs import env_base +from robohive.utils.quat_math import rotVecQuat # Define the task enum class Task(enum.Enum): @@ -75,6 +76,11 @@ def _setup(self, **kwargs, ): + # relax motor gains + for sim in [self.sim, self.sim_obsd]: + sim.model.actuator_gainprm[:] *= 0.1 + sim.model.actuator_biasprm[:] *= 0.1 + # user parameters self.which_task = Task(WHICH_TASK) self.drop_height_threshold = 0.06 @@ -96,6 +102,7 @@ def _setup(self, self.goal = self.create_goal_trajectory(time_step=frame_skip*self.sim.model.opt.timestep, time_period=6) # init target and body sites + self.palm_bid = self.sim.model.body_name2id('palm') self.object1_sid = self.sim.model.site_name2id('ball1_site') self.object2_sid = self.sim.model.site_name2id('ball2_site') self.target1_sid = self.sim.model.site_name2id('target1_site') @@ -140,7 +147,7 @@ def _setup(self, # pos_bounds=[[-40, 40]] * self.n_dofs, #dummy # vel_bounds=[[-3, 3]] * self.n_dofs, - def step(self, a): + def step(self, a, **kwargs): if self.which_task==Task.MOVE_TO_LOCATION: desired_pos = self.goal[self.counter].copy() # update both simhive with desired targets @@ -150,31 +157,35 @@ def step(self, a): sim.model.site_pos[self.target1_sid, 1] = desired_pos[1] sim.model.site_pos[self.target1_sid, 2] = desired_pos[2]+0.02 else : + # get desired targets desired_angle_wrt_palm = self.goal[self.counter].copy() desired_angle_wrt_palm[0] = desired_angle_wrt_palm[0] + self.ball_1_starting_angle desired_angle_wrt_palm[1] = desired_angle_wrt_palm[1] + self.ball_2_starting_angle - desired_positions_wrt_palm = [0,0,0,0] - desired_positions_wrt_palm[0] = self.x_radius*np.cos(desired_angle_wrt_palm[0]) + self.center_pos[0] - desired_positions_wrt_palm[1] = self.y_radius*np.sin(desired_angle_wrt_palm[0]) + self.center_pos[1] - desired_positions_wrt_palm[2] = self.x_radius*np.cos(desired_angle_wrt_palm[1]) + self.center_pos[0] - desired_positions_wrt_palm[3] = self.y_radius*np.sin(desired_angle_wrt_palm[1]) + self.center_pos[1] + ball1_desired_pos_wrt_palm = np.array([ + self.x_radius*np.cos(desired_angle_wrt_palm[0]) + self.center_pos[0], + -0.045, + self.y_radius*np.sin(desired_angle_wrt_palm[0]) + self.center_pos[1] + ]) + ball2_desired_pos_wrt_palm = np.array([ + self.x_radius*np.cos(desired_angle_wrt_palm[1]) + self.center_pos[0], + -0.045, + self.y_radius*np.sin(desired_angle_wrt_palm[1]) + self.center_pos[1] + ]) # update both simhive with desired targets + palm_pos = self.sim.data.body_xpos[self.palm_bid] + palm_quat = self.sim.data.body_xquat[self.palm_bid] + for sim in [self.sim, self.sim_obsd]: - sim.model.site_pos[self.target1_sid, 0] = desired_positions_wrt_palm[0] - sim.model.site_pos[self.target1_sid, 2] = desired_positions_wrt_palm[1] - sim.model.site_pos[self.target2_sid, 0] = desired_positions_wrt_palm[2] - sim.model.site_pos[self.target2_sid, 2] = desired_positions_wrt_palm[3] - # move upward, to be seen - sim.model.site_pos[self.target1_sid, 1] = -0.07 - sim.model.site_pos[self.target2_sid, 1] = -0.07 + sim.model.site_pos[self.target1_sid,:] = palm_pos + rotVecQuat(ball1_desired_pos_wrt_palm, palm_quat) + sim.model.site_pos[self.target2_sid,:] = palm_pos + rotVecQuat(ball2_desired_pos_wrt_palm, palm_quat) self.counter +=1 # V0: mean center and scaled differently # a[a>0] = self.act_mid[a>0] + a[a>0]*self.upper_rng[a>0] # a[a<=0] = self.act_mid[a<=0] + a[a<=0]*self.lower_rng[a<=0] - return super().step(a) + return super().step(a, **kwargs) def get_obs_dict(self, sim): obs_dict = {} @@ -208,7 +219,10 @@ def get_reward_dict(self, obs_dict): target1_dist = np.linalg.norm(obs_dict['target1_err'], axis=-1) target2_dist = np.linalg.norm(obs_dict['target2_err'], axis=-1) target_dist = target1_dist if self.which_task==Task.MOVE_TO_LOCATION else (target1_dist+target2_dist) - act_mag = np.linalg.norm(self.obs_dict['act'], axis=-1)/self.sim.model.na if self.sim.model.na !=0 else 0 + if self.sim.model.na ==0: + act_mag = np.array([[0]]) if obs_dict['hand_pos'].ndim==3 else 0 + else: + act_mag = np.linalg.norm(self.obs_dict['act'], axis=-1)/self.sim.model.na # wrist pose err (New in V1) hand_pos = obs_dict['hand_pos'][:,:,:3] if obs_dict['hand_pos'].ndim==3 else obs_dict['hand_pos'][:3] diff --git a/robohive/envs/hands/door_v0.py b/robohive/envs/hands/door_v0.py index 3fc78e78..d53e25e9 100644 --- a/robohive/envs/hands/door_v0.py +++ b/robohive/envs/hands/door_v0.py @@ -10,7 +10,7 @@ from mjrl.envs import mujoco_env from mujoco_py import MjViewer import os -from envs.obj_vec_dict import ObsVecDict +from robohive.envs.obs_vec_dict import ObsVecDict import collections # NOTES: diff --git a/robohive/envs/hands/door_v1.py b/robohive/envs/hands/door_v1.py index e3f3c414..877a84de 100644 --- a/robohive/envs/hands/door_v1.py +++ b/robohive/envs/hands/door_v1.py @@ -98,9 +98,9 @@ def get_reward_dict(self, obs_dict): def reset(self, reset_qpos=None, reset_qvel=None, **kwargs): self.sim.reset() - qp = self.init_qpos.copy() if reset_qpos==None else reset_qpos - qv = self.init_qvel.copy() if reset_qvel==None else reset_qvel - self.sim.set_state(qpos=qp, qvel=qv) + qp = self.init_qpos.copy() if reset_qpos is None else reset_qpos + qv = self.init_qvel.copy() if reset_qvel is None else reset_qvel + self.robot.reset(reset_pos=qp, reset_vel=qv, **kwargs) self.sim.model.body_pos[self.door_bid,0] = self.np_random.uniform(low=-0.3, high=-0.2) self.sim.model.body_pos[self.door_bid, 1] = self.np_random.uniform(low=0.25, high=0.35) diff --git a/robohive/envs/hands/hammer_v0.py b/robohive/envs/hands/hammer_v0.py index 77ba0ce3..080bf475 100644 --- a/robohive/envs/hands/hammer_v0.py +++ b/robohive/envs/hands/hammer_v0.py @@ -11,7 +11,7 @@ from mujoco_py import MjViewer from robohive.utils.quat_math import * import os -from envs.obj_vec_dict import ObsVecDict +from robohive.envs.obs_vec_dict import ObsVecDict import collections ADD_BONUS_REWARDS = True diff --git a/robohive/envs/hands/hammer_v1.py b/robohive/envs/hands/hammer_v1.py index 313db6f7..a7607b1d 100644 --- a/robohive/envs/hands/hammer_v1.py +++ b/robohive/envs/hands/hammer_v1.py @@ -122,9 +122,9 @@ def get_obs_dict(self, sim): def reset(self, reset_qpos=None, reset_qvel=None, **kwargs): self.sim.reset() - qp = self.init_qpos.copy() if reset_qpos==None else reset_qpos - qv = self.init_qvel.copy() if reset_qvel==None else reset_qvel - self.sim.set_state(qpos=qp, qvel=qv) + qp = self.init_qpos.copy() if reset_qpos is None else reset_qpos + qv = self.init_qvel.copy() if reset_qvel is None else reset_qvel + self.robot.reset(reset_pos=qp, reset_vel=qv, **kwargs) self.sim.model.body_pos[self.target_bid,2] = self.np_random.uniform(low=0.1, high=0.25) self.sim.forward() diff --git a/robohive/envs/hands/pen_v0.py b/robohive/envs/hands/pen_v0.py index df575b13..2019d9f2 100644 --- a/robohive/envs/hands/pen_v0.py +++ b/robohive/envs/hands/pen_v0.py @@ -11,7 +11,7 @@ from gym import utils from mjrl.envs import mujoco_env from robohive.utils.quat_math import euler2quat -from envs.obj_vec_dict import ObsVecDict +from robohive.envs.obs_vec_dict import ObsVecDict from mujoco_py import MjViewer OBS_KEYS = ['hand_jnt', 'obj_pos', 'obj_vel', 'obj_rot', 'obj_des_rot', 'obj_err_pos', 'obj_err_rot'] diff --git a/robohive/envs/hands/pen_v1.py b/robohive/envs/hands/pen_v1.py index 6a325192..2a4bcbcb 100644 --- a/robohive/envs/hands/pen_v1.py +++ b/robohive/envs/hands/pen_v1.py @@ -110,9 +110,9 @@ def get_reward_dict(self, obs_dict): def reset(self, reset_qpos=None, reset_qvel=None, **kwargs): self.sim.reset() - qp = self.init_qpos.copy() if reset_qpos==None else reset_qpos - qv = self.init_qvel.copy() if reset_qvel==None else reset_qvel - self.sim.set_state(qpos=qp, qvel=qv) + qp = self.init_qpos.copy() if reset_qpos is None else reset_qpos + qv = self.init_qvel.copy() if reset_qvel is None else reset_qvel + self.robot.reset(reset_pos=qp, reset_vel=qv, **kwargs) desired_orien = np.zeros(3) desired_orien[0] = self.np_random.uniform(low=-1, high=1) diff --git a/robohive/envs/hands/relocate_v0.py b/robohive/envs/hands/relocate_v0.py index 5253008c..4fbb5815 100644 --- a/robohive/envs/hands/relocate_v0.py +++ b/robohive/envs/hands/relocate_v0.py @@ -10,7 +10,7 @@ from mjrl.envs import mujoco_env from mujoco_py import MjViewer import os -from envs.obj_vec_dict import ObsVecDict +from robohive.envs.obs_vec_dict import ObsVecDict import collections ADD_BONUS_REWARDS = True diff --git a/robohive/envs/hands/relocate_v1.py b/robohive/envs/hands/relocate_v1.py index 2d295a2c..5c19b326 100644 --- a/robohive/envs/hands/relocate_v1.py +++ b/robohive/envs/hands/relocate_v1.py @@ -138,9 +138,10 @@ def get_obs_dict(self, sim): def reset(self, reset_qpos=None, reset_qvel=None, **kwargs): self.sim.reset() - qp = self.init_qpos.copy() if reset_qpos==None else reset_qpos - qv = self.init_qvel.copy() if reset_qvel==None else reset_qvel - self.sim.set_state(qpos=qp, qvel=qv) + qp = self.init_qpos.copy() if reset_qpos is None else reset_qpos + qv = self.init_qvel.copy() if reset_qvel is None else reset_qvel + self.robot.reset(reset_pos=qp, reset_vel=qv, **kwargs) + self.sim.model.body_pos[self.obj_bid,0] = self.np_random.uniform(low=-0.15, high=0.15) self.sim.model.body_pos[self.obj_bid,1] = self.np_random.uniform(low=-0.15, high=0.3) diff --git a/robohive/envs/multi_task/README.md b/robohive/envs/multi_task/README.md index afb69581..4e365916 100644 --- a/robohive/envs/multi_task/README.md +++ b/robohive/envs/multi_task/README.md @@ -8,10 +8,10 @@ This suite is designed to study generalization in multi-task settings. RoboHive' ### FrankaKitchen-v4(RoboHive) - A part of the RoboHive-v0.5 release. Designed and packaged specifically to study visual generalization. -- Introduces there env variants (fix to control default behavior) - - *Fixed-v4: State based stationary environments with no randomization - - *Random-v4: State based environments with random robot initialization (joint pose + relative position wrt to kitchen) - - *Random_v2d-v4: Visual environment with random robot initialization (joint pose + relative position wrt to kitchen) +- Introduces three env variants (fix to control env's default behavior) + - `Fixed-v4`: State based stationary environments with no randomization + - `Random-v4`: State based environments with random robot initialization (joint pose + relative position wrt to kitchen) + - `Random_v2d-v4`: Visual environment with random robot initialization (joint pose + relative position wrt to kitchen) - RoboHive introduces `get_obs()`, `get_prioprio()`, `get_extero()` features. All state envs shoud used `obs`. All visual envs should use `proprio` and `extero` data. - Init robot state recovered from([kitchen_demos_multitask-v0](https://github.com/google-research/relay-policy-learning/blob/master/kitchen_demos_multitask.zip)) - Four explicit cameras introduced to aid multi-view generalization studies -- `top`, `left`, `right`, `wrist` diff --git a/robohive/envs/multi_task/common/desk/franka_desk.xml b/robohive/envs/multi_task/common/desk/franka_desk.xml index 27976033..1e3d82be 100644 --- a/robohive/envs/multi_task/common/desk/franka_desk.xml +++ b/robohive/envs/multi_task/common/desk/franka_desk.xml @@ -37,7 +37,7 @@ - + diff --git a/robohive/envs/multi_task/common/franka_kitchen_v1.py b/robohive/envs/multi_task/common/franka_kitchen_v1.py deleted file mode 100644 index 132d344c..00000000 --- a/robohive/envs/multi_task/common/franka_kitchen_v1.py +++ /dev/null @@ -1,214 +0,0 @@ -""" ================================================= -Copyright (C) 2018 Vikash Kumar -Author :: Vikash Kumar (vikashplus@gmail.com) -Source :: https://github.com/vikashplus/robohive -License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. -================================================= """ - -import collections -import gym -import numpy as np - -from robohive.envs.multi_task.multi_task_base_v1 import KitchenBase - -# ToDo: Get these details from key_frame -DEMO_RESET_QPOS = np.array( - [ - 1.01020992e-01, - -1.76349747e00, - 1.88974607e00, - -2.47661710e00, - 3.25189114e-01, - 8.29094410e-01, - 1.62463629e00, - 3.99760380e-02, - 3.99791002e-02, - 2.45778156e-05, - 2.95590127e-07, - 2.45777410e-05, - 2.95589217e-07, - 2.45777410e-05, - 2.95589217e-07, - 2.45777410e-05, - 2.95589217e-07, - 2.16196258e-05, - 5.08073663e-06, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - -2.68999994e-01, - 3.49999994e-01, - 1.61928391e00, - 6.89039584e-19, - -2.26122120e-05, - -8.87580375e-19, - ] -) -DEMO_RESET_QVEL = np.array( - [ - -1.24094905e-02, - 3.07730486e-04, - 2.10558046e-02, - -2.11170651e-02, - 1.28676305e-02, - 2.64535546e-02, - -7.49515183e-03, - -1.34369839e-04, - 2.50969693e-04, - 1.06229627e-13, - 7.14243539e-16, - 1.06224762e-13, - 7.19794728e-16, - 1.06224762e-13, - 7.21644648e-16, - 1.06224762e-13, - 7.14243539e-16, - -1.19464428e-16, - -1.47079926e-17, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 2.93530267e-09, - -1.99505748e-18, - 3.42031125e-14, - -4.39396125e-17, - 6.64174740e-06, - 3.52969879e-18, - ] -) - -class KitchenFrankaFixed(KitchenBase): - - ENV_CREDIT = """\ - Relay Policy Learning: Solving Long-Horizon Tasks via Imitation and Reinforcement Learning - Abhishek Gupta, Vikash Kumar, Corey Lynch, Sergey Levine, Karol Hausman - CoRL-2019 | https://relay-policy-learning.github.io/ - """ - - OBJ_INTERACTION_SITES = ( - "knob1_site", - "knob2_site", - "knob3_site", - "knob4_site", - "light_site", - "slide_site", - "leftdoor_site", - "rightdoor_site", - "microhandle_site", - "kettle_site0", - "kettle_site0", - "kettle_site0", - "kettle_site0", - "kettle_site0", - "kettle_site0", - ) - - OBJ_JNT_NAMES = ( - "knob1_joint", - "knob2_joint", - "knob3_joint", - "knob4_joint", - "lightswitch_joint", - "slidedoor_joint", - "leftdoorhinge", - "rightdoorhinge", - "micro0joint", - "kettle0:Tx", - "kettle0:Ty", - "kettle0:Tz", - "kettle0:Rx", - "kettle0:Ry", - "kettle0:Rz", - ) - - ROBOT_JNT_NAMES = ( - "panda0_joint1", - "panda0_joint2", - "panda0_joint3", - "panda0_joint4", - "panda0_joint5", - "panda0_joint6", - "panda0_joint7", - "panda0_finger_joint1", - "panda0_finger_joint2", - ) - - def _setup( - self, - robot_jnt_names=ROBOT_JNT_NAMES, - obj_jnt_names=OBJ_JNT_NAMES, - obj_interaction_site=OBJ_INTERACTION_SITES, - **kwargs, - ): - super()._setup( - robot_jnt_names=robot_jnt_names, - obj_jnt_names=obj_jnt_names, - obj_interaction_site=obj_interaction_site, - **kwargs, - ) - - -class KitchenFrankaDemo(KitchenFrankaFixed): - - def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs): - # EzPickle.__init__(**locals()) is capturing the input dictionary of the init method of this class. - # In order to successfully capture all arguments we need to call gym.utils.EzPickle.__init__(**locals()) - # at the leaf level, when we do inheritance like we do here. - # kwargs is needed at the top level to account for injection of __class__ keyword. - # Also see: https://github.com/openai/gym/pull/1497 - gym.utils.EzPickle.__init__(self, model_path, obsd_model_path, seed, **kwargs) - - # This two step construction is required for pickling to work correctly. All arguments to all __init__ - # calls must be pickle friendly. Things like sim / sim_obsd are NOT pickle friendly. Therefore we - # first construct the inheritance chain, which is just __init__ calls all the way down, with env_base - # creating the sim / sim_obsd instances. Next we run through "setup" which relies on sim / sim_obsd - # created in __init__ to complete the setup. - super().__init__(model_path=model_path, obsd_model_path=obsd_model_path, seed=seed, env_credits=self.ENV_CREDIT) - - super()._setup(**kwargs) - - def reset(self, reset_qpos=None, reset_qvel=None): - if reset_qpos is None: - reset_qpos = self.init_qpos.copy() - reset_qvel = self.init_qvel.copy() - reset_qpos[self.robot_dofs] = DEMO_RESET_QPOS[self.robot_dofs] - reset_qvel[self.robot_dofs] = DEMO_RESET_QVEL[self.robot_dofs] - return super().reset(reset_qpos=reset_qpos, reset_qvel=reset_qvel) - - -class KitchenFrankaRandom(KitchenFrankaFixed): - - def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs): - # EzPickle.__init__(**locals()) is capturing the input dictionary of the init method of this class. - # In order to successfully capture all arguments we need to call gym.utils.EzPickle.__init__(**locals()) - # at the leaf level, when we do inheritance like we do here. - # kwargs is needed at the top level to account for injection of __class__ keyword. - # Also see: https://github.com/openai/gym/pull/1497 - gym.utils.EzPickle.__init__(self, model_path, obsd_model_path, seed, **kwargs) - - # This two step construction is required for pickling to work correctly. All arguments to all __init__ - # calls must be pickle friendly. Things like sim / sim_obsd are NOT pickle friendly. Therefore we - # first construct the inheritance chain, which is just __init__ calls all the way down, with env_base - # creating the sim / sim_obsd instances. Next we run through "setup" which relies on sim / sim_obsd - # created in __init__ to complete the setup. - super().__init__(model_path=model_path, obsd_model_path=obsd_model_path, seed=seed, env_credits=self.ENV_CREDIT) - - super()._setup(**kwargs) - - def reset(self, reset_qpos=None, reset_qvel=None, reset_franka_wrt_kitchen=True): - if reset_qpos is None: - reset_qpos = self.init_qpos.copy() - reset_qpos[self.robot_dofs] += ( - 0.05 - * (self.np_random.uniform(size=len(self.robot_dofs)) - 0.5) - * (self.robot_ranges[:, 1] - self.robot_ranges[:, 0]) - ) - - # reset franka wrt kitchen - if reset_franka_wrt_kitchen: - bid = self.sim.model.body_name2id("chef") - self.sim.model.body_pos[bid] = np.array([0, 0, 1.8]) + self.np_random.uniform(-0.1, 0.1, (3,)) - - return super().reset(reset_qpos=reset_qpos, reset_qvel=reset_qvel) \ No newline at end of file diff --git a/robohive/envs/multi_task/common/franka_kitchen_v2.py b/robohive/envs/multi_task/common/franka_kitchen_v2.py index 72c6488c..50217e5b 100644 --- a/robohive/envs/multi_task/common/franka_kitchen_v2.py +++ b/robohive/envs/multi_task/common/franka_kitchen_v2.py @@ -94,7 +94,6 @@ def _setup( obs_keys_wt = DEFAULT_OBS_KEYS_N_WT, robot_jnt_reset_noise_scale = 0.0, # Joint noise amp (around qpos_init) for reset robot_base_reset_range = None, # Randomization range (around the default pose) for the chef's base - robot_base_name = "chef", # Name of the robot chef robot_jnt_names=ROBOT_JNT_NAMES, # Robot joint names obj_jnt_names=OBJ_JNT_NAMES, # Object joint name obj_interaction_site=OBJ_INTERACTION_SITES, # Interaction point for object of interest @@ -102,9 +101,7 @@ def _setup( # configure resets self.robot_jnt_reset_noise_scale = robot_jnt_reset_noise_scale - self.robot_base_bid = self.sim.model.body_name2id(robot_base_name) self.robot_base_range = robot_base_reset_range - self.robot_base_pos = self.sim.model.body_pos[self.robot_base_bid].copy() # Setup env super()._setup( diff --git a/robohive/envs/multi_task/common/microwave/franka_microwave.xml b/robohive/envs/multi_task/common/microwave/franka_microwave.xml index 40c98289..27d71b5c 100644 --- a/robohive/envs/multi_task/common/microwave/franka_microwave.xml +++ b/robohive/envs/multi_task/common/microwave/franka_microwave.xml @@ -28,7 +28,7 @@ - + diff --git a/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.xml b/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.xml index 843eb80b..49ba3284 100644 --- a/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.xml +++ b/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.xml @@ -29,7 +29,7 @@ - + diff --git a/robohive/envs/multi_task/multi_task_base_v1.py b/robohive/envs/multi_task/multi_task_base_v1.py index dba702fd..3b4a443f 100644 --- a/robohive/envs/multi_task/multi_task_base_v1.py +++ b/robohive/envs/multi_task/multi_task_base_v1.py @@ -10,7 +10,7 @@ import numpy as np from robohive.envs import env_base -from robohive.utils.quat_math import mat2euler +from robohive.utils.quat_math import mat2euler, quat2euler VIZ = False @@ -40,7 +40,7 @@ class KitchenBase(env_base.MujocoEnv): DEFAULT_PROPRIO_KEYS_AND_WEIGHTS = { "robot_jnt": 1.0, # radian "robot_vel": 1.0, # radian - "ee_pose": 1.0 # [meters, radians] + "ee_pose_wrt_robot": 1.0 # [meters, radians] } def _setup(self, @@ -48,6 +48,7 @@ def _setup(self, obj_jnt_names, obj_interaction_site, obj_goal, + robot_base_name = "chef", # Name of the robot chef interact_site="end_effector", obj_init=None, obs_keys_wt=list(DEFAULT_OBS_KEYS_AND_WEIGHTS.keys()), @@ -68,6 +69,8 @@ def _setup(self, # configure env-site self.grasp_sid = self.sim.model.site_name2id("end_effector") self.obj_interaction_site = obj_interaction_site + self.robot_base_bid = self.sim.model.body_name2id(robot_base_name) + self.robot_base_pos = self.sim.model.body_pos[self.robot_base_bid].copy() # configure env-robot self.robot_dofs = [] @@ -167,10 +170,19 @@ def get_obs_dict(self, sim): - self.sim.data.site_xpos[self.grasp_sid] ) obs_dict["pose_err"] = self.robot_meanpos - obs_dict["robot_jnt"] - obs_dict["end_effector"] = self.sim.data.site_xpos[self.grasp_sid] - ee_pos = self.sim.data.site_xpos[self.grasp_sid] + + # End effector global pose + ee_pos = self.sim.data.site_xpos[self.grasp_sid].copy() ee_euler = mat2euler(np.reshape(sim.data.site_xmat[self.grasp_sid],(3,3))) obs_dict["ee_pose"] = np.concatenate([ee_pos, ee_euler]) + + # End effector local pose wrt robot (for proprioception). + # ee_pose as proprioception introduced bug in v0.5 that led to information + # leakage about the relative robot-kitchen positioning + robot_pos = self.sim.model.body_pos[self.robot_base_bid].copy() + robot_euler = quat2euler(self.sim.model.body_quat[self.robot_base_bid]) + obs_dict["ee_pose_wrt_robot"] = np.concatenate([ee_pos-robot_pos, ee_euler-robot_euler]) + obs_dict["qpos"] = self.sim.data.qpos.copy() for site in self.obj_interaction_site: site_id = self.sim.model.site_name2id(site) diff --git a/robohive/envs/multi_task/substeps1/__init__.py b/robohive/envs/multi_task/substeps1/__init__.py index c2d87938..839a8f43 100644 --- a/robohive/envs/multi_task/substeps1/__init__.py +++ b/robohive/envs/multi_task/substeps1/__init__.py @@ -103,333 +103,4 @@ "obj_body_randomize": ("slidecabinet",), "interact_site": "slide_site", }, -) - - -# Kitchen-V3 ============================================================================ -# The observations consist of info on robot, objects, and goals -# Distance between end effector and all relevent objects in the scene is also appended -# No task specific feature is leaked into the observation forcing multi-task generalization - -print("RoboHive:> Registering Kitchen Envs") -from robohive.envs.multi_task.common.franka_kitchen_v1 import KitchenFrankaFixed, KitchenFrankaRandom, KitchenFrankaDemo - -MODEL_PATH = CURR_DIR + "/../common/kitchen/franka_kitchen.xml" -CONFIG_PATH = CURR_DIR + "/../common/kitchen/franka_kitchen.config" - -DEMO_ENTRY_POINT = "robohive.envs.multi_task.common.franka_kitchen_v1:KitchenFrankaDemo" -RANDOM_ENTRY_POINT = "robohive.envs.multi_task.common.franka_kitchen_v1:KitchenFrankaRandom" -FIXED_ENTRY_POINT = "robohive.envs.multi_task.common.franka_kitchen_v1:KitchenFrankaFixed" -ENTRY_POINT = RANDOM_ENTRY_POINT - -obs_keys_wt = {"robot_jnt": 1.0, "objs_jnt": 1.0, "obj_goal": 1.0, "end_effector": 1.0} -for site in KitchenFrankaFixed.OBJ_INTERACTION_SITES: - obs_keys_wt[site + "_err"] = 1.0 - -visual_obs_keys_wt = {"robot_jnt": 1.0, - "end_effector": 1.0, - # "rgb:right_cam:224x224:r3m18": 1.0, - # "rgb:left_cam:224x224:r3m18": 1.0, - "rgb:right_cam:224x224:1d": 1.0, - "rgb:left_cam:224x224:1d": 1.0, - } - -# Kitchen (base-env; obj_init==obj_goal => do nothing in the env) -register( - id="kitchen-v3", - entry_point=ENTRY_POINT, - max_episode_steps=280, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_goal": {}, - "obj_init": { - "knob1_joint": 0, - "knob2_joint": 0, - "knob3_joint": 0, - "knob4_joint": 0, - "lightswitch_joint": 0, - "slidedoor_joint": 0, - "micro0joint": 0, - "rightdoorhinge": 0, - "leftdoorhinge": 0, - }, - "obs_keys_wt": obs_keys_wt, - }, -) - -register( - id="kitchen_rgb-v3", - entry_point=ENTRY_POINT, - max_episode_steps=280, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_goal": {}, - "obj_init": { - "knob1_joint": 0, - "knob2_joint": 0, - "knob3_joint": 0, - "knob4_joint": 0, - "lightswitch_joint": 0, - "slidedoor_joint": 0, - "micro0joint": 0, - "rightdoorhinge": 0, - "leftdoorhinge": 0, - }, - "obs_keys_wt": visual_obs_keys_wt, - }, -) - -# Microwave door -register( - id="kitchen_micro_open-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"micro0joint": 0}, - "obj_goal": {"micro0joint": -1.25}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "microhandle_site", - }, -) - -register( - id="kitchen_micro_close-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"micro0joint": -1.25}, - "obj_goal": {"micro0joint": 0}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "microhandle_site", - }, -) - -# Right hinge cabinet -register( - id="kitchen_rdoor_open-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"rightdoorhinge": 0}, - "obj_goal": {"rightdoorhinge": 1.57}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "rightdoor_site", - }, -) -register( - id="kitchen_rdoor_close-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"rightdoorhinge": 1.57}, - "obj_goal": {"rightdoorhinge": 0}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "rightdoor_site", - }, -) - -# Left hinge cabinet -register( - id="kitchen_ldoor_open-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"leftdoorhinge": 0}, - "obj_goal": {"leftdoorhinge": -1.25}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "leftdoor_site", - }, -) -register( - id="kitchen_ldoor_close-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"leftdoorhinge": -1.25}, - "obj_goal": {"leftdoorhinge": 0}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "leftdoor_site", - }, -) - -# Slide cabinet -register( - id="kitchen_sdoor_open-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"slidedoor_joint": 0}, - "obj_goal": {"slidedoor_joint": 0.44}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "slide_site", - }, -) -register( - id="kitchen_sdoor_close-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"slidedoor_joint": 0.44}, - "obj_goal": {"slidedoor_joint": 0}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "slide_site", - }, -) - -# Lights -register( - id="kitchen_light_on-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"lightswitch_joint": 0}, - "obj_goal": {"lightswitch_joint": -0.7}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "light_site", - }, -) -register( - id="kitchen_light_off-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"lightswitch_joint": -0.7}, - "obj_goal": {"lightswitch_joint": 0}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "light_site", - }, -) - -# Knob4 -register( - id="kitchen_knob4_on-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"knob4_joint": 0}, - "obj_goal": {"knob4_joint": -1.57}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "knob4_site", - }, -) -register( - id="kitchen_knob4_off-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"knob4_joint": -1.57}, - "obj_goal": {"knob4_joint": 0}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "knob4_site", - }, -) - -# Knob3 -register( - id="kitchen_knob3_on-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"knob3_joint": 0}, - "obj_goal": {"knob3_joint": -1.57}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "knob3_site", - }, -) -register( - id="kitchen_knob3_off-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"knob3_joint": -1.57}, - "obj_goal": {"knob3_joint": 0}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "knob3_site", - }, -) - -# Knob2 -register( - id="kitchen_knob2_on-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"knob2_joint": 0}, - "obj_goal": {"knob2_joint": -1.57}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "knob2_site", - }, -) -register( - id="kitchen_knob2_off-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"knob2_joint": -1.57}, - "obj_goal": {"knob2_joint": 0}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "knob2_site", - }, -) - -# Knob1 -register( - id="kitchen_knob1_on-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"knob1_joint": 0}, - "obj_goal": {"knob1_joint": -1.57}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "knob1_site", - }, -) -register( - id="kitchen_knob1_off-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, - "obj_init": {"knob1_joint": -1.57}, - "obj_goal": {"knob1_joint": 0}, - "obs_keys_wt": obs_keys_wt, - "interact_site": "knob1_site", - }, ) \ No newline at end of file diff --git a/robohive/envs/multi_task/substeps1/franka_kitchen.py b/robohive/envs/multi_task/substeps1/franka_kitchen.py index 7b72e661..6305681e 100644 --- a/robohive/envs/multi_task/substeps1/franka_kitchen.py +++ b/robohive/envs/multi_task/substeps1/franka_kitchen.py @@ -10,7 +10,108 @@ from robohive.envs.multi_task.common.franka_kitchen_v2 import FrankaKitchen import copy +print("RoboHive:> Registering FrankaKitchen (FK1) Envs") + +# =================================================================== +# ENVs are provided in three VARIANTs (controlling env's behavior) +# - Fixed-v4: State based stationary environments with no randomization +# - Random-v4: State based environments with random robot initialization (joint pose + relative position wrt to kitchen) +# - Random_v2d-v4: Visual environment with random robot initialization (joint pose + relative position wrt to kitchen) +# =================================================================== + + +# =================================================================== +# ENVs are organized into SUBGROUPS: +# - SUBGROUPS are arranged to ensure diversity and a balance between the groups +# - SUBGROUPS are helpful for standardizing partial results, Test-Train spilt, generalization studies, etc +# - Without these subgroups, random subsets were being picked by different papers making comparisons difficult +# =================================================================== + +# Fixed-v4: State based stationary environments with no randomization +FK1_FIXED_5A = [ + "FK1_MicroOpenFixed-v4", + "FK1_Knob1OnFixed-v4", + "FK1_Knob2OffFixed-v4", + "FK1_SdoorOpenFixed-v4", + "FK1_LdoorOpenFixed-v4"] +FK1_FIXED_5B = [ + "FK1_MicroCloseFixed-v4", + "FK1_Knob1OffFixed-v4", + "FK1_Knob2OnFixed-v4", + "FK1_LightOnFixed-v4", + "FK1_RdoorOpenFixed-v4",] +FK1_FIXED_5C = [ + "FK1_Stove1KettleFixed-v4", + "FK1_Knob3OnFixed-v4", + "FK1_Knob4OffFixed-v4", + "FK1_SdoorCloseFixed-v4", + "FK1_RdoorCloseFixed-v4"] +FK1_FIXED_5D = [ + "FK1_Stove4KettleFixed-v4", + "FK1_Knob3OffFixed-v4", + "FK1_Knob4OnFixed-v4", + "FK1_LightOffFixed-v4", + "FK1_LdoorCloseFixed-v4"] +FK1_FIXED_20 = FK1_FIXED_5A+FK1_FIXED_5B+FK1_FIXED_5C+FK1_FIXED_5D + +# Random-v4: State based environments with random robot initialization (joint pose + relative position wrt to kitchen) +FK1_RANDOM_5A = [ + "FK1_MicroOpenRandom-v4", + "FK1_Knob1OnRandom-v4", + "FK1_Knob2OffRandom-v4", + "FK1_SdoorOpenRandom-v4", + "FK1_LdoorOpenRandom-v4"] +FK1_RANDOM_5B = [ + "FK1_MicroCloseRandom-v4", + "FK1_Knob1OffRandom-v4", + "FK1_Knob2OnRandom-v4", + "FK1_LightOnRandom-v4", + "FK1_RdoorOpenRandom-v4",] +FK1_RANDOM_5C = [ + "FK1_Stove1KettleRandom-v4", + "FK1_Knob3OnRandom-v4", + "FK1_Knob4OffRandom-v4", + "FK1_SdoorCloseRandom-v4", + "FK1_RdoorCloseRandom-v4"] +FK1_RANDOM_5D = [ + "FK1_Stove4KettleRandom-v4", + "FK1_Knob3OffRandom-v4", + "FK1_Knob4OnRandom-v4", + "FK1_LightOffRandom-v4", + "FK1_LdoorCloseRandom-v4"] +FK1_RANDOM_20 = FK1_RANDOM_5A+FK1_RANDOM_5B+FK1_RANDOM_5C+FK1_RANDOM_5D + +# Random_v2d-v4: Visual environment with random robot initialization (joint pose + relative position wrt to kitchen) +FK1_RANDOM_V2D_5A = [ + "FK1_MicroOpenRandom_v2d-v4", + "FK1_Knob1OnRandom_v2d-v4", + "FK1_Knob2OffRandom_v2d-v4", + "FK1_SdoorOpenRandom_v2d-v4", + "FK1_LdoorOpenRandom_v2d-v4"] +FK1_RANDOM_V2D_5B = [ + "FK1_MicroCloseRandom_v2d-v4", + "FK1_Knob1OffRandom_v2d-v4", + "FK1_Knob2OnRandom_v2d-v4", + "FK1_LightOnRandom_v2d-v4", + "FK1_RdoorOpenRandom_v2d-v4",] +FK1_RANDOM_V2D_5C = [ + "FK1_Stove1KettleRandom_v2d-v4", + "FK1_Knob3OnRandom_v2d-v4", + "FK1_Knob4OffRandom_v2d-v4", + "FK1_SdoorCloseRandom_v2d-v4", + "FK1_RdoorCloseRandom_v2d-v4"] +FK1_RANDOM_V2D_5D = [ + "FK1_Stove4KettleRandom_v2d-v4", + "FK1_Knob3OffRandom_v2d-v4", + "FK1_Knob4OnRandom_v2d-v4", + "FK1_LightOffRandom_v2d-v4", + "FK1_LdoorCloseRandom_v2d-v4"] +FK1_RANDOM_V2D_20 = FK1_RANDOM_V2D_5A+FK1_RANDOM_V2D_5B+FK1_RANDOM_V2D_5C+FK1_RANDOM_V2D_5D + + +# =================================================================== # Global Configs +# =================================================================== CURR_DIR = os.path.dirname(os.path.abspath(__file__)) MODEL_PATH = CURR_DIR + "/../common/kitchen/franka_kitchen.xml" CONFIG_PATH = CURR_DIR + "/../common/kitchen/franka_kitchen.config" @@ -18,7 +119,9 @@ ENV_VERSION = "-v4" VISUAL_ENCODER = "v2d" -# Register different env variants - Fixed, Random, Random_v2d +# =================================================================== +# Register env variants +# =================================================================== def register_all_env_variants( task_id:str, # task task_configs:dict, # task details @@ -32,7 +135,7 @@ def register_all_env_variants( "config_path": CONFIG_PATH }) - # register state based fixed variants + # register state based fixed variants, no randomization register( id = task_id+"Fixed"+ENV_VERSION, entry_point=ENTRY_POINT, @@ -41,7 +144,7 @@ def register_all_env_variants( ) # print("'"+task_id+"Fixed"+ENV_VERSION+"'", end=", ") - # update env's randomization configs + # update env's randomization configs: random robot initialization (joint pose + relative position wrt to kitchen) if random_configs == None: random_configs = { "robot_jnt_reset_noise_scale": 0.05, # Joint noise scale for reset @@ -61,7 +164,7 @@ def register_all_env_variants( # print("'"+task_id+"Fixed"+ENV_VERSION+"'", end=", ") - # update env's visual configs + # update env's visual configs: random robot initialization (joint pose + relative position wrt to kitchen) task_configs.update({ "visual_keys": FrankaKitchen.DEFAULT_VISUAL_KEYS, # override the obs to avoid accidental leakage of oracle state info while using the visual envs @@ -283,86 +386,4 @@ def register_all_env_variants( "obj_goal": {"kettle0:Ty": .78}, "interact_site": "kettle_site0", }, -) - - -# =================================================================== -# SUB GROUPS: helpful for standardizing partial results, Test-Train spilit, generalization studies, etc -# =================================================================== -FK1_FIXED_5A = [ - "FK1_MicroOpenFixed-v4", - "FK1_Knob1OnFixed-v4", - "FK1_Knob2OffFixed-v4", - "FK1_SdoorOpenFixed-v4", - "FK1_LdoorOpenFixed-v4"] -FK1_FIXED_5B = [ - "FK1_MicroCloseFixed-v4", - "FK1_Knob1OffFixed-v4", - "FK1_Knob2OnFixed-v4", - "FK1_LightOnFixed-v4", - "FK1_RdoorOpenFixed-v4",] -FK1_FIXED_5C = [ - "FK1_Stove1KettleFixed-v4", - "FK1_Knob3OnFixed-v4", - "FK1_Knob4OffFixed-v4", - "FK1_SdoorCloseFixed-v4", - "FK1_RdoorCloseFixed-v4"] -FK1_FIXED_5D = [ - "FK1_Stove4KettleFixed-v4", - "FK1_Knob3OffFixed-v4", - "FK1_Knob4OnFixed-v4", - "FK1_LightOffFixed-v4", - "FK1_LdoorCloseFixed-v4"] -FK1_FIXED_20 = FK1_FIXED_5A+FK1_FIXED_5B+FK1_FIXED_5C+FK1_FIXED_5D - -FK1_RANDOM_5A = [ - "FK1_MicroOpenRandom-v4", - "FK1_Knob1OnRandom-v4", - "FK1_Knob2OffRandom-v4", - "FK1_SdoorOpenRandom-v4", - "FK1_LdoorOpenRandom-v4"] -FK1_RANDOM_5B = [ - "FK1_MicroCloseRandom-v4", - "FK1_Knob1OffRandom-v4", - "FK1_Knob2OnRandom-v4", - "FK1_LightOnRandom-v4", - "FK1_RdoorOpenRandom-v4",] -FK1_RANDOM_5C = [ - "FK1_Stove1KettleRandom-v4", - "FK1_Knob3OnRandom-v4", - "FK1_Knob4OffRandom-v4", - "FK1_SdoorCloseRandom-v4", - "FK1_RdoorCloseRandom-v4"] -FK1_RANDOM_5D = [ - "FK1_Stove4KettleRandom-v4", - "FK1_Knob3OffRandom-v4", - "FK1_Knob4OnRandom-v4", - "FK1_LightOffRandom-v4", - "FK1_LdoorCloseRandom-v4"] -FK1_RANDOM_20 = FK1_RANDOM_5A+FK1_RANDOM_5B+FK1_RANDOM_5C+FK1_RANDOM_5D - -FK1_RANDOM_V2D_5A = [ - "FK1_MicroOpenRandom_v2d-v4", - "FK1_Knob1OnRandom_v2d-v4", - "FK1_Knob2OffRandom_v2d-v4", - "FK1_SdoorOpenRandom_v2d-v4", - "FK1_LdoorOpenRandom_v2d-v4"] -FK1_RANDOM_V2D_5B = [ - "FK1_MicroCloseRandom_v2d-v4", - "FK1_Knob1OffRandom_v2d-v4", - "FK1_Knob2OnRandom_v2d-v4", - "FK1_LightOnRandom_v2d-v4", - "FK1_RdoorOpenRandom_v2d-v4",] -FK1_RANDOM_V2D_5C = [ - "FK1_Stove1KettleRandom_v2d-v4", - "FK1_Knob3OnRandom_v2d-v4", - "FK1_Knob4OffRandom_v2d-v4", - "FK1_SdoorCloseRandom_v2d-v4", - "FK1_RdoorCloseRandom_v2d-v4"] -FK1_RANDOM_V2D_5D = [ - "FK1_Stove4KettleRandom_v2d-v4", - "FK1_Knob3OffRandom_v2d-v4", - "FK1_Knob4OnRandom_v2d-v4", - "FK1_LightOffRandom_v2d-v4", - "FK1_LdoorCloseRandom_v2d-v4"] -FK1_RANDOM_V2D_20 = FK1_RANDOM_V2D_5A+FK1_RANDOM_V2D_5B+FK1_RANDOM_V2D_5C+FK1_RANDOM_V2D_5D +) \ No newline at end of file diff --git a/robohive/envs/multi_task/substeps2/__init__.py b/robohive/envs/multi_task/substeps2/__init__.py index 8ccbfe91..28c381da 100644 --- a/robohive/envs/multi_task/substeps2/__init__.py +++ b/robohive/envs/multi_task/substeps2/__init__.py @@ -5,38 +5,21 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================= """ -import os -from gym.envs.registration import register - -CURR_DIR = os.path.dirname(os.path.abspath(__file__)) - -# Kitchen-V3 ============================================================================ -# In this version of the environment, the observations consist of the -# distance between end effector and all relevent objects in the scene - print("RoboHive:> Registering Multi-Task (2 subtasks) Envs") -from robohive.envs.multi_task.common.franka_kitchen_v1 import KitchenFrankaFixed, KitchenFrankaRandom, KitchenFrankaDemo - -MODEL_PATH = CURR_DIR + "/../common/kitchen/franka_kitchen.xml" -CONFIG_PATH = CURR_DIR + "/../common/kitchen/franka_kitchen.config" - -DEMO_ENTRY_POINT = "robohive.envs.multi_task.common.franka_kitchen_v1:KitchenFrankaDemo" -RANDOM_ENTRY_POINT = "robohive.envs.multi_task.common.franka_kitchen_v1:KitchenFrankaRandom" -FIXED_ENTRY_POINT = "robohive.envs.multi_task.common.franka_kitchen_v1:KitchenFrankaFixed" -ENTRY_POINT = RANDOM_ENTRY_POINT +from robohive.envs.multi_task.substeps1.franka_kitchen import register_all_env_variants -obs_keys_wt = {"robot_jnt": 1.0, "objs_jnt": 1.0, "obj_goal": 1.0, "end_effector": 1.0} -for site in KitchenFrankaFixed.OBJ_INTERACTION_SITES: - obs_keys_wt[site + "_err"] = 1.0 +# =================================================================== +# ENVs are provided in three VARIANTs (controlling env's behavior) +# - Fixed-v4: State based stationary environments with no randomization +# - Random-v4: State based environments with random robot initialization (joint pose + relative position wrt to kitchen) +# - Random_v2d-v4: Visual environment with random robot initialization (joint pose + relative position wrt to kitchen) +# =================================================================== # Kitchen (close microwave and slidedoor) -register( - id="kitchen_micro_slide_close-v3", - entry_point=ENTRY_POINT, +register_all_env_variants( + task_id="FK1_MicroSlideClose", max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, + task_configs={ "obj_goal": { "micro0joint": 0, "slidedoor_joint": 0, @@ -45,6 +28,5 @@ "micro0joint": -1.25, "slidedoor_joint": 0.44, }, - "obs_keys_wt": obs_keys_wt, }, ) \ No newline at end of file diff --git a/robohive/envs/multi_task/substeps9/__init__.py b/robohive/envs/multi_task/substeps9/__init__.py index 0b30ecd1..7e2b71fd 100644 --- a/robohive/envs/multi_task/substeps9/__init__.py +++ b/robohive/envs/multi_task/substeps9/__init__.py @@ -5,38 +5,22 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================= """ -import os -from gym.envs.registration import register - -CURR_DIR = os.path.dirname(os.path.abspath(__file__)) - -# Kitchen-V3 ============================================================================ -# In this version of the environment, the observations consist of the -# distance between end effector and all relevent objects in the scene - print("RoboHive:> Registering Multi-Task (9 subtasks) Envs") -from robohive.envs.multi_task.common.franka_kitchen_v1 import KitchenFrankaFixed, KitchenFrankaRandom, KitchenFrankaDemo +from robohive.envs.multi_task.substeps1.franka_kitchen import register_all_env_variants -MODEL_PATH = CURR_DIR + "/../common/kitchen/franka_kitchen.xml" -CONFIG_PATH = CURR_DIR + "/../common/kitchen/franka_kitchen.config" +# =================================================================== +# ENVs are provided in three VARIANTs (controlling env's behavior) +# - Fixed-v4: State based stationary environments with no randomization +# - Random-v4: State based environments with random robot initialization (joint pose + relative position wrt to kitchen) +# - Random_v2d-v4: Visual environment with random robot initialization (joint pose + relative position wrt to kitchen) +# =================================================================== -DEMO_ENTRY_POINT = "robohive.envs.multi_task.common.franka_kitchen_v1:KitchenFrankaDemo" -RANDOM_ENTRY_POINT = "robohive.envs.multi_task.common.franka_kitchen_v1:KitchenFrankaRandom" -FIXED_ENTRY_POINT = "robohive.envs.multi_task.common.franka_kitchen_v1:KitchenFrankaFixed" -ENTRY_POINT = RANDOM_ENTRY_POINT - -obs_keys_wt = {"robot_jnt": 1.0, "objs_jnt": 1.0, "obj_goal": 1.0, "end_effector": 1.0} -for site in KitchenFrankaFixed.OBJ_INTERACTION_SITES: - obs_keys_wt[site + "_err"] = 1.0 # Kitchen (close everything) -register( - id="kitchen_openall-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, +register_all_env_variants( + task_id="FK1_OpenAll", + max_episode_steps=280, + task_configs={ "obj_init": {}, "obj_goal": { "knob1_joint": -1.57, @@ -49,16 +33,13 @@ "rightdoorhinge": 1.57, "leftdoorhinge": -1.25, }, - "obs_keys_wt": obs_keys_wt, }, ) -register( - id="kitchen_closeall-v3", - entry_point=ENTRY_POINT, - max_episode_steps=50, - kwargs={ - "model_path": MODEL_PATH, - "config_path": CONFIG_PATH, + +register_all_env_variants( + task_id="FK1_CloseAll", + max_episode_steps=280, + task_configs={ "obj_goal": {}, "obj_init": { "knob1_joint": -1.57, @@ -71,6 +52,5 @@ "rightdoorhinge": 1.57, "leftdoorhinge": -1.25, }, - "obs_keys_wt": obs_keys_wt, }, ) \ No newline at end of file diff --git a/robohive/envs/myo/__init__.py b/robohive/envs/myo/__init__.py index 2e578d5c..e0076880 100644 --- a/robohive/envs/myo/__init__.py +++ b/robohive/envs/myo/__init__.py @@ -52,7 +52,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.reach_v0:ReachEnvV0', max_episode_steps=200, kwargs={ - 'model_path': curr_dir+'/../../simhive/myo_sim/finger/motor_finger_v0.xml', + 'model_path': curr_dir+'/../../simhive/myo_sim/finger/motorfinger_v0.xml', 'target_reach_range': {'IFtip': ((0.2, 0.05, 0.20), (0.2, 0.05, 0.20)),}, 'normalize_act': True, 'frame_skip': 5, @@ -62,7 +62,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.reach_v0:ReachEnvV0', max_episode_steps=200, kwargs={ - 'model_path': curr_dir+'/../../simhive/myo_sim/finger/motor_finger_v0.xml', + 'model_path': curr_dir+'/../../simhive/myo_sim/finger/motorfinger_v0.xml', 'target_reach_range': {'IFtip': ((.1, -.1, .1), (0.27, .1, .3)),}, 'normalize_act': True, 'frame_skip': 5, @@ -72,7 +72,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.reach_v0:ReachEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/../../simhive/myo_sim/finger/myo_finger_v0.xml', + 'model_path': curr_dir+'/../../simhive/myo_sim/finger/myofinger_v0.xml', 'target_reach_range': {'IFtip': ((0.2, 0.05, 0.20), (0.2, 0.05, 0.20)),}, 'normalize_act': True, } @@ -81,7 +81,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.reach_v0:ReachEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/../../simhive/myo_sim/finger/myo_finger_v0.xml', + 'model_path': curr_dir+'/../../simhive/myo_sim/finger/myofinger_v0.xml', 'target_reach_range': {'IFtip': ((.1, -.1, .1), (0.27, .1, .3)),}, 'normalize_act': True, } @@ -92,7 +92,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pose_v0:PoseEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/assets/arm/myo_elbow_1dof6muscles.xml', + 'model_path': curr_dir+'/assets/elbow/myoelbow_1dof6muscles.xml', 'target_jnt_range': {'r_elbow_flex':(2, 2),}, 'viz_site_targets': ('wrist',), 'normalize_act': True, @@ -104,7 +104,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pose_v0:PoseEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/assets/arm/myo_elbow_1dof6muscles.xml', + 'model_path': curr_dir+'/assets/elbow/myoelbow_1dof6muscles.xml', 'target_jnt_range': {'r_elbow_flex':(0, 2.27),}, 'viz_site_targets': ('wrist',), 'normalize_act': True, @@ -119,7 +119,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pose_v0:PoseEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/assets/arm/myo_elbow_1dof6muscles_1dofexo.xml', + 'model_path': curr_dir+'/assets/elbow/myoelbow_1dof6muscles_1dofexo.xml', 'target_jnt_range': {'r_elbow_flex':(2, 2),}, 'viz_site_targets': ('wrist',), 'normalize_act': True, @@ -137,7 +137,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pose_v0:PoseEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/assets/arm/myo_elbow_1dof6muscles_1dofexo.xml', + 'model_path': curr_dir+'/assets/elbow/myoelbow_1dof6muscles_1dofexo.xml', 'target_jnt_range': {'r_elbow_flex':(0, 2.27),}, 'viz_site_targets': ('wrist',), 'normalize_act': True, @@ -160,7 +160,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pose_v0:PoseEnvV0', max_episode_steps=200, kwargs={ - 'model_path': curr_dir+'/../../simhive/myo_sim/finger/motor_finger_v0.xml', + 'model_path': curr_dir+'/../../simhive/myo_sim/finger/motorfinger_v0.xml', 'target_jnt_range': {'IFadb':(0, 0), 'IFmcp':(0, 0), 'IFpip':(.75, .75), @@ -175,7 +175,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pose_v0:PoseEnvV0', max_episode_steps=200, kwargs={ - 'model_path': curr_dir+'/../../simhive/myo_sim/finger/motor_finger_v0.xml', + 'model_path': curr_dir+'/../../simhive/myo_sim/finger/motorfinger_v0.xml', 'target_jnt_range': {'IFadb':(-.2, .2), 'IFmcp':(-.4, 1), 'IFpip':(.1, 1), @@ -190,7 +190,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pose_v0:PoseEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/../../simhive/myo_sim/finger/myo_finger_v0.xml', + 'model_path': curr_dir+'/../../simhive/myo_sim/finger/myofinger_v0.xml', 'target_jnt_range': {'IFadb':(0, 0), 'IFmcp':(0, 0), 'IFpip':(.75, .75), @@ -204,7 +204,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pose_v0:PoseEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/../../simhive/myo_sim/finger/myo_finger_v0.xml', + 'model_path': curr_dir+'/../../simhive/myo_sim/finger/myofinger_v0.xml', 'target_jnt_range': {'IFadb':(-.2, .2), 'IFmcp':(-.4, 1), 'IFpip':(.1, 1), @@ -222,7 +222,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pose_v0:PoseEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_pose.xml', + 'model_path': curr_dir+'/assets/hand/myohand_pose.xml', 'viz_site_targets': ('THtip','IFtip','MFtip','RFtip','LFtip'), 'target_jnt_value': np.array([0, 0, 0, -0.0904, 0.0824475, -0.681555, -0.514888, 0, -0.013964, -0.0458132, 0, 0.67553, -0.020944, 0.76979, 0.65982, 0, 0, 0, 0, 0.479155, -0.099484, 0.95831, 0]), 'normalize_act': True, @@ -253,7 +253,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pose_v0:PoseEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_pose.xml', + 'model_path': curr_dir+'/assets/hand/myohand_pose.xml', 'viz_site_targets': ('THtip','IFtip','MFtip','RFtip','LFtip'), 'target_jnt_value': np.array(ASL_qpos[k],'float'), 'normalize_act': True, @@ -273,7 +273,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pose_v0:PoseEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_pose.xml', + 'model_path': curr_dir+'/assets/hand/myohand_pose.xml', 'viz_site_targets': ('THtip','IFtip','MFtip','RFtip','LFtip'), 'target_jnt_range': Rpos, 'normalize_act': True, @@ -285,11 +285,18 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): # Gait Torso Reaching ============================== +from robohive.physics.sim_scene import SimBackend +sim_backend = SimBackend.get_sim_backend() +if sim_backend == SimBackend.MUJOCO_PY: + leg_model='/../../simhive/myo_sim/leg/myolegs_v0.54(mj210).mjb' +elif sim_backend == SimBackend.MUJOCO: + leg_model='/../../simhive/myo_sim/leg/myolegs_v0.54(mj236).mjb' + register_env_with_variants(id='myoLegReachFixed-v0', entry_point='robohive.envs.myo.walk_v0:ReachEnvV0', max_episode_steps=150, kwargs={ - 'model_path': curr_dir+'/../../simhive/myo_sim/gait/myolegs.xml', + 'model_path': curr_dir+leg_model, 'target_reach_range': { # 'pelvis': ((-.05, -.05, .92), (0.05, 0.05, .92)), 'pelvis': ((-.005, -.005, .9), (0.005, 0.005, .9)), @@ -300,13 +307,31 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): } ) +# Gait Torso Walking ============================== +register_env_with_variants(id='myoLegWalk-v0', + entry_point='robohive.envs.myo.walk_v0:WalkEnvV0', + max_episode_steps=1000, + kwargs={ + 'model_path': curr_dir + leg_model, + 'normalize_act': True, + 'min_height':0.8, # minimum center of mass height before reset + 'max_rot':0.8, # maximum rotation before reset + 'hip_period':100, # desired periodic hip angle movement + 'reset_type':'init', # none, init, random + 'target_x_vel':0.0, # desired x velocity in m/s + 'target_y_vel':1.2, # desired y velocity in m/s + 'target_rot': None # if None then the initial root pos will be taken, otherwise provide quat + } + ) + + # Hand-Joint Reaching ============================== register_env_with_variants(id='myoHandReachFixed-v0', entry_point='robohive.envs.myo.reach_v0:ReachEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_pose.xml', + 'model_path': curr_dir+'/assets/hand/myohand_pose.xml', 'target_reach_range': { 'THtip': ((-0.165, -0.537, 1.495), (-0.165, -0.537, 1.495)), 'IFtip': ((-0.151, -0.547, 1.455), (-0.151, -0.547, 1.455)), @@ -322,7 +347,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.reach_v0:ReachEnvV0', max_episode_steps=100, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_pose.xml', + 'model_path': curr_dir+'/assets/hand/myohand_pose.xml', 'target_reach_range': { 'THtip': ((-0.165-0.020, -0.537-0.040, 1.495-0.040), (-0.165+0.040, -0.537+0.020, 1.495+0.040)), 'IFtip': ((-0.151-0.040, -0.547-0.020, 1.455-0.010), (-0.151+0.040, -0.547+0.020, 1.455+0.010)), @@ -341,7 +366,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.key_turn_v0:KeyTurnEnvV0', max_episode_steps=200, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_keyturn.xml', + 'model_path': curr_dir+'/assets/hand/myohand_keyturn.xml', 'normalize_act': True } ) @@ -349,7 +374,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.key_turn_v0:KeyTurnEnvV0', max_episode_steps=200, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_keyturn.xml', + 'model_path': curr_dir+'/assets/hand/myohand_keyturn.xml', 'normalize_act': True, 'key_init_range':(-np.pi/2, np.pi/2), 'goal_th': 2*np.pi @@ -362,7 +387,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.obj_hold_v0:ObjHoldFixedEnvV0', max_episode_steps=75, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_hold.xml', + 'model_path': curr_dir+'/assets/hand/myohand_hold.xml', 'normalize_act': True } ) @@ -370,7 +395,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.obj_hold_v0:ObjHoldRandomEnvV0', max_episode_steps=75, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_hold.xml', + 'model_path': curr_dir+'/assets/hand/myohand_hold.xml', 'normalize_act': True } ) @@ -381,7 +406,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pen_v0:PenTwirlFixedEnvV0', max_episode_steps=50, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_pen.xml', + 'model_path': curr_dir+'/assets/hand/myohand_pen.xml', 'normalize_act': True, 'frame_skip': 5, } @@ -390,7 +415,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.pen_v0:PenTwirlRandomEnvV0', max_episode_steps=50, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_pen.xml', + 'model_path': curr_dir+'/assets/hand/myohand_pen.xml', 'normalize_act': True, 'frame_skip': 5, } @@ -402,7 +427,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.reorient_v0:ReorientEnvV0', max_episode_steps=50, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_die.xml', + 'model_path': curr_dir+'/assets/hand/myohand_die.xml', 'normalize_act': True, 'goal_pos': (0.0, 0.0), # 0 meters 'goal_rot': (.785, .785) # 45 degrees @@ -413,7 +438,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.reorient_v0:ReorientEnvV0', max_episode_steps=50, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_die.xml', + 'model_path': curr_dir+'/assets/hand/myohand_die.xml', 'normalize_act': True, 'goal_pos': (-.020, .020), # +-2 cm 'goal_rot': (-.785, .785) # +-45 degrees @@ -425,7 +450,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.baoding_v1:BaodingEnvV1', max_episode_steps=200, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_baoding.xml', + 'model_path': curr_dir+'/assets/hand/myohand_baoding.xml', 'normalize_act': True, } ) @@ -433,7 +458,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.baoding_v1:BaodingEnvV1', max_episode_steps=200, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_baoding.xml', + 'model_path': curr_dir+'/assets/hand/myohand_baoding.xml', 'normalize_act': True, } ) @@ -441,7 +466,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.baoding_v1:BaodingEnvV1', max_episode_steps=200, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_baoding.xml', + 'model_path': curr_dir+'/assets/hand/myohand_baoding.xml', 'normalize_act': True, 'n_shifts_per_period':4, } @@ -450,7 +475,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs): entry_point='robohive.envs.myo.baoding_v1:BaodingEnvV1', max_episode_steps=200, kwargs={ - 'model_path': curr_dir+'/assets/hand/myo_hand_baoding.xml', + 'model_path': curr_dir+'/assets/hand/myohand_baoding.xml', 'normalize_act': True, 'n_shifts_per_period':8, } diff --git a/robohive/envs/myo/assets/arm/myo_elbow_1dof6muscles.xml b/robohive/envs/myo/assets/elbow/myoelbow_1dof6muscles.xml similarity index 88% rename from robohive/envs/myo/assets/arm/myo_elbow_1dof6muscles.xml rename to robohive/envs/myo/assets/elbow/myoelbow_1dof6muscles.xml index 7af9109b..f9a3b1df 100644 --- a/robohive/envs/myo/assets/arm/myo_elbow_1dof6muscles.xml +++ b/robohive/envs/myo/assets/elbow/myoelbow_1dof6muscles.xml @@ -7,8 +7,8 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ====================================================== --> - - + + diff --git a/robohive/envs/myo/assets/arm/myo_elbow_1dof6muscles_1dofexo.xml b/robohive/envs/myo/assets/elbow/myoelbow_1dof6muscles_1dofexo.xml similarity index 87% rename from robohive/envs/myo/assets/arm/myo_elbow_1dof6muscles_1dofexo.xml rename to robohive/envs/myo/assets/elbow/myoelbow_1dof6muscles_1dofexo.xml index 9356c132..bb1da4c3 100644 --- a/robohive/envs/myo/assets/arm/myo_elbow_1dof6muscles_1dofexo.xml +++ b/robohive/envs/myo/assets/elbow/myoelbow_1dof6muscles_1dofexo.xml @@ -7,8 +7,8 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ====================================================== --> - - + + diff --git a/robohive/envs/myo/assets/hand/myo_hand_baoding.xml b/robohive/envs/myo/assets/hand/myohand_baoding.xml similarity index 94% rename from robohive/envs/myo/assets/hand/myo_hand_baoding.xml rename to robohive/envs/myo/assets/hand/myohand_baoding.xml index ef8e0846..9dbad8bd 100644 --- a/robohive/envs/myo/assets/hand/myo_hand_baoding.xml +++ b/robohive/envs/myo/assets/hand/myohand_baoding.xml @@ -7,8 +7,7 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ====================================================== --> - - + @@ -20,6 +19,7 @@ + diff --git a/robohive/envs/myo/assets/hand/myo_hand_die.xml b/robohive/envs/myo/assets/hand/myohand_die.xml similarity index 97% rename from robohive/envs/myo/assets/hand/myo_hand_die.xml rename to robohive/envs/myo/assets/hand/myohand_die.xml index 4c984a94..b8c4c599 100644 --- a/robohive/envs/myo/assets/hand/myo_hand_die.xml +++ b/robohive/envs/myo/assets/hand/myohand_die.xml @@ -7,8 +7,7 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ====================================================== --> - - + @@ -19,6 +18,7 @@ + diff --git a/robohive/envs/myo/assets/hand/myo_hand_hold.xml b/robohive/envs/myo/assets/hand/myohand_hold.xml similarity index 90% rename from robohive/envs/myo/assets/hand/myo_hand_hold.xml rename to robohive/envs/myo/assets/hand/myohand_hold.xml index 92b9a4df..f8916d82 100644 --- a/robohive/envs/myo/assets/hand/myo_hand_hold.xml +++ b/robohive/envs/myo/assets/hand/myohand_hold.xml @@ -7,12 +7,12 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ====================================================== --> - - + + diff --git a/robohive/envs/myo/assets/hand/myo_hand_keyturn.xml b/robohive/envs/myo/assets/hand/myohand_keyturn.xml similarity index 91% rename from robohive/envs/myo/assets/hand/myo_hand_keyturn.xml rename to robohive/envs/myo/assets/hand/myohand_keyturn.xml index 6afa3b9e..16f52601 100644 --- a/robohive/envs/myo/assets/hand/myo_hand_keyturn.xml +++ b/robohive/envs/myo/assets/hand/myohand_keyturn.xml @@ -7,8 +7,7 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ====================================================== --> - - + @@ -19,6 +18,7 @@ + diff --git a/robohive/envs/myo/assets/hand/myo_hand_pen.xml b/robohive/envs/myo/assets/hand/myohand_pen.xml similarity index 95% rename from robohive/envs/myo/assets/hand/myo_hand_pen.xml rename to robohive/envs/myo/assets/hand/myohand_pen.xml index e110f128..86325665 100644 --- a/robohive/envs/myo/assets/hand/myo_hand_pen.xml +++ b/robohive/envs/myo/assets/hand/myohand_pen.xml @@ -7,8 +7,7 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ====================================================== --> - - + @@ -20,6 +19,8 @@ + + diff --git a/robohive/envs/myo/assets/hand/myo_hand_pose.xml b/robohive/envs/myo/assets/hand/myohand_pose.xml similarity index 93% rename from robohive/envs/myo/assets/hand/myo_hand_pose.xml rename to robohive/envs/myo/assets/hand/myohand_pose.xml index 82352343..55a01439 100644 --- a/robohive/envs/myo/assets/hand/myo_hand_pose.xml +++ b/robohive/envs/myo/assets/hand/myohand_pose.xml @@ -7,13 +7,13 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ====================================================== --> - - + - + + diff --git a/robohive/envs/myo/base_v0.py b/robohive/envs/myo/base_v0.py index 18c4439c..c5d3247f 100644 --- a/robohive/envs/myo/base_v0.py +++ b/robohive/envs/myo/base_v0.py @@ -46,9 +46,8 @@ def _setup(self, weighted_reward_keys=weighted_reward_keys, frame_skip=frame_skip, **kwargs) + self.viewer_setup(azimuth=90, distance=1.5, render_actuator=True) - self.viewer_setup(azimuth=90, distance=1.5) - # self.sim.renderer._onscreen_renderer.vopt.flags[3] = 1 # render actuators #ToDo: make it binding agnostic def initializeConditions(self): # for muscle weakness we assume that a weaker muscle has a @@ -79,7 +78,7 @@ def step(self, a, **kwargs): muscle_a = a.copy() # Explicitely project normalized space (-1,1) to actuator space (0,1) if muscles - if self.sim.model.na: + if self.sim.model.na and self.normalize_act: # find muscle actuators muscle_act_ind = self.sim.model.actuator_dyntype==3 muscle_a[muscle_act_ind] = 1.0/(1.0+np.exp(-5.0*(muscle_a[muscle_act_ind]-0.5))) diff --git a/robohive/envs/myo/myochallenge/__init__.py b/robohive/envs/myo/myochallenge/__init__.py index 1a37a269..b816fe29 100644 --- a/robohive/envs/myo/myochallenge/__init__.py +++ b/robohive/envs/myo/myochallenge/__init__.py @@ -4,12 +4,31 @@ curr_dir = os.path.dirname(os.path.abspath(__file__)) import numpy as np +# MyoChallenge 2023 envs ============================================== +# MyoChallenge Die: Trial env +register(id='myoChallengeRelocateDemo-v0', + entry_point='robohive.envs.myo.myochallenge.relocate_v0:RelocateEnvV0', + max_episode_steps=150, + kwargs={ + 'model_path': curr_dir+'/../../../simhive/myo_sim/arm/myoarm_object_v0.14(mj236).mjb', + 'normalize_act': True, + 'frame_skip': 5, + 'rot_th': np.inf, # ignore rotation errors + 'target_xyz_range': {'high':[0.2, -.35, 0.9], 'low':[0.0, -.1, 0.9]}, + 'target_rxryrz_range': {'high':[0.0, 0.0, 0.0], 'low':[0.0, 0.0, 0.0]} + } + ) + + + +# MyoChallenge 2022 envs ============================================== + # MyoChallenge Die: Trial env register(id='myoChallengeDieReorientDemo-v0', entry_point='robohive.envs.myo.myochallenge.reorient_v0:ReorientEnvV0', max_episode_steps=150, kwargs={ - 'model_path': curr_dir+'/../assets/hand/myo_hand_die.xml', + 'model_path': curr_dir+'/../assets/hand/myohand_die.xml', 'normalize_act': True, 'frame_skip': 5, 'pos_th': np.inf, # ignore position error threshold @@ -22,7 +41,7 @@ entry_point='robohive.envs.myo.myochallenge.reorient_v0:ReorientEnvV0', max_episode_steps=150, kwargs={ - 'model_path': curr_dir+'/../assets/hand/myo_hand_die.xml', + 'model_path': curr_dir+'/../assets/hand/myohand_die.xml', 'normalize_act': True, 'frame_skip': 5, 'goal_pos': (-.010, .010), # +- 1 cm @@ -34,7 +53,7 @@ entry_point='robohive.envs.myo.myochallenge.reorient_v0:ReorientEnvV0', max_episode_steps=150, kwargs={ - 'model_path': curr_dir+'/../assets/hand/myo_hand_die.xml', + 'model_path': curr_dir+'/../assets/hand/myohand_die.xml', 'normalize_act': True, 'frame_skip': 5, # Randomization in goals @@ -52,7 +71,7 @@ entry_point='robohive.envs.myo.myochallenge.baoding_v1:BaodingEnvV1', max_episode_steps=200, kwargs={ - 'model_path': curr_dir+'/../assets/hand/myo_hand_baoding.xml', + 'model_path': curr_dir+'/../assets/hand/myohand_baoding.xml', 'normalize_act': True, 'goal_time_period': (5, 5), 'goal_xrange': (0.025, 0.025), @@ -65,7 +84,7 @@ entry_point='robohive.envs.myo.myochallenge.baoding_v1:BaodingEnvV1', max_episode_steps=200, kwargs={ - 'model_path': curr_dir+'/../assets/hand/myo_hand_baoding.xml', + 'model_path': curr_dir+'/../assets/hand/myohand_baoding.xml', 'normalize_act': True, 'goal_time_period': (4, 6), 'goal_xrange': (0.020, 0.030), diff --git a/robohive/envs/myo/myochallenge/relocate_v0.py b/robohive/envs/myo/myochallenge/relocate_v0.py new file mode 100644 index 00000000..04b6f024 --- /dev/null +++ b/robohive/envs/myo/myochallenge/relocate_v0.py @@ -0,0 +1,130 @@ +""" ================================================= +# Copyright (c) Facebook, Inc. and its affiliates +Authors :: Vikash Kumar (vikashplus@gmail.com), Vittorio Caggiano (caggiano@gmail.com) +================================================= """ + +import collections +from traceback import print_tb +import numpy as np +import gym + +from robohive.envs.myo.base_v0 import BaseV0 +from robohive.utils.quat_math import mat2euler, euler2quat + +class RelocateEnvV0(BaseV0): + + DEFAULT_OBS_KEYS = ['hand_qpos', 'hand_qvel', 'obj_pos', 'goal_pos', 'pos_err', 'obj_rot', 'goal_rot', 'rot_err'] + DEFAULT_RWD_KEYS_AND_WEIGHTS = { + "pos_dist": 100.0, + "rot_dist": 1.0, + } + + def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs): + # Two step construction (init+setup) is required for pickling to work correctly. + gym.utils.EzPickle.__init__(self, model_path, obsd_model_path, seed, **kwargs) + super().__init__(model_path=model_path, obsd_model_path=obsd_model_path, seed=seed, env_credits=self.MYO_CREDIT) + self._setup(**kwargs) + + def _setup(self, + target_xyz_range, # target position range (relative to initial pos) + target_rxryrz_range, # target rotation range (relative to initial rot) + obs_keys:list = DEFAULT_OBS_KEYS, + weighted_reward_keys:list = DEFAULT_RWD_KEYS_AND_WEIGHTS, + pos_th = .025, # position error threshold + rot_th = 0.262, # rotation error threshold + drop_th = 0.50, # drop height threshold + **kwargs, + ): + self.palm_sid = self.sim.model.site_name2id("S_grasp") + self.object_sid = self.sim.model.site_name2id("object_o") + self.goal_sid = self.sim.model.site_name2id("target_o") + self.success_indicator_sid = self.sim.model.site_name2id("target_ball") + self.goal_bid = self.sim.model.body_name2id("target") + self.target_xyz_range = target_xyz_range + self.target_rxryrz_range = target_rxryrz_range + self.pos_th = pos_th + self.rot_th = rot_th + self.drop_th = drop_th + + super()._setup(obs_keys=obs_keys, + weighted_reward_keys=weighted_reward_keys, + **kwargs, + ) + keyFrame_id = 0 + self.init_qpos[:] = self.sim.model.key_qpos[keyFrame_id].copy() + + + def get_obs_dict(self, sim): + obs_dict = {} + obs_dict['time'] = np.array([sim.data.time]) + obs_dict['hand_qpos'] = sim.data.qpos[:-7].copy() + obs_dict['hand_qvel'] = sim.data.qvel[:-6].copy()*self.dt + obs_dict['obj_pos'] = sim.data.site_xpos[self.object_sid] + obs_dict['goal_pos'] = sim.data.site_xpos[self.goal_sid] + obs_dict['palm_pos'] = sim.data.site_xpos[self.palm_sid] + obs_dict['pos_err'] = obs_dict['goal_pos'] - obs_dict['obj_pos'] + obs_dict['reach_err'] = obs_dict['palm_pos'] - obs_dict['obj_pos'] + obs_dict['obj_rot'] = mat2euler(np.reshape(sim.data.site_xmat[self.object_sid],(3,3))) + obs_dict['goal_rot'] = mat2euler(np.reshape(sim.data.site_xmat[self.goal_sid],(3,3))) + obs_dict['rot_err'] = obs_dict['goal_rot'] - obs_dict['obj_rot'] + + if sim.model.na>0: + obs_dict['act'] = sim.data.act[:].copy() + return obs_dict + + def get_reward_dict(self, obs_dict): + reach_dist = np.abs(np.linalg.norm(self.obs_dict['reach_err'], axis=-1)) + pos_dist = np.abs(np.linalg.norm(self.obs_dict['pos_err'], axis=-1)) + rot_dist = np.abs(np.linalg.norm(self.obs_dict['rot_err'], axis=-1)) + act_mag = np.linalg.norm(self.obs_dict['act'], axis=-1)/self.sim.model.na if self.sim.model.na !=0 else 0 + drop = reach_dist > self.drop_th + rwd_dict = collections.OrderedDict(( + # Perform reward tuning here -- + # Update Optional Keys section below + # Update reward keys (DEFAULT_RWD_KEYS_AND_WEIGHTS) accordingly to update final rewards + # Examples: Env comes pre-packaged with two keys pos_dist and rot_dist + + # Optional Keys + ('pos_dist', -1.*pos_dist), + ('rot_dist', -1.*rot_dist), + # Must keys + ('act_reg', -1.*act_mag), + ('sparse', -rot_dist-10.0*pos_dist), + ('solved', (pos_dist successful_steps: + num_success += 1 + score = num_success/num_paths + + # average activations over entire trajectory (can be shorter than horizon, if done) realized + effort = -1.0*np.mean([np.mean(p['env_infos']['rwd_dict']['act_reg']) for p in paths]) + + metrics = { + 'score': score, + 'effort':effort, + } + return metrics + + def reset(self, reset_qpos=None, reset_qvel=None): + self.sim.model.body_pos[self.goal_bid] = self.np_random.uniform(**self.target_xyz_range) + self.sim.model.body_quat[self.goal_bid] = euler2quat(self.np_random.uniform(**self.target_rxryrz_range)) + obs = super().reset(reset_qpos, reset_qvel) + return obs \ No newline at end of file diff --git a/robohive/envs/myo/pen_v0.py b/robohive/envs/myo/pen_v0.py index d27fcfa9..6a0f9f87 100644 --- a/robohive/envs/myo/pen_v0.py +++ b/robohive/envs/myo/pen_v0.py @@ -10,7 +10,6 @@ from robohive.envs.myo.base_v0 import BaseV0 from robohive.utils.quat_math import euler2quat from robohive.utils.vector_math import calculate_cosine -from os import sendfile class PenTwirlFixedEnvV0(BaseV0): diff --git a/robohive/envs/myo/sync_myo.sh b/robohive/envs/myo/sync_myo.sh index d41c67e3..52779f17 100755 --- a/robohive/envs/myo/sync_myo.sh +++ b/robohive/envs/myo/sync_myo.sh @@ -19,18 +19,18 @@ xml2mjb() } # Models -cd $mjc_path -xml2mjb arm myo_elbow_1dof6muscles_1dofexo -xml2mjb arm myo_elbow_1dof6muscles -xml2mjb basic myo_load -xml2mjb finger motor_finger_v0 -xml2mjb finger myo_finger_v0 -xml2mjb hand myo_hand_baoding -xml2mjb hand myo_hand_hold -xml2mjb hand myo_hand_keyturn -xml2mjb hand myo_hand_pen -xml2mjb hand myo_hand_pose -xml2mjb hand myo_hand_die +# cd $mjc_path +# xml2mjb arm myo_elbow_1dof6muscles_1dofexo +# xml2mjb arm myo_elbow_1dof6muscles +# xml2mjb basic myo_load +# xml2mjb finger motor_finger_v0 +# xml2mjb finger myo_finger_v0 +# xml2mjb hand myo_hand_baoding +# xml2mjb hand myo_hand_hold +# xml2mjb hand myo_hand_keyturn +# xml2mjb hand myo_hand_pen +# xml2mjb hand myo_hand_pose +# xml2mjb hand myo_hand_die cd $src_path echo $PWD @@ -38,11 +38,13 @@ echo $PWD mkdir -p $2/myosuite/envs/myo rsync -av --progress $src_path/robohive/envs/env_base.py $dst_path/myosuite/envs/ rsync -av --progress $src_path/robohive/envs/env_variants.py $dst_path/myosuite/envs/ +rsync -av --progress $src_path/robohive/envs/obs_vec_dict.py $dst_path/myosuite/envs/ rsync -av --progress $src_path/robohive/envs/myo/*.md $dst_path/myosuite/envs/myo/ +rsync -av --progress $src_path/robohive/envs/myo/assets/* $dst_path/myosuite/envs/myo/assets rsync -av --progress $src_path/robohive/envs/myo/*.py $dst_path/myosuite/envs/myo/ rsync -av --progress $src_path/robohive/envs/myo/myochallenge/*.py $dst_path/myosuite/envs/myo/myochallenge/ -sed -i '' "s/xml/mjb/g" $dst_path/myosuite/envs/myo/__init__.py -sed -i '' "s/xml/mjb/g" $dst_path/myosuite/envs/myo/myochallenge/__init__.py +# sed -i '' "s/xml/mjb/g" $dst_path/myosuite/envs/myo/__init__.py +# sed -i '' "s/xml/mjb/g" $dst_path/myosuite/envs/myo/myochallenge/__init__.py # Robot mkdir -p $2/myosuite/robot @@ -52,14 +54,31 @@ rsync -av --progress $src_path/robohive/robot/robot.py $dst_path/myosuite/robot/ mkdir -p $2/myosuite/utils rsync -av --progress $src_path/robohive/utils/*.py $dst_path/myosuite/utils/ +# Physics +mkdir -p $2/myosuite/physics +rsync -av --progress $src_path/robohive/physics/*.py $dst_path/myosuite/physics/ + +# renderer +mkdir -p $2/myosuite/renderer +rsync -av --progress $src_path/robohive/renderer/*.py $dst_path/myosuite/renderer/ + +# logger +mkdir -p $2/myosuite/logger +rsync -av --progress $src_path/robohive/logger/*.py $dst_path/myosuite/logger/ + # Test mkdir -p $2/myosuite/tests rsync -av --progress $src_path/robohive/tests/test_envs.py $dst_path/myosuite/tests/ rsync -av --progress $src_path/robohive/tests/test_myo.py $dst_path/myosuite/tests/ # Replace -# sed -i '' "s/robohive/myosuite/g" $dst_path/myosuite/envs/myo/__init__.py -find $dst_path/myosuite -type f -name "*.py" -exec sed -i '' "s/robohive\./myosuite\./g" {} \; +# sed -i "s/robohive\./myosuite\./g" $dst_path/myosuite/envs/myo/__init__.py +find $dst_path/myosuite -type f -name "*.py" -exec sed -i "s/robohive\./myosuite\./g" {} \; +find $dst_path/myosuite -type f -name "*.py" -exec sed -i "s/RoboHive:>/MyoSuite:>/g" {} \; # configs rsync -av --progress $src_path/.gitignore $dst_path/ + +# Clean unnecessary +rm $dst_path/myosuite/envs/myo/baoding_v1.py +rm $dst_path/myosuite/envs/myo/reorient_v0.py \ No newline at end of file diff --git a/robohive/envs/myo/walk_v0.py b/robohive/envs/myo/walk_v0.py index ad8eadaf..cc1d1268 100644 --- a/robohive/envs/myo/walk_v0.py +++ b/robohive/envs/myo/walk_v0.py @@ -1,6 +1,6 @@ """ ================================================= # Copyright (c) Facebook, Inc. and its affiliates -Authors :: Vikash Kumar (vikashplus@gmail.com), Vittorio Caggiano (caggiano@gmail.com) +Authors :: Vikash Kumar (vikashplus@gmail.com), Vittorio Caggiano (caggiano@gmail.com), Pierre Schumacher (schumacherpier@gmail.com) ================================================= """ import collections @@ -8,7 +8,7 @@ import numpy as np from robohive.envs.myo.base_v0 import BaseV0 - +from robohive.utils.quat_math import quat2mat class ReachEnvV0(BaseV0): @@ -18,7 +18,7 @@ class ReachEnvV0(BaseV0): "reach": 1.0, "bonus": 4.0, "penalty": 50, - "act_reg":1 + "act_reg": 1 } def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs): @@ -55,24 +55,12 @@ def _setup(self, **kwargs, ) self.init_qpos[:] = self.sim.model.key_qpos[0] + self.init_qvel[:] = self.sim.model.key_qvel[0] + # find geometries with ID == 1 which indicates the skins + geom_1_indices = np.where(self.sim.model.geom_group == 1) + # Change the alpha value to make it transparent + self.sim.model.geom_rgba[geom_1_indices, 3] = 0 - def get_obs_vec(self): - self.obs_dict['time'] = np.array([self.sim.data.time]) - self.obs_dict['qpos'] = self.sim.data.qpos[:].copy() - self.obs_dict['qvel'] = self.sim.data.qvel[:].copy()*self.dt - if self.sim.model.na>0: - self.obs_dict['act'] = self.sim.data.act[:].copy() - - # reach error - self.obs_dict['tip_pos'] = np.array([]) - self.obs_dict['target_pos'] = np.array([]) - for isite in range(len(self.tip_sids)): - self.obs_dict['tip_pos'] = np.append(self.obs_dict['tip_pos'], self.sim.data.site_xpos[self.tip_sids[isite]].copy()) - self.obs_dict['target_pos'] = np.append(self.obs_dict['target_pos'], self.sim.data.site_xpos[self.target_sids[isite]].copy()) - self.obs_dict['reach_err'] = np.array(self.obs_dict['target_pos'])-np.array(self.obs_dict['tip_pos']) - - t, obs = self.obsdict2obsvec(self.obs_dict, self.obs_keys) - return obs def get_obs_dict(self, sim): obs_dict = {} @@ -124,4 +112,263 @@ def reset(self): self.generate_target_pose() self.robot.sync_sims(self.sim, self.sim_obsd) obs = super().reset() - return obs \ No newline at end of file + return obs + +class WalkEnvV0(BaseV0): + + DEFAULT_OBS_KEYS = [ + 'qpos_without_xy', + 'qvel', + 'com_vel', + 'torso_angle', + 'feet_heights', + 'height', + 'feet_rel_positions', + 'phase_var', + 'muscle_length', + 'muscle_velocity', + 'muscle_force' + ] + + DEFAULT_RWD_KEYS_AND_WEIGHTS = { + "vel_reward": 5.0, + "done": -100, + "cyclic_hip": -10, + "ref_rot": 10.0, + "joint_angle_rew": 5.0 + } + + def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs): + + # EzPickle.__init__(**locals()) is capturing the input dictionary of the init method of this class. + # In order to successfully capture all arguments we need to call gym.utils.EzPickle.__init__(**locals()) + # at the leaf level, when we do inheritance like we do here. + # kwargs is needed at the top level to account for injection of __class__ keyword. + # Also see: https://github.com/openai/gym/pull/1497 + gym.utils.EzPickle.__init__(self, model_path, obsd_model_path, seed, **kwargs) + + # This two step construction is required for pickling to work correctly. All arguments to all __init__ + # calls must be pickle friendly. Things like sim / sim_obsd are NOT pickle friendly. Therefore we + # first construct the inheritance chain, which is just __init__ calls all the way down, with env_base + # creating the sim / sim_obsd instances. Next we run through "setup" which relies on sim / sim_obsd + # created in __init__ to complete the setup. + super().__init__(model_path=model_path, obsd_model_path=obsd_model_path, seed=seed) + self._setup(**kwargs) + + def _setup(self, + obs_keys: list = DEFAULT_OBS_KEYS, + weighted_reward_keys: dict = DEFAULT_RWD_KEYS_AND_WEIGHTS, + min_height = 0.8, + max_rot = 0.8, + hip_period = 100, + reset_type='init', + target_x_vel=0.0, + target_y_vel=1.2, + target_rot = None, + **kwargs, + ): + self.min_height = min_height + self.max_rot = max_rot + self.hip_period = hip_period + self.reset_type = reset_type + self.target_x_vel = target_x_vel + self.target_y_vel = target_y_vel + self.target_rot = target_rot + self.steps = 0 + super()._setup(obs_keys=obs_keys, + weighted_reward_keys=weighted_reward_keys, + **kwargs + ) + self.init_qpos[:] = self.sim.model.key_qpos[0] + self.init_qvel[:] = 0.0 + + def get_obs_dict(self, sim): + obs_dict = {} + obs_dict['t'] = np.array([sim.data.time]) + obs_dict['time'] = np.array([sim.data.time]) + obs_dict['qpos_without_xy'] = sim.data.qpos[2:].copy() + obs_dict['qvel'] = sim.data.qvel[:].copy() * self.dt + obs_dict['com_vel'] = np.array([self._get_com_velocity().copy()]) + obs_dict['torso_angle'] = np.array([self._get_torso_angle().copy()]) + obs_dict['feet_heights'] = self._get_feet_heights().copy() + obs_dict['height'] = np.array([self._get_height()]).copy() + obs_dict['feet_rel_positions'] = self._get_feet_relative_position().copy() + obs_dict['phase_var'] = np.array([(self.steps/self.hip_period) % 1]).copy() + obs_dict['muscle_length'] = self.muscle_lengths() + obs_dict['muscle_velocity'] = self.muscle_velocities() + obs_dict['muscle_force'] = self.muscle_forces() + + if sim.model.na>0: + obs_dict['act'] = sim.data.act[:].copy() + + return obs_dict + + def get_reward_dict(self, obs_dict): + vel_reward = self._get_vel_reward() + cyclic_hip = self._get_cyclic_rew() + ref_rot = self._get_ref_rotation_rew() + joint_angle_rew = self._get_joint_angle_rew(['hip_adduction_l', 'hip_adduction_r', 'hip_rotation_l', + 'hip_rotation_r']) + act_mag = np.linalg.norm(self.obs_dict['act'], axis=-1)/self.sim.model.na if self.sim.model.na !=0 else 0 + + rwd_dict = collections.OrderedDict(( + # Optional Keys + ('vel_reward', vel_reward), + ('cyclic_hip', cyclic_hip), + ('ref_rot', ref_rot), + ('joint_angle_rew', joint_angle_rew), + ('act_mag', act_mag), + # Must keys + ('sparse', vel_reward), + ('solved', vel_reward >= 1.0), + ('done', False), + # ('done', self._get_done()), + )) + rwd_dict['dense'] = np.sum([wt*rwd_dict[key] for key, wt in self.rwd_keys_wt.items()], axis=0) + return rwd_dict + + def get_randomized_initial_state(self): + # randomly start with flexed left or right knee + if self.np_random.uniform() < 0.5: + qpos = self.sim.model.key_qpos[2].copy() + qvel = self.sim.model.key_qvel[2].copy() + else: + qpos = self.sim.model.key_qpos[3].copy() + qvel = self.sim.model.key_qvel[3].copy() + + # randomize qpos coordinates + # but dont change height or rot state + rot_state = qpos[3:7] + height = qpos[2] + qpos[:] = qpos[:] + self.np_random.normal(0, 0.02, size=qpos.shape) + qpos[3:7] = rot_state + qpos[2] = height + return qpos, qvel + + def step(self, *args, **kwargs): + obs, reward, done, info = super().step(*args, **kwargs) + self.steps += 1 + return obs, reward, done, info + + def reset(self): + self.steps = 0 + if self.reset_type == 'random': + qpos, qvel = self.get_randomized_initial_state() + elif self.reset_type == 'init': + qpos, qvel = self.sim.model.key_qpos[2], self.sim.model.key_qvel[2] + else: + qpos, qvel = self.sim.model.key_qpos[0], self.sim.model.key_qvel[0] + self.robot.sync_sims(self.sim, self.sim_obsd) + obs = super().reset(reset_qpos=qpos, reset_qvel=qvel) + return obs + + def muscle_lengths(self): + return self.sim.data.actuator_length + + def muscle_forces(self): + return np.clip(self.sim.data.actuator_force / 1000, -100, 100) + + def muscle_velocities(self): + return np.clip(self.sim.data.actuator_velocity, -100, 100) + + def _get_done(self): + height = self._get_height() + if height < self.min_height: + return 1 + if self._get_rot_condition(): + return 1 + return 0 + + def _get_joint_angle_rew(self, joint_names): + """ + Get a reward proportional to the specified joint angles. + """ + mag = 0 + joint_angles = self._get_angle(joint_names) + mag = np.mean(np.abs(joint_angles)) + return np.exp(-5 * mag) + + def _get_feet_heights(self): + """ + Get the height of both feet. + """ + foot_id_l = self.sim.model.body_name2id('talus_l') + foot_id_r = self.sim.model.body_name2id('talus_r') + return np.array([self.sim.data.body_xpos[foot_id_l][2], self.sim.data.body_xpos[foot_id_r][2]]) + + def _get_feet_relative_position(self): + """ + Get the feet positions relative to the pelvis. + """ + foot_id_l = self.sim.model.body_name2id('talus_l') + foot_id_r = self.sim.model.body_name2id('talus_r') + pelvis = self.sim.model.body_name2id('pelvis') + return np.array([self.sim.data.body_xpos[foot_id_l]-self.sim.data.body_xpos[pelvis], self.sim.data.body_xpos[foot_id_r]-self.sim.data.body_xpos[pelvis]]) + + def _get_vel_reward(self): + """ + Gaussian that incentivizes a walking velocity. Going + over only achieves flat rewards. + """ + vel = self._get_com_velocity() + return np.exp(-np.square(self.target_y_vel - vel[1])) + np.exp(-np.square(self.target_x_vel - vel[0])) + + def _get_cyclic_rew(self): + """ + Cyclic extension of hip angles is rewarded to incentivize a walking gait. + """ + phase_var = (self.steps/self.hip_period) % 1 + des_angles = np.array([0.8 * np.cos(phase_var * 2 * np.pi + np.pi), 0.8 * np.cos(phase_var * 2 * np.pi)], dtype=np.float32) + angles = self._get_angle(['hip_flexion_l', 'hip_flexion_r']) + return np.linalg.norm(des_angles - angles) + + def _get_ref_rotation_rew(self): + """ + Incentivize staying close to the initial reference orientation up to a certain threshold. + """ + target_rot = [self.target_rot if self.target_rot is not None else self.init_qpos[3:7]][0] + return np.exp(-np.linalg.norm(5.0 * (self.sim.data.qpos[3:7] - target_rot))) + + def _get_torso_angle(self): + body_id = self.sim.model.body_name2id('torso') + return self.sim.data.body_xquat[body_id] + + def _get_com_velocity(self): + """ + Compute the center of mass velocity of the model. + """ + mass = np.expand_dims(self.sim.model.body_mass, -1) + cvel = - self.sim.data.cvel + return (np.sum(mass * cvel, 0) / np.sum(mass))[3:5] + + def _get_height(self): + """ + Get center-of-mass height. + """ + return self._get_com()[2] + + def _get_rot_condition(self): + """ + MuJoCo specifies the orientation as a quaternion representing the rotation + from the [1,0,0] vector to the orientation vector. To check if + a body is facing in the right direction, we can check if the + quaternion when applied to the vector [1,0,0] as a rotation + yields a vector with a strong x component. + """ + # quaternion of root + quat = self.sim.data.qpos[3:7].copy() + return [1 if np.abs((quat2mat(quat) @ [1, 0, 0])[0]) > self.max_rot else 0][0] + + def _get_com(self): + """ + Compute the center of mass of the robot. + """ + mass = np.expand_dims(self.sim.model.body_mass, -1) + com = self.sim.data.xipos + return (np.sum(mass * com, 0) / np.sum(mass)) + + def _get_angle(self, names): + """ + Get the angles of a list of named joints. + """ + return np.array([self.sim.data.qpos[self.sim.model.jnt_qposadr[self.sim.model.joint_name2id(name)]] for name in names]) \ No newline at end of file diff --git a/robohive/envs/obj_vec_dict.py b/robohive/envs/obs_vec_dict.py similarity index 100% rename from robohive/envs/obj_vec_dict.py rename to robohive/envs/obs_vec_dict.py diff --git a/robohive/envs/quadrupeds/__init__.py b/robohive/envs/quadrupeds/__init__.py new file mode 100644 index 00000000..73da8df1 --- /dev/null +++ b/robohive/envs/quadrupeds/__init__.py @@ -0,0 +1,130 @@ +from gym.envs.registration import register +from robohive.envs.env_variants import register_env_variant +import numpy as np +import os +curr_dir = os.path.dirname(os.path.abspath(__file__)) + +print("RoboHive:> Registering Quadruped Envs") + +WALK_HORIZON = 160 + +# Reach to fixed target +register( + id='DKittyWalkFixed-v0', + entry_point='robohive.envs.quadrupeds.walk_v0:WalkBaseV0', + max_episode_steps=WALK_HORIZON, #50steps*40Skip*2ms = 4s + kwargs={ + 'model_path': curr_dir+'/dkitty/dkitty_walk_v0.xml', + 'config_path': curr_dir+'/dkitty/dkitty_walk_v0.config', + 'dof_range_names':('A:FRJ10', 'A:BRJ42'), + 'act_range_names':('A:FRJ10', 'A:BRJ42') + } +) + +# Reach to random target +register_env_variant( + variant_id='DKittyWalkRandom-v0', + env_id='DKittyWalkFixed-v0', + variants={ + 'target_distance_range':(1.75, 2.25), + 'target_angle_range':(np.pi/2-np.pi/6, np.pi/2+np.pi/6), + }, + silent=True +) + +# Reach to random target using proprio and visual inputs +from robohive.envs.quadrupeds.walk_v0 import WalkBaseV0 +register_env_variant( + variant_id='DKittyWalkRandom_v2d-v0', + env_id='DKittyWalkRandom-v0', + variants={ + "visual_keys": WalkBaseV0.DEFAULT_VISUAL_KEYS, + # override the obs to avoid accidental leakage of oracle state info while using the visual envs + # using time as dummy obs. time keys are added twice to avoid unintended singleton expansion errors. + # Use proprioceptive data if needed - proprio_keys to configure, env.get_proprioception() to access + "obs_keys": ['time', 'time'] + }, + silent=True +) + + + +ORIENT_HORIZON = 80 +# Orient to fixed orientation +register( + id='DKittyOrientFixed-v0', + entry_point='robohive.envs.quadrupeds.orient_v0:OrientBaseV0', + max_episode_steps=ORIENT_HORIZON, #50steps*40Skip*2ms = 4s + kwargs={ + 'model_path': curr_dir+'/dkitty/dkitty_walk_v0.xml', + 'config_path': curr_dir+'/dkitty/dkitty_walk_v0.config', + 'dof_range_names':('A:FRJ10', 'A:BRJ42'), + 'act_range_names':('A:FRJ10', 'A:BRJ42'), + 'target_distance_range':(0, 0), + 'target_angle_range':(-np.pi/2, -np.pi/2), + } +) + +# Orient to random orientation +register_env_variant( + variant_id='DKittyOrientRandom-v0', + env_id='DKittyOrientFixed-v0', + variants={ + 'target_distance_range':(0, 0), + 'target_angle_range':(-np.pi/6, 2*np.pi/3), # Robel was (-2*np.pi/3, -np.pi/6) + }, + silent=True +) +# Orient to random orientation using proprio and visual inputs +from robohive.envs.quadrupeds.orient_v0 import OrientBaseV0 +register_env_variant( + variant_id='DKittyOrientRandom_v2d-v0', + env_id='DKittyOrientRandom-v0', + variants={ + "visual_keys": OrientBaseV0.DEFAULT_VISUAL_KEYS, + # override the obs to avoid accidental leakage of oracle state info while using the visual envs + # using time as dummy obs. time keys are added twice to avoid unintended singleton expansion errors. + # Use proprioceptive data if needed - proprio_keys to configure, env.get_proprioception() to access + "obs_keys": ['time', 'time'] + }, + silent=True +) + + + +STAND_HORIZON = 80 +# Stand-Up +register( + id='DKittyStandFixed-v0', + entry_point='robohive.envs.quadrupeds.stand_v0:StandBaseV0', + max_episode_steps=STAND_HORIZON, #50steps*40Skip*2ms = 4s + kwargs={ + 'model_path': curr_dir+'/dkitty/dkitty_walk_v0.xml', + 'config_path': curr_dir+'/dkitty/dkitty_stand_v0.config', + 'dof_range_names':('A:FRJ10', 'A:BRJ42'), + 'act_range_names':('A:FRJ10', 'A:BRJ42') + } +) +# Stand-Up from random pose +register_env_variant( + variant_id='DKittyStandRandom-v0', + env_id='DKittyStandFixed-v0', + variants={ + 'reset_type': 'random' + }, + silent=True +) +# Stand-Up from random pose using proprio and visual inputs +from robohive.envs.quadrupeds.orient_v0 import OrientBaseV0 +register_env_variant( + variant_id='DKittyStandRandom_v2d-v0', + env_id='DKittyStandRandom-v0', + variants={ + "visual_keys": OrientBaseV0.DEFAULT_VISUAL_KEYS, + # override the obs to avoid accidental leakage of oracle state info while using the visual envs + # using time as dummy obs. time keys are added twice to avoid unintended singleton expansion errors. + # Use proprioceptive data if needed - proprio_keys to configure, env.get_proprioception() to access + "obs_keys": ['time', 'time'] + }, + silent=True +) diff --git a/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config b/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config new file mode 100644 index 00000000..7e1c3b58 --- /dev/null +++ b/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config @@ -0,0 +1,63 @@ +{ + # device0: sensors, actuators + 'dkitty_root':{ + 'interface': {'type': 'optitrack', 'server_name': '169.254.163.86', 'client_name': '169.254.163.96','port':5000, 'packet_size':36, 'id':'1'}, + 'sensor':[ + {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'A:Tx'}, + {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'A:Ty'}, + {'range':(-2.00, 2.00), 'noise':0.005, 'hdr_id':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'A:Rx'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'A:Ry'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':0, 'name':'A:Rz'}, + ], + 'actuator':[] + }, + + # device1: sensors, actuators + 'dkitty':{ + 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/DKitty"}, + 'sensor':[ + {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_id':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_id':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_id':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'}, + {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_id':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':42, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ42_pos_sensor'} + ], + 'actuator':[ + {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10'}, + {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11'}, + {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12'}, + {'pos_range':(-0.279, 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20'}, + {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21'}, + {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22'}, + {'pos_range':(-0.279, 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30'}, + {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31'}, + {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32'}, + {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40'}, + {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41'}, + {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'} + ] + + # 'actuator':[ # restricted shoulder + # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12'}, + # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22'}, + # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32'}, + # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'} + # ] + } +} \ No newline at end of file diff --git a/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config b/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config new file mode 100644 index 00000000..33dfe430 --- /dev/null +++ b/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config @@ -0,0 +1,63 @@ +{ + # device0: sensors, actuators + 'dkitty_root':{ + 'interface': {'type': 'optitrack', 'server_name': '169.254.163.86', 'client_name': '169.254.163.96','port':5000, 'packet_size':36, 'id':'1'}, + 'sensor':[ + {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'A:Tx'}, + {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'A:Ty'}, + {'range':(-2.00, 2.00), 'noise':0.005, 'hdr_id':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'A:Rx'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'A:Ry'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':0, 'name':'A:Rz'}, + ], + 'actuator':[] + }, + + # device1: sensors, actuators + 'dkitty':{ + 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/DKitty"}, + 'sensor':[ + {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_id':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_id':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_id':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'}, + {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_id':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':42, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ42_pos_sensor'} + ], + 'actuator':[ + {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10'}, + {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11'}, + {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12'}, + {'pos_range':(-0.279, 0.75 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20'}, + {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21'}, + {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22'}, + {'pos_range':(-0.279, 0.75 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30'}, + {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31'}, + {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32'}, + {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40'}, + {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41'}, + {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'} + ] + + # 'actuator':[ # restricted shoulder + # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12'}, + # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22'}, + # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32'}, + # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'} + # ] + } +} \ No newline at end of file diff --git a/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.xml b/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.xml new file mode 100644 index 00000000..e38ea72e --- /dev/null +++ b/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.xml @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robohive/envs/quadrupeds/orient_v0.py b/robohive/envs/quadrupeds/orient_v0.py new file mode 100644 index 00000000..56ea8631 --- /dev/null +++ b/robohive/envs/quadrupeds/orient_v0.py @@ -0,0 +1,187 @@ +""" ================================================= +Copyright (C) 2018 Vikash Kumar +Author :: Vikash Kumar (vikashplus@gmail.com) +Source :: https://github.com/vikashplus/robohive +License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. +================================================= """ + +import collections +import gym +import numpy as np + +from robohive.envs import env_base +from robohive.utils.vector_math import calculate_cosine + +class OrientBaseV0(env_base.MujocoEnv): + + DEFAULT_OBS_KEYS = [ + 'upright', 'kitty_qpos', 'heading', 'target_error', 'last_a' + ] + + # Original Robel Obs (pinned to the origin) + # DEFAULT_OBS_KEYS = ['root_pos', 'root_euler', 'kitty_qpos', 'root_vel', 'root_angular_vel', 'kitty_qvel', 'last_action', 'upright', 'current_facing', 'desired_facing'] + + DEFAULT_RWD_KEYS_AND_WEIGHTS = { + 'target_dist_cost': 4.0, + 'upright': 2.0, + 'falling': 100.0, + 'heading': 5.0, + 'height': 0.5, + 'bonus_small': 5.0, + 'bonus_big': 10.0 + } + + DEFAULT_VISUAL_KEYS = [ + 'rgb:A:headCam:256x256:2d', + 'rgb:A:tailCam:256x256:2d', + 'rgb:A:leftCam:256x256:2d', + 'rgb:A:rightCam:256x256:2d', + ] + + DEFAULT_PROPRIO_KEYS = [ + "kitty_qpos", # radian + ] + + ENV_CREDIT = """\ + ROBEL: RObotics BEnchmarks for Learning with low-cost robots + Michael Ahn, Henry Zhu, Kristian Hartikainen, Hugo Ponte + Abhishek Gupta, Sergey Levine, Vikash Kumar + CoRL-2019 | https://sites.google.com/view/roboticsbenchmarks/ + """ + + def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs): + gym.utils.EzPickle.__init__(self, model_path, obsd_model_path, seed, **kwargs) + super().__init__(model_path=model_path, obsd_model_path=obsd_model_path, seed=seed, env_credits=self.ENV_CREDIT) + self._setup(**kwargs) + + + def _setup(self, + dof_range_names=None, + act_range_names=None, + upright_threshold: float = 0.9, + torso_site_name='torso', + target_site_name='target', + heading_site_name='heading', + target_distance_range = (0, 0), + target_angle_range = (np.pi/2, np.pi/2), + target_height_range = (0.28, 0.28), + frame_skip = 40, + obs_keys=DEFAULT_OBS_KEYS, + weighted_reward_keys=DEFAULT_RWD_KEYS_AND_WEIGHTS, + proprio_keys=DEFAULT_PROPRIO_KEYS, + **kwargs, + ): + + self._upright_threshold = upright_threshold + # ids + self.torso_sid = self.sim.model.site_name2id(torso_site_name) + self.target_sid = self.sim.model.site_name2id(target_site_name) + self.heading_sid = self.sim.model.site_name2id(heading_site_name) + self.target_distance_range, self.target_angle_range, self.target_height_range = target_distance_range, target_angle_range, target_height_range + + self.dof_range = range(self.sim.model.jnt_dofadr[self.sim.model.joint_name2id(dof_range_names[0])], + self.sim.model.jnt_dofadr[self.sim.model.joint_name2id(dof_range_names[1])]+1) + self.act_range = range(self.sim.model.actuator_name2id(act_range_names[0]), + self.sim.model.actuator_name2id(act_range_names[1])+1) + + super()._setup(obs_keys=obs_keys, + weighted_reward_keys=weighted_reward_keys, + frame_skip=frame_skip, + proprio_keys=proprio_keys, + **kwargs) + + # configure + for name, device in self.robot.robot_config.items(): + for act_id, actuator in enumerate(device['actuator']): + self.init_qpos[actuator['data_id']] = np.mean(actuator['pos_range']) + + + def get_obs_dict(self, sim): + target_xy = sim.data.site_xpos[self.target_sid, :2] + heading_xy = sim.data.site_xpos[self.heading_sid, :2] + kitty_xy = sim.data.site_xpos[self.torso_sid, :2] + kitty_xyz = sim.data.site_xpos[self.torso_sid, :3] + + # Get the heading of the torso (the y-axis). + current_heading = sim.data.site_xmat[self.torso_sid, [1, 4]] + + # Get the direction towards the heading location. + desired_heading = heading_xy - kitty_xy + + # Calculate the alignment of the heading with the desired direction. + heading = calculate_cosine(current_heading, desired_heading) + + # upright + up = sim.data.site_xmat[self.torso_sid][8] + + # target error in torso coordinate + rot_mat = np.reshape(sim.data.site_xmat[self.torso_sid], [3, 3]) + target_error_rel = np.matmul(rot_mat[:2,:2].transpose(), (target_xy - kitty_xy)) + + obs_dict = collections.OrderedDict(( + # Add observation terms relating to being upright. + ('time', np.array([self.time])), + ('last_a', self.last_ctrl.copy()), + ('upright', np.array([up])), + ('root_pos', kitty_xyz.copy()), # sim.data.qpos[:3], torso_track_state.pos), + ('root_euler', sim.data.qpos[3:6].copy()), #torso_track_state.rot_euler), + ('root_vel', sim.data.qvel[:3].copy()), # torso_track_state.vel), + ('root_angular_vel', sim.data.qvel[3:6].copy()), #torso_track_state.angular_vel), + ('kitty_qpos', sim.data.qpos[self.dof_range].copy()), #robot_state.qpos), + ('kitty_qvel', sim.data.qvel[self.dof_range].copy()), #robot_state.qvel), + ('heading', np.array([heading])), + ('target_pos', target_xy), + ('target_error', target_error_rel), + )) + return obs_dict + + + def get_reward_dict(self, obs_dict): + """Returns the reward for the given action and observation.""" + target_xy_dist = np.linalg.norm(obs_dict['target_error'], axis=-1) + heading = obs_dict['heading'][:,:,0] if obs_dict['heading'].ndim==3 else obs_dict['heading'][0] + upright = obs_dict['upright'][:,:,0] if obs_dict['upright'].ndim==3 else obs_dict['upright'][0] + quad_height = obs_dict['root_pos'][:,:,2] if obs_dict['root_pos'].ndim==3 else obs_dict['root_pos'][2] + + target_height_th = np.mean(self.target_height_range) + + rwd_dict = collections.OrderedDict(( + # Reward for proximity to the target. + ('target_dist_cost', -1.0 * target_xy_dist), + # staying upright + ('upright', (upright - self._upright_threshold)), + # not falling + ('falling', -1.0* (upright < self._upright_threshold)), + # Heading - 1 @ cos(0) to 0 @ cos(25deg). + # ('heading', (heading - 0.9) / 0.1), + ('heading', (heading + 1.0)), + # height + ('height', -1.*abs(quad_height-target_height_th)), + # Bonus when mean error < 15deg or upright within 15deg. + ('bonus_small', 1.0*(upright > self._upright_threshold) + 1.0*(heading > 0.9)), + # Bonus when mean error < 5deg or upright within 5deg. + ('bonus_big', 1.0 * (upright > self._upright_threshold) * (heading > 0.996)), + # Must keys + ('sparse', heading), + ('solved', (upright > self._upright_threshold) * (heading > 0.996)), + # ('done', upright < self._upright_threshold), + ('done', False), + )) + + rwd_dict['dense'] = np.sum([wt*rwd_dict[key] for key, wt in self.rwd_keys_wt.items()], axis=0) + return rwd_dict + + + def reset(self, reset_qpos=None, reset_qvel=None): + + reset_qpos = self.init_qpos.copy() if reset_qpos is None else reset_qpos + reset_qpos[6:] += np.pi/8*self.np_random.uniform(low=-1, high=1, size=self.sim.model.nq-6) + + target_dist = self.np_random.uniform(*self.target_distance_range) + target_theta = self.np_random.uniform(*self.target_angle_range) + + self.sim.model.site_pos[self.target_sid] = target_dist * np.array([np.cos(target_theta), np.sin(target_theta), 0]) + # Heading target is a bit farther away to avoid heading oscillations when quad is near xy_target + self.sim.model.site_pos[self.heading_sid] = (target_dist+0.5) * np.array([np.cos(target_theta), np.sin(target_theta), 0]) + obs = super().reset(reset_qpos, reset_qvel) + return obs diff --git a/robohive/envs/quadrupeds/stand_v0.py b/robohive/envs/quadrupeds/stand_v0.py new file mode 100644 index 00000000..071b7e17 --- /dev/null +++ b/robohive/envs/quadrupeds/stand_v0.py @@ -0,0 +1,194 @@ +""" ================================================= +Copyright (C) 2018 Vikash Kumar +Author :: Vikash Kumar (vikashplus@gmail.com) +Source :: https://github.com/vikashplus/robohive +License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. +================================================= """ + +import collections +import gym +import numpy as np + +from robohive.envs import env_base +from robohive.utils.vector_math import calculate_cosine + + +class StandBaseV0(env_base.MujocoEnv): + + DEFAULT_OBS_KEYS = [ + 'root_pos', + 'root_euler', + 'kitty_qpos', + 'root_vel', + 'root_angular_vel', + 'kitty_qvel', + 'last_action', + 'upright', + 'pose_error', + ] + + DEFAULT_RWD_KEYS_AND_WEIGHTS = { + 'pose_error_cost': 4.0, + 'center_distance_cost': 2.0, + 'bonus_small': 5.0, + 'bonus_big': 10.0, + 'upright': 1.0, + 'falling': 100, + } + + DEFAULT_VISUAL_KEYS = [ + 'rgb:A:headCam:256x256:2d', + 'rgb:A:tailCam:256x256:2d', + 'rgb:A:leftCam:256x256:2d', + 'rgb:A:rightCam:256x256:2d', + ] + + DEFAULT_PROPRIO_KEYS = [ + "kitty_qpos", # radian + ] + + ENV_CREDIT = """\ + ROBEL: RObotics BEnchmarks for Learning with low-cost robots + Michael Ahn, Henry Zhu, Kristian Hartikainen, Hugo Ponte + Abhishek Gupta, Sergey Levine, Vikash Kumar + CoRL-2019 | https://sites.google.com/view/roboticsbenchmarks/ + """ + + def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs): + gym.utils.EzPickle.__init__(self, model_path, obsd_model_path, seed, **kwargs) + super().__init__(model_path=model_path, obsd_model_path=obsd_model_path, seed=seed, env_credits=self.ENV_CREDIT) + self._setup(**kwargs) + + + def _setup(self, + dof_range_names=None, + act_range_names=None, + upright_threshold: float = 0.0, + torso_site_name='torso', + target_site_name='target', + heading_site_name='heading', + target_distance_range = (2.3, 2.3), + target_angle_range = (np.pi/2, np.pi/2), + target_height_range = (0.28, 0.28), + frame_skip = 40, + obs_keys=DEFAULT_OBS_KEYS, + weighted_reward_keys=DEFAULT_RWD_KEYS_AND_WEIGHTS, + reset_type='fixed', + proprio_keys=DEFAULT_PROPRIO_KEYS, + **kwargs, + ): + + self._upright_threshold = upright_threshold + self.reset_type = reset_type + + # ids + self.torso_sid = self.sim.model.site_name2id(torso_site_name) + self.target_sid = self.sim.model.site_name2id(target_site_name) + self.heading_sid = self.sim.model.site_name2id(heading_site_name) + self.target_distance_range, self.target_angle_range, self.target_height_range = target_distance_range, target_angle_range, target_height_range + + self.dof_range = range(self.sim.model.jnt_dofadr[self.sim.model.joint_name2id(dof_range_names[0])], + self.sim.model.jnt_dofadr[self.sim.model.joint_name2id(dof_range_names[1])]+1) + self.act_range = range(self.sim.model.actuator_name2id(act_range_names[0]), + self.sim.model.actuator_name2id(act_range_names[1])+1) + self._desired_pose = np.zeros(len(self.dof_range)) + + super()._setup(obs_keys=obs_keys, + weighted_reward_keys=weighted_reward_keys, + frame_skip=frame_skip, + proprio_keys=proprio_keys, + **kwargs) + + + def get_obs_dict(self, sim): + target_xy = sim.data.site_xpos[self.target_sid, :2] + heading_xy = sim.data.site_xpos[self.heading_sid, :2] + kitty_xy = sim.data.site_xpos[self.torso_sid, :2] + kitty_xyz = sim.data.site_xpos[self.torso_sid, :3] + + # Get the heading of the torso (the y-axis). + current_heading = sim.data.site_xmat[self.torso_sid, [1, 4]] + + # Get the direction towards the heading location. + desired_heading = heading_xy - kitty_xy + + # Calculate the alignment of the heading with the desired direction. + heading = calculate_cosine(current_heading, desired_heading) + + # upright + up = sim.data.site_xmat[self.torso_sid][8] + + # target error in torso coordinate + rot_mat = np.reshape(sim.data.site_xmat[self.torso_sid], [3, 3]) + target_error_rel = np.matmul(rot_mat[:2,:2].transpose(), (target_xy - kitty_xy)) + + obs_dict = collections.OrderedDict(( + # Add observation terms relating to being upright. + ('time', np.array([self.time])), + ('last_action', self.last_ctrl.copy()), + ('upright', np.array([up])), + ('root_pos', kitty_xyz.copy()), # sim.data.qpos[:3], torso_track_state.pos), + ('root_euler', sim.data.qpos[3:6].copy()), #torso_track_state.rot_euler), + ('root_vel', sim.data.qvel[:3].copy()), # torso_track_state.vel), + ('root_angular_vel', sim.data.qvel[3:6].copy()), #torso_track_state.angular_vel), + ('kitty_qpos', sim.data.qpos[self.dof_range].copy()), #robot_state.qpos), + ('kitty_qvel', sim.data.qvel[self.dof_range].copy()), #robot_state.qvel), + ('heading', np.array([heading])), + ('target_pos', target_xy), + ('target_error', target_error_rel), + ('pose_error', self._desired_pose - sim.data.qpos[self.dof_range]), + )) + return obs_dict + + + def get_reward_dict(self, obs_dict): + """Returns the reward for the given action and observation.""" + + pose_mean_error = np.abs(obs_dict['pose_error']).mean(axis=-1) + upright = obs_dict['upright'][:,:,0] if obs_dict['upright'].ndim==3 else obs_dict['upright'][0] + center_dist = np.linalg.norm(obs_dict['root_pos'][:2], axis=-1) + + rwd_dict = collections.OrderedDict(( + # staying upright + ('upright', (upright - self._upright_threshold)/(1 - self._upright_threshold)), + # not falling + ('falling', -1.0* (upright < self._upright_threshold)), + # Reward for closeness to desired pose. + ('pose_error_cost', -1 * pose_mean_error), + # Reward for closeness to center; i.e. being stationary. + ('center_distance_cost', -1 * center_dist), + # Bonus when mean error < 30deg, scaled by uprightedness. + ('bonus_small', 1.0 * (pose_mean_error < (np.pi / 6)) * upright), + # Bonus when mean error < 15deg and upright within 30deg. + ('bonus_big', 1.0 * (pose_mean_error < (np.pi / 12)) * (upright > 0.9)), + # Must keys + ('sparse', (1 - np.maximum(pose_mean_error / (np.pi / 3), 1))), # Normalized pose error by 60deg. + ('solved', (pose_mean_error < (np.pi / 12)) * (upright > 0.9)), + ('done', upright < self._upright_threshold), + )) + rwd_dict['dense'] = np.sum([wt*rwd_dict[key] for key, wt in self.rwd_keys_wt.items()], axis=0) + return rwd_dict + + + def reset(self, reset_qpos=None, reset_qvel=None): + + if reset_qpos is None: + reset_qpos = self.init_qpos.copy() + quad_pose = np.zeros(len(self.dof_range)) + + if self.reset_type.lower() == 'fixed': + quad_pose[[0, 3, 6, 9]] = 0 + quad_pose[[1, 4, 7, 10]] = np.pi / 4 + quad_pose[[2, 5, 8, 11]] = -np.pi / 2 + reset_qpos[self.dof_range] = quad_pose + + elif self.reset_type.lower() == 'random': + for name, device in self.robot.robot_config.items(): + for act_id, actuator in enumerate(device['actuator']): + reset_qpos[actuator['data_id']] = self.np_random.uniform(low=actuator['pos_range'][0], high=actuator['pos_range'][1]) + else: + raise TypeError(f"Unknown reset type: {self.reset_type}") + + obs = super().reset(reset_qpos, reset_qvel) + return obs + diff --git a/robohive/envs/quadrupeds/walk_v0.py b/robohive/envs/quadrupeds/walk_v0.py new file mode 100644 index 00000000..4a793c3b --- /dev/null +++ b/robohive/envs/quadrupeds/walk_v0.py @@ -0,0 +1,187 @@ +""" ================================================= +Copyright (C) 2018 Vikash Kumar +Author :: Vikash Kumar (vikashplus@gmail.com) +Source :: https://github.com/vikashplus/robohive +License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. +================================================= """ + +import collections +import gym +import numpy as np + +from robohive.envs import env_base +from robohive.utils.vector_math import calculate_cosine + +class WalkBaseV0(env_base.MujocoEnv): + + DEFAULT_OBS_KEYS = [ + 'upright', 'kitty_qpos', 'heading', 'target_error', 'last_a' + ] + + # Original Robel Obs (pinned to the origin) + # DEFAULT_OBS_KEYS = ['upright', 'root_pos', 'root_euler', 'kitty_qpos', 'heading', 'target_pos', 'target_error', 'last_a'] + + DEFAULT_RWD_KEYS_AND_WEIGHTS = { + 'target_dist_cost': 4.0, + 'upright': 1.0, + 'falling': 100.0, + 'heading': 2.0, + 'height': 0.5, + 'bonus_small': 5.0, + 'bonus_big': 10.0 + } + + DEFAULT_VISUAL_KEYS = [ + 'rgb:A:headCam:256x256:2d', + 'rgb:A:tailCam:256x256:2d', + 'rgb:A:leftCam:256x256:2d', + 'rgb:A:rightCam:256x256:2d', + ] + + DEFAULT_PROPRIO_KEYS = [ + "kitty_qpos", # radian + ] + + ENV_CREDIT = """\ + ROBEL: RObotics BEnchmarks for Learning with low-cost robots + Michael Ahn, Henry Zhu, Kristian Hartikainen, Hugo Ponte + Abhishek Gupta, Sergey Levine, Vikash Kumar + CoRL-2019 | https://sites.google.com/view/roboticsbenchmarks/ + """ + + + def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs): + gym.utils.EzPickle.__init__(self, model_path, obsd_model_path, seed, **kwargs) + super().__init__(model_path=model_path, obsd_model_path=obsd_model_path, seed=seed, env_credits=self.ENV_CREDIT) + self._setup(**kwargs) + + + def _setup(self, + dof_range_names=None, + act_range_names=None, + upright_threshold: float = 0.9, + torso_site_name='torso', + target_site_name='target', + heading_site_name='heading', + target_distance_range = (2.3, 2.3), + target_angle_range = (np.pi/2, np.pi/2), + target_height_range = (0.28, 0.28), + frame_skip = 40, + obs_keys=DEFAULT_OBS_KEYS, + weighted_reward_keys=DEFAULT_RWD_KEYS_AND_WEIGHTS, + proprio_keys=DEFAULT_PROPRIO_KEYS, + **kwargs, + ): + + self._upright_threshold = upright_threshold + # ids + self.torso_sid = self.sim.model.site_name2id(torso_site_name) + self.target_sid = self.sim.model.site_name2id(target_site_name) + self.heading_sid = self.sim.model.site_name2id(heading_site_name) + self.target_distance_range, self.target_angle_range, self.target_height_range = target_distance_range, target_angle_range, target_height_range + + self.dof_range = range(self.sim.model.jnt_dofadr[self.sim.model.joint_name2id(dof_range_names[0])], + self.sim.model.jnt_dofadr[self.sim.model.joint_name2id(dof_range_names[1])]+1) + self.act_range = range(self.sim.model.actuator_name2id(act_range_names[0]), + self.sim.model.actuator_name2id(act_range_names[1])+1) + + super()._setup(obs_keys=obs_keys, + proprio_keys=proprio_keys, + weighted_reward_keys=weighted_reward_keys, + frame_skip=frame_skip, + **kwargs) + + # configure + for name, device in self.robot.robot_config.items(): + for act_id, actuator in enumerate(device['actuator']): + self.init_qpos[actuator['data_id']] = np.mean(actuator['pos_range']) + + + def get_obs_dict(self, sim): + target_xy = sim.data.site_xpos[self.target_sid, :2] + heading_xy = sim.data.site_xpos[self.heading_sid, :2] + kitty_xy = sim.data.site_xpos[self.torso_sid, :2] + kitty_xyz = sim.data.site_xpos[self.torso_sid, :3] + + # Get the heading of the torso (the y-axis). + current_heading = sim.data.site_xmat[self.torso_sid, [1, 4]] + + # Get the direction towards the heading location. + desired_heading = heading_xy - kitty_xy + + # Calculate the alignment of the heading with the desired direction. + heading = calculate_cosine(current_heading, desired_heading) + + # upright + up = sim.data.site_xmat[self.torso_sid][8] + + # target error in torso coordinate + rot_mat = np.reshape(sim.data.site_xmat[self.torso_sid], [3, 3]) + target_error_rel = np.matmul(rot_mat[:2,:2].transpose(), (target_xy - kitty_xy)) + + obs_dict = collections.OrderedDict(( + # Add observation terms relating to being upright. + ('time', np.array([self.time])), + ('last_a', self.last_ctrl.copy()), + ('upright', np.array([up])), + ('root_pos', kitty_xyz.copy()), # sim.data.qpos[:3], torso_track_state.pos), + ('root_euler', sim.data.qpos[3:6].copy()), #torso_track_state.rot_euler), + ('root_vel', sim.data.qvel[:3].copy()), # torso_track_state.vel), + ('root_angular_vel', sim.data.qvel[3:6].copy()), #torso_track_state.angular_vel), + ('kitty_qpos', sim.data.qpos[self.dof_range].copy()), #robot_state.qpos), + ('kitty_qvel', sim.data.qvel[self.dof_range].copy()), #robot_state.qvel), + ('heading', np.array([heading])), + ('target_pos', target_xy), + ('target_error', target_error_rel), + )) + return obs_dict + + + def get_reward_dict(self, obs_dict): + """Returns the reward for the given action and observation.""" + target_xy_dist = np.linalg.norm(obs_dict['target_error'], axis=-1) + heading = obs_dict['heading'][:,:,0] if obs_dict['heading'].ndim==3 else obs_dict['heading'][0] + upright = obs_dict['upright'][:,:,0] if obs_dict['upright'].ndim==3 else obs_dict['upright'][0] + quad_height = obs_dict['root_pos'][:,:,2] if obs_dict['root_pos'].ndim==3 else obs_dict['root_pos'][2] + + target_distance_th = np.mean(self.target_distance_range) + target_height_th = np.mean(self.target_height_range) + + rwd_dict = collections.OrderedDict(( + # Reward for proximity to the target. + ('target_dist_cost', -1.0 * target_xy_dist), + # staying upright + ('upright', (upright - self._upright_threshold)), + # not falling + ('falling', -1.0* (upright < self._upright_threshold)), + # Heading - 1 @ cos(0) to 0 @ cos(25deg). + ('heading', (heading - 0.9) / 0.1), + # height + ('height', -1.*abs(quad_height-target_height_th)), + # Bonus + ('bonus_small', 1.0*(target_xy_dist < 0.75*target_distance_th) + 1.0*(heading > 0.9)), + ('bonus_big', 1.0 * (target_xy_dist < 0.5*target_distance_th) * (heading > 0.9)), + # Must keys + ('sparse', -1.0*target_xy_dist), + ('solved', (target_xy_dist < 0.5)), + ('done', upright < self._upright_threshold), + )) + + rwd_dict['dense'] = np.sum([wt*rwd_dict[key] for key, wt in self.rwd_keys_wt.items()], axis=0) + return rwd_dict + + + def reset(self, reset_qpos=None, reset_qvel=None): + + reset_qpos = self.init_qpos.copy() if reset_qpos is None else reset_qpos + reset_qpos[6:] += np.pi/8*self.np_random.uniform(low=-1, high=1, size=self.sim.model.nq-6) + + target_dist = self.np_random.uniform(*self.target_distance_range) + target_theta = self.np_random.uniform(*self.target_angle_range) + + self.sim.model.site_pos[self.target_sid] = target_dist * np.array([np.cos(target_theta), np.sin(target_theta), 0]) + # Heading target is a bit farther away to avoid heading oscillations when quad is near xy_target + self.sim.model.site_pos[self.heading_sid] = (target_dist+0.5) * np.array([np.cos(target_theta), np.sin(target_theta), 0]) + obs = super().reset(reset_qpos, reset_qvel) + return obs + diff --git a/robohive/envs/tcdm/track.py b/robohive/envs/tcdm/track.py index 4e1f0b9b..580debf3 100644 --- a/robohive/envs/tcdm/track.py +++ b/robohive/envs/tcdm/track.py @@ -8,7 +8,7 @@ import gym from robohive.envs import env_base from robohive.envs.tcdm.reference_motion import ReferenceMotion -from robohive.utils.quat_math import quat2euler, euler2quat, quatDiff2Vel +from robohive.utils.quat_math import quat2euler, euler2quat, quatDiff2Vel, mat2quat import numpy as np import os import collections @@ -45,7 +45,6 @@ def __init__(self, object_name, model_path, obsd_model_path=None, seed=None, **k processed_model_path = curr_dir+model_path[:-4]+"_processed.xml" with open(processed_model_path, 'w') as file: file.write(processed_xml) - self._object_name = object_name # Process obsd_model_path to import the right object if obsd_model_path == model_path: processed_obsd_model_path = processed_model_path @@ -101,7 +100,12 @@ def __init__(self, object_name, model_path, obsd_model_path=None, seed=None, **k self.TermPose = False ########################################## - self._lift_z = self.sim.data.get_body_xipos(self._object_name)[2] + self.lift_bonus_thresh + self.object_bid = self.sim.model.body_name2id(object_name) + self.wrist_bid = self.sim.model.body_name2id("wrist") + + self._lift_z = self.sim.data.xipos[self.object_bid][2] + self.lift_bonus_thresh + + self.initialized_pos = False self._setup(**kwargs) @@ -178,10 +182,10 @@ def get_obs_dict(self, sim): obs_dict['targ_hand_qvel'] = curr_ref.robot_vel ## info about current object com + rotations - obs_dict['curr_obj_com'] = sim.data.get_body_xipos(self._object_name).copy() - obs_dict['curr_obj_rot'] = sim.data.get_body_xquat(self._object_name).copy() + obs_dict['curr_obj_com'] = self.sim.data.xipos[self.object_bid].copy() + obs_dict['curr_obj_rot'] = mat2quat(np.reshape(self.sim.data.ximat[self.object_bid].copy(), (3,3))) - obs_dict['wrist_err'] = sim.data.get_body_xipos('wrist') + obs_dict['wrist_err'] = self.sim.data.xipos[self.wrist_bid].copy() obs_dict['base_error'] = obs_dict['curr_obj_com'] - obs_dict['wrist_err'] diff --git a/robohive/logger/examine_logs.py b/robohive/logger/examine_logs.py index ef1387fa..6452f02a 100644 --- a/robohive/logger/examine_logs.py +++ b/robohive/logger/examine_logs.py @@ -41,9 +41,9 @@ @click.option('-cp', '--compress_paths', type=bool, default=True, help=('compress paths. Remove obs and env_info/state keys')) @click.option('-pp', '--plot_paths', type=bool, default=False, help=('2D-plot of individual paths')) @click.option('-ea', '--env_args', type=str, default=None, help=('env args. E.g. --env_args "{\'is_hardware\':True}"')) -@click.option('-ns', '--noise_scale', type=float, default=0.0, help=('Noise amplitude in randians}"')) - -def main(env_name, rollout_path, rollout_format, mode, horizon, seed, num_repeat, render, camera_name, frame_size, output_dir, output_name, save_paths, compress_paths, plot_paths, env_args, noise_scale): +@click.option('-ns', '--noise_scale', type=float, default=0.0, help=('Noise amplitude in randians}')) +@click.option('-ie', '--include_exteroception', type=bool, default=False, help=('Include exteroception')) +def examine_logs(env_name, rollout_path, rollout_format, mode, horizon, seed, num_repeat, render, camera_name, frame_size, output_dir, output_name, save_paths, compress_paths, plot_paths, env_args, noise_scale, include_exteroception): # seed and load environments np.random.seed(seed) @@ -73,7 +73,7 @@ def main(env_name, rollout_path, rollout_format, mode, horizon, seed, num_repeat if output_name is None: # default to the rollout name rollout_name = os.path.split(rollout_path)[-1] output_name, output_type = os.path.splitext(rollout_name) - paths = Trace.load(rollout_path) + paths = Trace.load(trace_path=rollout_path, trace_type=rollout_format) # Resolve rendering if render == 'onscreen': @@ -121,8 +121,8 @@ def main(env_name, rollout_path, rollout_format, mode, horizon, seed, num_repeat trace_horizon = horizon if mode=='record' else path_data['time'].shape[0]-1 # Rollout path -------------------------------- - ep_rwd = 0.0 - obs, rwd, done, env_info = env.forward() + obs, rwd, done, env_info = env.forward(update_exteroception=include_exteroception) + ep_rwd = rwd for i_step in range(trace_horizon+1): # Get step's actions ---------------------- @@ -197,17 +197,17 @@ def main(env_name, rollout_path, rollout_format, mode, horizon, seed, num_repeat if rollout_format=='RoboSet': env.sim.data.qpos[:nq_arm]= path_data['qp_arm'][i_step+1] env.sim.data.qpos[nq_arm:nq_arm+nq_ee]= path_data['qp_ee'][i_step+1] - env.sim.data.qve[:nq_arm]= path_data['qv_arm'][i_step+1] + env.sim.data.qvel[:nq_arm]= path_data['qv_arm'][i_step+1] env.sim.data.qvel[nq_arm:nq_arm+nq_ee]= path_data['qv_ee'][i_step+1] env.sim.data.time = path_data['time'][i_step+1] elif rollout_format=='RoboHive' and "state" in path_data['env_infos'].keys(): env.set_env_state(path_state[i_step+1]) else: raise NotImplementedError("Settings not found") - obs, rwd, done, env_info = env.forward() + obs, rwd, done, env_info = env.forward(update_exteroception=include_exteroception) ep_rwd += rwd elif i_step < trace_horizon: # incase last step actions (nans) can cause issues in step - obs, rwd, done, env_info = env.step(act) + obs, rwd, done, env_info = env.step(act, update_exteroception=include_exteroception) ep_rwd += rwd # save offscreen buffers as video and clear the dataset @@ -216,7 +216,8 @@ def main(env_name, rollout_path, rollout_format, mode, horizon, seed, num_repeat trace.remove_dataset(group_keys=[path_name], dataset_key=camera_name) # Finish rollout - print(f"Finishing {path_name} rollout in {(time.time()-ep_t0):0.4} sec. Total rewards {ep_rwd}") + old_stat = f"(Old rollout rewards: {sum(path_data['rewards'][:])})" if path_data else "" + print(f"Finishing {path_name[:-2]} rollout in {(time.time()-ep_t0):0.4} sec. Total rewards {ep_rwd} "+old_stat) # Finish loop print("Finished rollout loop:{}".format(i_loop)) @@ -237,4 +238,4 @@ def main(env_name, rollout_path, rollout_format, mode, horizon, seed, num_repeat if __name__ == '__main__': - main() + examine_logs() diff --git a/robohive/logger/grouped_datasets.py b/robohive/logger/grouped_datasets.py index f0c56d9b..3450af9e 100644 --- a/robohive/logger/grouped_datasets.py +++ b/robohive/logger/grouped_datasets.py @@ -33,9 +33,9 @@ def get_type(input_type): A more robust way of getting trace type. Supports strings """ if type(input_type) == str: - if input_type.to_lower() == "robohive": + if input_type.lower() == "robohive": return TraceType.ROBOHIVE - elif input_type.to_lower() == "roboset": + elif input_type.lower() == "roboset": return TraceType.ROBOSET else: prompt(f"unknown TraceType{input_type}. Setting it to TraceType.UNSET", type=Prompt.WARN) diff --git a/robohive/physics/mj_sim_scene.py b/robohive/physics/mj_sim_scene.py index 006e767b..0beaa806 100644 --- a/robohive/physics/mj_sim_scene.py +++ b/robohive/physics/mj_sim_scene.py @@ -12,11 +12,12 @@ from typing import Any import robohive.utils.import_utils as import_utils +from robohive.utils.prompt_utils import prompt, Prompt import_utils.dm_control_isavailable() import_utils.mujoco_isavailable() import dm_control.mujoco as dm_mujoco -from robohive.renderer.mj_renderer import DMRenderer +from robohive.renderer.mj_renderer import MJRenderer from robohive.physics.sim_scene import SimScene @@ -47,15 +48,19 @@ def _load_simulation(self, model_handle: Any) -> Any: def advance(self, substeps: int = 1, render:bool = True): """Advances the simulation for one step.""" # Step the simulation substeps (frame_skip) times. - self.sim.step(substeps) + try: + self.sim.step(substeps) + except: + prompt("Simulation couldn't be stepped as intended. Issuing a reset", type=Prompt.WARN) + self.sim.reset() + if render: # self.renderer.refresh_window() self.renderer.render_to_window() - - def _create_renderer(self, sim: Any) -> DMRenderer: + def _create_renderer(self, sim: Any) -> MJRenderer: """Creates a renderer for the given simulation.""" - return DMRenderer(sim) + return MJRenderer(sim) def copy_model(self) -> Any: """Returns a copy of the MjModel object.""" diff --git a/robohive/physics/sim_scene.py b/robohive/physics/sim_scene.py index a79b3f11..0056ebb6 100644 --- a/robohive/physics/sim_scene.py +++ b/robohive/physics/sim_scene.py @@ -19,7 +19,18 @@ class SimBackend(enum.Enum): """Simulation library types.""" MUJOCO_PY = 0 - DM_CONTROL = 1 + MUJOCO = 1 + + # resolve sim backend + @staticmethod + def get_sim_backend()->'SimBackend': + sim_backend = os.getenv('sim_backend') + if sim_backend == 'MUJOCO_PY': + return SimBackend.MUJOCO_PY + elif sim_backend == 'MUJOCO' or sim_backend == None: + return SimBackend.MUJOCO + else: + raise ValueError("Unknown sim_backend: {}. Available choices: MUJOCO_PY, MUJOCO") class SimScene(metaclass=abc.ABCMeta): @@ -41,21 +52,21 @@ def create(*args, backend: Union[SimBackend, int], **kwargs) -> 'SimScene': if backend == SimBackend.MUJOCO_PY: from robohive.physics import mjpy_sim_scene # type: ignore return mjpy_sim_scene.MjPySimScene(*args, **kwargs) - elif backend == SimBackend.DM_CONTROL: + elif backend == SimBackend.MUJOCO: from robohive.physics import mj_sim_scene # type: ignore return mj_sim_scene.DMSimScene(*args, **kwargs) else: raise NotImplementedError(backend) - # resolve backend and return the sim + # Get sim as per the sim_backend @staticmethod def get_sim(model_handle: Any) -> 'SimScene': - sim_backend = os.getenv('sim_backend') - if sim_backend == 'MUJOCO_PY' or sim_backend == None: + sim_backend = SimBackend.get_sim_backend() + if sim_backend == SimBackend.MUJOCO_PY: return SimScene.create(model_handle=model_handle, backend=SimBackend.MUJOCO_PY) - elif sim_backend == 'MUJOCO': - return SimScene.create(model_handle=model_handle, backend=SimBackend.DM_CONTROL) + elif sim_backend == SimBackend.MUJOCO: + return SimScene.create(model_handle=model_handle, backend=SimBackend.MUJOCO) else: raise ValueError("Unknown sim_backend: {}. Available choices: MUJOCO_PY, MUJOCO") diff --git a/robohive/physics/sim_scene_test.py b/robohive/physics/sim_scene_test.py index f8c77905..4a3418f1 100644 --- a/robohive/physics/sim_scene_test.py +++ b/robohive/physics/sim_scene_test.py @@ -64,7 +64,7 @@ def test_fn(self: absltest.TestCase): fn( self, SimScene.create( - test_file_path, backend=SimBackend.DM_CONTROL)) + test_file_path, backend=SimBackend.MUJOCO)) return test_fn diff --git a/robohive/renderer/mj_renderer.py b/robohive/renderer/mj_renderer.py index ac8efd23..2e025178 100644 --- a/robohive/renderer/mj_renderer.py +++ b/robohive/renderer/mj_renderer.py @@ -7,15 +7,11 @@ """Rendering simulation using dm_control.""" -import sys - import numpy as np -import dm_control.mujoco as dm_mujoco -import dm_control.viewer as dm_viewer -import dm_control._render as dm_render +import mujoco +from mujoco import viewer from typing import Union - from robohive.renderer.renderer import Renderer, RenderMode # Default window dimensions. @@ -23,18 +19,23 @@ DEFAULT_WINDOW_HEIGHT = 480 # Default window title. -DEFAULT_WINDOW_TITLE = 'MuJoCo Viewer' +DEFAULT_WINDOW_TITLE = 'RoboHive Viewer' -# Internal renderbuffer size, in pixels. -_MAX_RENDERBUFFER_SIZE = 2048 - -class DMRenderer(Renderer): +class MJRenderer(Renderer): """Renders DM Control Physics objects.""" - def __init__(self, sim: dm_mujoco.Physics): + def __init__(self, sim): super().__init__(sim) self._window = None + self._renderer = None + + + def setup_renderer(self, model, height, width): + self._renderer = mujoco.Renderer(model, height=height, width=width) + self._scene_option = mujoco.MjvOption() + self._update_renderer_settings(self._scene_option) + def render_to_window(self): """Renders the Physics object to a window. @@ -44,22 +45,24 @@ def render_to_window(self): This function is a no-op if the window was already created. """ if not self._window: - self._window = DMRenderWindow() - self._window.load_model(self._sim) - self._update_camera_properties(self._window.camera) + self._window = viewer.launch_passive(self._sim.model.ptr, self._sim.data.ptr) + self._update_camera_properties(self._window.cam) + self._update_viewer_settings(self._window.opt) + + # self._window.cam.azimuth+=.1 # trick to rotate camera for 360 videos + self.refresh_window() - self._window.run_frame() def refresh_window(self): """Refreshes the rendered window if one is present.""" if self._window is None: return - self._window.run_frame() + self._window.sync() + def render_offscreen(self, - width: int, - height: int, - # mode: RenderMode = RenderMode.RGB, + width: int = DEFAULT_WINDOW_WIDTH, + height: int = DEFAULT_WINDOW_HEIGHT, rgb: bool = True, depth: bool = False, segmentation: bool = False, @@ -77,36 +80,29 @@ def render_offscreen(self, Returns: A numpy array of the pixels. """ - assert width > 0 and height > 0 + if camera_id == None: + camera_id = 0 + if self._renderer is None: + self.setup_renderer(self._sim.model.ptr, width=width, height=height) - if isinstance(camera_id, str): - camera_id = self._sim.model.name2id(camera_id, 'camera') - elif camera_id == None: - camera_id = -1 - - # TODO(michaelahn): Consider caching the camera. - camera = dm_mujoco.Camera( - physics=self._sim, height=height, width=width, camera_id=camera_id) - - # Update the camera configuration for the free-camera. - if camera_id == -1: - self._update_camera_properties(camera._render_camera) # pylint: disable=protected-access - - # image = camera.render( - # depth=(mode == RenderMode.DEPTH), - # segmentation=(mode == RenderMode.SEGMENTATION)) rgb_arr = None; dpt_arr = None; seg_arr = None if rgb: - rgb_arr = camera.render() - # rgb_arr = rgb_arr[::-1, :, :] + self._renderer.update_scene(self._sim.data.ptr, camera=camera_id, scene_option=self._scene_option) + rgb_arr = self._renderer.render() if depth: - dpt_arr = camera.render(depth=True) + self._renderer.enable_depth_rendering() + self._renderer.update_scene(self._sim.data.ptr, camera=camera_id, scene_option=self._scene_option) + dpt_arr = self._renderer.render() dpt_arr = dpt_arr[::-1, :] + self._renderer.disable_depth_rendering() if segmentation: - seg_arr = camera.render(segmentation=True) + self._renderer.enable_segmentation_rendering() + self._renderer.update_scene(self._sim.data.ptr, camera=camera_id, scene_option=self._scene_option) + dpt_arr = self._renderer.render() + dpt_arr = dpt_arr[::-1, :] + self._renderer.disable_segmentation_rendering() seg_arr = seg_arr[::-1, :] - camera._scene.free() # pylint: disable=protected-access if depth and segmentation: return rgb_arr, dpt_arr, seg_arr elif depth: @@ -116,74 +112,27 @@ def render_offscreen(self, else: return rgb_arr + def _update_viewer_settings(self, viewer): + """Updates the given camera object with the current camera settings.""" + for key, value in self._viewer_settings.items(): + if key == 'render_tendon': + viewer.flags[7] = value - def close(self): - """Cleans up any resources being used by the renderer.""" - if self._window: - self._window.close() - self._window = None - - -class DMRenderWindow: - """Class that encapsulates a graphical window.""" - - def __init__(self, - width: int = DEFAULT_WINDOW_WIDTH, - height: int = DEFAULT_WINDOW_HEIGHT, - title: str = DEFAULT_WINDOW_TITLE): - """Creates a graphical render window. + if key == 'render_actuator': + viewer.flags[4] = value - Args: - width: The width of the window. - height: The height of the window. - title: The title of the window. - """ - self._viewport = dm_viewer.renderer.Viewport(width, height) - self._window = dm_viewer.gui.RenderWindow(width, height, title) - self._viewer = dm_viewer.viewer.Viewer( - self._viewport, self._window.mouse, self._window.keyboard) - self._draw_surface = None - self._renderer = dm_viewer.renderer.NullRenderer() - @property - def camera(self): - return self._viewer._camera._camera # pylint: disable=protected-access + def _update_renderer_settings(self, renderer): + """Updates the given renderer object with the current camera settings.""" + for key, value in self._viewer_settings.items(): + if key == 'render_tendon': + renderer.flags[mujoco.mjtVisFlag.mjVIS_TENDON] = value + if key == 'render_actuator': + renderer.flags[mujoco.mjtVisFlag.mjVIS_ACTUATOR] = value + renderer.flags[mujoco.mjtVisFlag.mjVIS_ACTIVATION] = value def close(self): - self._viewer.deinitialize() - self._renderer.release() - self._draw_surface.free() - self._window.close() - - def load_model(self, physics): - """Loads the given Physics object to render.""" - self._viewer.deinitialize() - - self._draw_surface = dm_render.Renderer( - max_width=_MAX_RENDERBUFFER_SIZE, max_height=_MAX_RENDERBUFFER_SIZE) - self._renderer = dm_viewer.renderer.OffScreenRenderer( - physics.model, self._draw_surface) - - self._viewer.initialize(physics, self._renderer, touchpad=False) - - def run_frame(self): - """Renders one frame of the simulation. - - NOTE: This is extremely slow at the moment. - """ - # pylint: disable=protected-access - glfw = dm_viewer.gui.glfw_gui.glfw - glfw_window = self._window._context.window - if glfw.window_should_close(glfw_window): - sys.exit(0) - - self._viewport.set_size(*self._window.shape) - self._viewer.render() - pixels = self._renderer.pixels - - with self._window._context.make_current() as ctx: - ctx.call(self._window._update_gui_on_render_thread, glfw_window, - pixels) - self._window._mouse.process_events() - self._window._keyboard.process_events() - # pylint: enable=protected-access + """Cleans up any resources being used by the renderer.""" + if self._window: + self._window.close() + self._window = None \ No newline at end of file diff --git a/robohive/renderer/mjpy_renderer.py b/robohive/renderer/mjpy_renderer.py index 827dbd2c..05836639 100644 --- a/robohive/renderer/mjpy_renderer.py +++ b/robohive/renderer/mjpy_renderer.py @@ -23,21 +23,25 @@ def __init__(self, sim): self._onscreen_renderer = None self._offscreen_renderer = None + def render_to_window(self): """Renders the simulation to a window.""" if not self._onscreen_renderer: self._onscreen_renderer = mujoco_py.MjViewer(self._sim) self._update_camera_properties(self._onscreen_renderer.cam) + self._update_viewer_settings(self._onscreen_renderer.vopt) - self._onscreen_renderer.render() + self.refresh_window() # self._onscreen_renderer.cam.azimuth+=.1 # trick to rotate camera for 360 videos + def refresh_window(self): """Refreshes the rendered window if one is present.""" if self._onscreen_renderer is None: return self._onscreen_renderer.render() + def render_offscreen(self, width: int, height: int, @@ -93,3 +97,11 @@ def render_offscreen(self, return data[::-1, :] else: raise NotImplementedError(mode) + + def _update_viewer_settings(self, viewer): + """Updates the given camera object with the current camera settings.""" + for key, value in self._viewer_settings.items(): + if key == 'render_tendon': + viewer.flags[7] = value + if key == 'render_actuator': + viewer.flags[3] = value # mujoco diff --git a/robohive/renderer/renderer.py b/robohive/renderer/renderer.py index d6274a5e..efcaccbc 100644 --- a/robohive/renderer/renderer.py +++ b/robohive/renderer/renderer.py @@ -39,6 +39,7 @@ def __init__(self, sim): """ self._sim = sim self._camera_settings = {} + self._viewer_settings = {} @abc.abstractmethod def render_to_window(self): @@ -105,6 +106,26 @@ def set_free_camera_settings( self._camera_settings = settings + + def set_viewer_settings( + self, + render_tendon: Optional[float] = None, + render_actuator: Optional[float] = None, + ): + """Sets the viewer parameters. + + Args: + render_tendon: Turn tendon rendering on/off + render_actuator: Turn tendon rendering on/off + """ + viewer_settings = {} + if render_tendon is not None: + viewer_settings['render_tendon'] = render_tendon + if render_actuator is not None: + viewer_settings['render_actuator'] = render_actuator + + self._viewer_settings = viewer_settings + def close(self): """Cleans up any resources being used by the renderer.""" diff --git a/robohive/robot/hardware_franka.py b/robohive/robot/hardware_franka.py index b59d5be6..bb2b89d9 100644 --- a/robohive/robot/hardware_franka.py +++ b/robohive/robot/hardware_franka.py @@ -45,8 +45,8 @@ def forward(self, state_dict: Dict[str, torch.Tensor]): # Parse states q_current = state_dict["joint_positions"] qd_current = state_dict["joint_velocities"] - # self.feedback.Kp = self.kp - # self.feedback.Kd = self.kd + self.feedback.Kp = torch.diag(self.kp) + self.feedback.Kd = torch.diag(self.kd) # Execute PD control output = self.feedback( @@ -57,11 +57,12 @@ def forward(self, state_dict: Dict[str, torch.Tensor]): class FrankaArm(hardwareBase): - def __init__(self, name, ip_address, gain_scale=1.0, **kwargs): + def __init__(self, name, ip_address, gain_scale=1.0, reset_gain_scale=1.0, **kwargs): self.name = name self.ip_address = ip_address self.robot = None self.gain_scale = gain_scale + self.reset_gain_scale = reset_gain_scale def connect(self, policy=None): @@ -160,9 +161,20 @@ def reset(self, reset_pos=None, time_to_go=5): # generate min jerk trajectory dt = 0.1 waypoints = generate_joint_space_min_jerk(start=q_current, goal=reset_pos, time_to_go=time_to_go, dt=dt) + # reset using min_jerk traj for i in range(len(waypoints)): - self.apply_commands(q_desired=waypoints[i]['position']) + self.apply_commands( + q_desired=waypoints[i]['position'], + kp=self.reset_gain_scale * torch.Tensor(self.robot.metadata.default_Kq), + kd=self.reset_gain_scale * torch.Tensor(self.robot.metadata.default_Kqd), + ) time.sleep(dt) + + # reset back gains to gain-policy + self.apply_commands( + kp=self.gain_scale*torch.Tensor(self.robot.metadata.default_Kq), + kd=self.gain_scale*torch.Tensor(self.robot.metadata.default_Kqd) + ) else: # Use default controller print("Resetting using default controller") @@ -247,21 +259,19 @@ def get_args(): q_desired[5] = q_initial[5] + m * np.sin(np.pi * i / (T * hz)) # q_desired[5] = q_initial[5] + 0.05*np.random.uniform(high=1, low=-1) # q_desired = q_initial + 0.01*np.random.uniform(high=1, low=-1, size=7) - franka.apply_commands(q_desired = q_desired) time.sleep(1 / hz) - # import ipdb; ipdb.set_trace() # Udpate the gains - # kp_new = 0.0001* torch.Tensor(franka.robot.metadata.default_Kq) - # kd_new = 0.0001* torch.Tensor(franka.robot.metadata.default_Kqd) - # franka.apply_commands(kp=kp_new, kd=kd_new) - - # print("Starting sine motion updates again ...") - # for i in range(int(time_to_go * hz)): - # q_desired[5] = q_initial[5] + m * np.sin(np.pi * i / (T * hz)) - # franka.apply_commands(q_desired = q_desired) - # time.sleep(1 / hz) + kp_new = 0.1* torch.Tensor(franka.robot.metadata.default_Kq) + kd_new = 0.1* torch.Tensor(franka.robot.metadata.default_Kqd) + franka.apply_commands(kp=kp_new, kd=kd_new) + print("Starting sine motion updates again with updated gains.") + for i in range(int(time_to_go * hz)): + q_desired[5] = q_initial[5] + m * np.sin(np.pi * i / (T * hz)) + franka.apply_commands(q_desired = q_desired) + time.sleep(1 / hz) + print("Closing and exiting hardware connection") franka.close() diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index ee815161..bc1d47d2 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -71,7 +71,7 @@ def __init__(self, if mj_sim is None: # (creates new robot everytime to facilitate parallelization) prompt("Preparing robot-sim from %s" % model_path) - self.sim =SimScene.get_sim(model_path=model_path) + self.sim =SimScene.get_sim(model_handle=model_path) else: # use provided sim self.sim = mj_sim @@ -99,7 +99,7 @@ def __init__(self, # Robot's time self.time_start = time.time() - self.time = time.time() - self.time_start + self.time_wall = time.time()-self.time_start # Wall time (used for realtime factors) for both sim and hardware # refresh the sensor cache self._sensor_cache_refresh() @@ -410,7 +410,7 @@ def get_sensors(self, noise_scale=None, random_generator=None): self._sensor_cache.append(current_sen) # Update time - self.time = current_sen['time'] + self.time_wall = time.time()-self.time_start return current_sen @@ -694,10 +694,11 @@ def step(self, ctrl_desired, step_duration, ctrl_normalized=True, realTimeSim=Fa # synchronize time to maintain step_duration if self.is_hardware or realTimeSim: time_now = (time.time() - self.time_start) - time_left_in_step = step_duration - (time_now - self.time) + time_left_in_step = step_duration - (time_now-self.time_wall) if (time_left_in_step > 0.001): time.sleep(time_left_in_step) - # prompt("Step took %0.4fs, time left in step %0.4f"% ((time_now-self.time), time_left_in_step)) + elif time_left_in_step < 0.0: + prompt("Step duration %0.4fs, Step took %0.4fs, Time left %0.4f"% (step_duration, (time_now-self.time_wall), time_left_in_step), type=Prompt.WARN) if _ROBOT_VIZ: global timing_SRV_t0 @@ -753,7 +754,6 @@ def reset(self, # Ideally we should use actuator/ reset mechanism as in the real world # but choosing to directly resetting sim for efficiency self.sim.reset() - self.time = self.sim.data.time # update robot time self.sim.data.qpos[:] = feasibe_pos self.sim.data.qvel[:] = feasibe_vel self.sim.forward() # ???Vik alternatively should following be called? functions.mj_step1(self.sim.model, self.sim.data) @@ -768,8 +768,9 @@ def reset(self, # refresh sensor cache before exiting reset self._sensor_cache_refresh() - # restart the clock + # restart the robot clock self.time_start = time.time() + self.time_wall = time.time()-self.time_start global timing_SRV_t0 timing_SRV_t0 = time.time() @@ -790,31 +791,29 @@ def __del__(self): self.close() -def main(): +def demo_robot(): import gym - import darwin - import pprompt - prompt("Starting Robot===================") - env = gym.make('DarwinRideRandom-v0') - rob = env.env.mpl + env = gym.make('FrankaReachFixed-v0') + rob = env.env.robot prompt("Getting sensor data==============") - sen = rob.get_sensors(env.env) - pprompt.pprompt(sen) + sen = rob.get_sensors() + prompt("Sensor data: ", end="") + prompt(sen) prompt("stepping forward=================") ctrl = env.env.np_random.uniform(size=env.env.sim.model.nu) - rob.step(env.env, ctrl, 1.0) + rob.step(ctrl, 1.0) prompt("Resetting Robot==================") pos = env.env.np_random.uniform(size=env.env.sim.model.nq) vel = env.env.np_random.uniform(size=env.env.sim.model.nv) - rob.reset(env.env, pos, vel) + rob.reset(pos, vel) prompt("Closing Robot====================") rob.close() if __name__ == '__main__': - main() \ No newline at end of file + demo_robot() \ No newline at end of file diff --git a/robohive/simhive/myo_sim b/robohive/simhive/myo_sim index cde1442b..c798fbaf 160000 --- a/robohive/simhive/myo_sim +++ b/robohive/simhive/myo_sim @@ -1 +1 @@ -Subproject commit cde1442b92523feb26786a38b0b11ba2a9429dd3 +Subproject commit c798fbafe994eea7be7ed51e72ca1670289261e8 diff --git a/robohive/simhive/robel_sim b/robohive/simhive/robel_sim index 68030f77..4459db96 160000 --- a/robohive/simhive/robel_sim +++ b/robohive/simhive/robel_sim @@ -1 +1 @@ -Subproject commit 68030f77f73e247518f9620ab0eed01286ace7b4 +Subproject commit 4459db96edc359822356030e6223797e257ae4cc diff --git a/robohive/simhive/scene_sim b/robohive/simhive/scene_sim index f73acd52..e2eb354d 160000 --- a/robohive/simhive/scene_sim +++ b/robohive/simhive/scene_sim @@ -1 +1 @@ -Subproject commit f73acd52f3546939828d750744704603de03edf3 +Subproject commit e2eb354dce9aa1797dda081ab2dedf734a8761e6 diff --git a/robohive/tests/test_all.py b/robohive/tests/test_all.py index 2167e737..f1521739 100644 --- a/robohive/tests/test_all.py +++ b/robohive/tests/test_all.py @@ -1,12 +1,18 @@ import unittest from robohive.tests.test_arms import TestArms from robohive.tests.test_fm import TestFM -from robohive.tests.test_hand_manipulation_suite import TestHMS +from robohive.tests.test_hands import TestHMS from robohive.tests.test_multitask import TestKitchen from robohive.tests.test_myo import TestMyo from robohive.tests.test_claws import TestClaws -from robohive.tests.test_tcdm import TestTCDM +# from robohive.tests.test_tcdm import TestTCDM +from robohive.tests.test_examine_env import TestExamineEnv +from robohive.tests.test_examine_robot import TestExamineRobot +from robohive.tests.test_logger import TestExamineTrace +from robohive.tests.test_robot import TestRobot if __name__ == '__main__': + print("\n=================================", flush=True) + print("Testing Entire RoboHive") unittest.main() \ No newline at end of file diff --git a/robohive/tests/test_arms.py b/robohive/tests/test_arms.py index 637243b5..2027eff9 100644 --- a/robohive/tests/test_arms.py +++ b/robohive/tests/test_arms.py @@ -1,31 +1,10 @@ import unittest from robohive.tests.test_envs import TestEnvs +from robohive import robohive_arm_suite class TestArms(TestEnvs): - def test_arms(self): - env_names = [ - 'FrankaReachFixed-v0', - 'FrankaReachRandom-v0', - 'FrankaPushFixed-v0', - 'FrankaPushRandom-v0', - 'FetchReachFixed-v0', - 'FetchReachRandom-v0', - 'FrankaPickPlaceFixed-v0', - 'FrankaPickPlaceRandom-v0' - ] - self.check_envs('Arms', env_names) - - def test_arms_visual(self): - env_names = [ - 'FrankaReachRandom_v2d-v0', - 'FrankaReachRandom_vr3m18-v0', - 'FrankaReachRandom_vr3m34-v0', - 'FrankaReachRandom_vr3m50-v0', - 'FrankaReachRandom_vrrl18-v0', - 'FrankaReachRandom_vrrl34-v0', - 'FrankaReachRandom_vrrl50-v0', - ] - self.check_envs('Arms(visual)', env_names) + def test_envs(self): + self.check_envs('Arm Suite', robohive_arm_suite) if __name__ == '__main__': unittest.main() diff --git a/robohive/tests/test_claws.py b/robohive/tests/test_claws.py index a9bfb794..01d84e2a 100644 --- a/robohive/tests/test_claws.py +++ b/robohive/tests/test_claws.py @@ -1,13 +1,10 @@ import unittest from robohive.tests.test_envs import TestEnvs +from robohive import robohive_claw_suite class TestClaws(TestEnvs): - def test_claws(self): - env_names = [ - 'TrifingerReachFixed-v0', - 'TrifingerReachRandom-v0', - ] - self.check_envs('Claws', env_names) + def test_envs(self): + self.check_envs('Claw Suite', robohive_claw_suite) if __name__ == '__main__': unittest.main() \ No newline at end of file diff --git a/robohive/tests/test_envs.py b/robohive/tests/test_envs.py index ca75b87c..9ce870d9 100644 --- a/robohive/tests/test_envs.py +++ b/robohive/tests/test_envs.py @@ -12,18 +12,26 @@ import pickle import copy import torch.testing - +import os class TestEnvs(unittest.TestCase): def check_envs(self, module_name, env_names, lite=False, input_seed=1234): - print("\nTesting module:: ", module_name) + print("\n=================================", flush=True) + print("Testing module:: ", module_name) for env_name in env_names: print("Testing env: ", env_name, flush=True) self.check_env(env_name, input_seed) def check_env(self, environment_id, input_seed): + + # Skip tests for envs that requires encoder downloading + ROBOHIVE_TEST = os.getenv('ROBOHIVE_TEST') + if ROBOHIVE_TEST == 'LITE': + if "r3m" in environment_id or "rrl" in environment_id or "vc1" in environment_id: + return + # test init env1 = gym.make(environment_id, seed=input_seed) assert env1.get_input_seed() == input_seed @@ -64,7 +72,7 @@ def check_env(self, environment_id, input_seed): extero2 = env2.env.get_exteroception() torch.testing.assert_close(obs1, obs2) torch.testing.assert_close(proprio1, proprio2) - torch.testing.assert_close(extero1, extero2) + torch.testing.assert_close(extero1, extero2, atol=2, rtol=0.04) torch.testing.assert_close(rwd1, rwd2) assert (done1==done2), (done1, done2) assert len(infos1)==len(infos2), (infos1, infos2) diff --git a/robohive/tests/test_examine_env.py b/robohive/tests/test_examine_env.py new file mode 100644 index 00000000..9aa459cc --- /dev/null +++ b/robohive/tests/test_examine_env.py @@ -0,0 +1,54 @@ +import click +import click.testing +import unittest +from robohive.utils.examine_env import main as examine_env +import os + + +class TestExamineEnv(unittest.TestCase): + def test_main(self): + # Call your function and test its output/assertions + print("Testing env with random policy") + runner = click.testing.CliRunner() + result = runner.invoke(examine_env, ["--env_name", "door-v1", \ + "--num_episodes", 1, \ + "--render", "none"]) + print("OUTPUT", result.output.strip(), flush=True) + print("RESULT", result, flush=True) + print("EXCEPTION", result.exception, flush=True) + # print(result.output.strip()) + self.assertEqual(result.exception, None) + + def test_offscreen_rendering(self): + # Call your function and test its output/assertions + print("Testing offscreen rendering") + runner = click.testing.CliRunner() + result = runner.invoke(examine_env, ["--env_name", "door-v1", \ + "--num_episodes", 1, \ + "--render", "offscreen",\ + "--camera_name", "view_1"]) + print("OUTPUT", result.output.strip(), flush=True) + print("RESULT", result, flush=True) + print("EXCEPTION", result.exception, flush=True) + # print(result.output.strip()) + self.assertEqual(result.exception, None) + os.remove('random_policy0.mp4') + + def no_test_scripted_policy_loading(self): + # Call your function and test its output/assertions + print("Testing scripted policy loading") + runner = click.testing.CliRunner() + result = runner.invoke(examine_env, ["--env_name", "door-v1", \ + "--num_episodes", 1, \ + "--render", "offscreen",\ + "--policy_path", "robohive.utils.examine_env.rand_policy"]) + print("OUTPUT", result.output.strip(), flush=True) + print("RESULT", result, flush=True) + print("EXCEPTION", result.exception, flush=True) + # print(result.output.strip()) + self.assertEqual(result.exception, None) + +if __name__ == '__main__': + print("\n=================================", flush=True) + print("Testing Examine Env") + unittest.main() \ No newline at end of file diff --git a/robohive/tests/test_examine_robot.py b/robohive/tests/test_examine_robot.py new file mode 100644 index 00000000..bbee6ddc --- /dev/null +++ b/robohive/tests/test_examine_robot.py @@ -0,0 +1,49 @@ +import click +import click.testing +import unittest +from robohive.tutorials.examine_robot import main as examine_robot +import os + +class TestExamineRobot(unittest.TestCase): + def test_main(self): + # Call your function and test its output/assertions + print("\n=================================", flush=True) + print("Testing examine robot") + curr_dir = os.path.dirname(os.path.abspath(__file__)) + + runner = click.testing.CliRunner() + result = runner.invoke(examine_robot, ["--sim_path", curr_dir+"/../envs/arms/franka/assets/franka_reach_v0.xml", \ + "--config_path", curr_dir+"/../envs/arms/franka/assets/franka_reach_v0.config", \ + "--live_render", "False"]) + print("OUTPUT", result.output.strip(), flush=True) + print("RESULT", result, flush=True) + print("EXCEPTION", result.exception, flush=True) + + self.assertEqual(result.exception, None) + + # def test_offscreen_rendering(self): + # # Call your function and test its output/assertions + # print("Testing offscreen rendering") + # runner = click.testing.CliRunner() + # result = runner.invoke(examine_env, ["--env_name", "door-v1", \ + # "--num_episodes", 1, \ + # "--render", "offscreen",\ + # "--camera_name", "top_cam"]) + # print(result.output.strip()) + # self.assertEqual(result.exception, None) + + # def no_test_scripted_policy_loading(self): + # # Call your function and test its output/assertions + # print("Testing scripted policy loading") + # runner = click.testing.CliRunner() + # result = runner.invoke(examine_env, ["--env_name", "door-v1", \ + # "--num_episodes", 1, \ + # "--render", "offscreen",\ + # "--policy_path", "robohive.utils.examine_env.rand_policy"]) + # print(result.output.strip()) + # self.assertEqual(result.exception, None) + +if __name__ == '__main__': + print("\n=================================", flush=True) + print("Testing Examine Robot") + unittest.main() \ No newline at end of file diff --git a/robohive/tests/test_fm.py b/robohive/tests/test_fm.py index a20dfd58..d52cfda9 100644 --- a/robohive/tests/test_fm.py +++ b/robohive/tests/test_fm.py @@ -1,16 +1,10 @@ import unittest from robohive.tests.test_envs import TestEnvs +from robohive import robohive_fm_suite class TestFM(TestEnvs): - def test_fm(self): - env_names = [ - 'rpFrankaDmanusPoseFixed-v0', - 'rpFrankaDmanusPoseRandom-v0', - 'rpFrankaRobotiqPoseFixed-v0', - 'rpFrankaRobotiqPoseRandom-v0', - 'rpFrankaRobotiqData-v0' - ] - self.check_envs('FM', env_names) + def test_envs(self): + self.check_envs('FM Suite', robohive_fm_suite) if __name__ == '__main__': unittest.main() \ No newline at end of file diff --git a/robohive/tests/test_hand_manipulation_suite.py b/robohive/tests/test_hand_manipulation_suite.py deleted file mode 100644 index 4692ed9a..00000000 --- a/robohive/tests/test_hand_manipulation_suite.py +++ /dev/null @@ -1,52 +0,0 @@ -import unittest -from robohive.tests.test_envs import TestEnvs - -class TestHMS(TestEnvs): - # Hand Manipulation Suite - def test_hand_manipulation_suite(self): - env_names = [ - 'pen-v1', - 'door-v1', - 'hammer-v1', - 'relocate-v1', - # 'baoding-v1', 'baoding4th-v1', 'baoding8th-v1' - ] - self.check_envs('Hand Manipulation', env_names) - - def test_hand_manipulation_suite_visual(self): - env_names = [ - 'door_vr3m18-v1', - 'door_vr3m34-v1', - 'door_vr3m50-v1', - 'door_vrrl18-v1', - 'door_vrrl34-v1', - 'door_vrrl50-v1', - 'door_v2d-v1', - 'hammer_vr3m18-v1', - 'hammer_vr3m34-v1', - 'hammer_vr3m50-v1', - 'hammer_vrrl18-v1', - 'hammer_vrrl34-v1', - 'hammer_vrrl50-v1', - 'hammer_v2d-v1', - 'relocate_vr3m18-v1', - 'relocate_vr3m34-v1', - 'relocate_vr3m50-v1', - 'relocate_vrrl18-v1', - 'relocate_vrrl34-v1', - 'relocate_vrrl50-v1', - 'relocate_v2d-v1', - 'pen_vr3m18-v1', - 'pen_vr3m34-v1', - 'pen_vr3m50-v1', - 'pen_vrrl18-v1', - 'pen_vrrl34-v1', - 'pen_vrrl50-v1', - 'pen_v2d-v1' - ] - self.check_envs('Hand Manipulation(visual)', env_names) - -if __name__ == '__main__': - from robohive.utils.prompt_utils import set_prompt_verbosity, Prompt - set_prompt_verbosity(verbose_mode=Prompt.WARN) - unittest.main() diff --git a/robohive/tests/test_hands.py b/robohive/tests/test_hands.py new file mode 100644 index 00000000..7670a0b4 --- /dev/null +++ b/robohive/tests/test_hands.py @@ -0,0 +1,11 @@ +import unittest +from robohive.tests.test_envs import TestEnvs +from robohive import robohive_hand_suite + +class TestHMS(TestEnvs): + # Hand Manipulation Suite + def test_hand_manipulation_suite(self): + self.check_envs('Hand Suite', robohive_hand_suite) + +if __name__ == '__main__': + unittest.main() diff --git a/robohive/tests/test_logger.py b/robohive/tests/test_logger.py new file mode 100644 index 00000000..7f2bfe3a --- /dev/null +++ b/robohive/tests/test_logger.py @@ -0,0 +1,59 @@ +import unittest +import click +import click.testing + +from robohive.logger.grouped_datasets import test_trace +from robohive.logger.examine_logs import examine_logs +from robohive.utils.examine_env import main as examine_env +import os + +class TestTrace(unittest.TestCase): + def teast_trace(self): + # Call your function and test its output/assertions + print("Testing Trace Basics") + test_trace() + + +class TestExamineTrace(unittest.TestCase): + def test_logs_playback(self): + + print("\nTesting logger: Logs playback") + runner = click.testing.CliRunner() + result = runner.invoke(examine_env, ["--env_name", "door-v1", \ + "--num_episodes", 1, \ + "--render", "none",\ + "--save_paths", True,\ + "--output_name", "door_test_logs"]) + log_name = result.output.strip()[-38:] + + result = runner.invoke(examine_logs, ["--env_name", "door-v1", \ + "--rollout_path", log_name, \ + "--render", "none",\ + "--mode", "playback"]) + print(result.output.strip()) + os.remove(log_name) + + + def test_logs_render(self): + + print("\nTesting logger: Logs render") + runner = click.testing.CliRunner() + result = runner.invoke(examine_env, ["--env_name", "door-v1", \ + "--num_episodes", 1, \ + "--render", "none",\ + "--save_paths", True,\ + "--output_name", "door_test_logs"]) + log_name = result.output.strip()[-38:] + + result = runner.invoke(examine_logs, ["--env_name", "door-v1", \ + "--rollout_path", log_name, \ + "--render", "none",\ + "--mode", "render"]) + print(result.output.strip()) + os.remove(log_name) + + +if __name__ == '__main__': + print("\n=================================", flush=True) + print("Testing logger") + unittest.main() \ No newline at end of file diff --git a/robohive/tests/test_multitask.py b/robohive/tests/test_multitask.py index 87776847..138efab6 100644 --- a/robohive/tests/test_multitask.py +++ b/robohive/tests/test_multitask.py @@ -1,54 +1,11 @@ import unittest from robohive.tests.test_envs import TestEnvs +from robohive import robohive_multitask_suite class TestKitchen(TestEnvs): - # def test_kitchen(self): - # env_names = [ - # 'kitchen-v3', - # 'kitchen_rgb-v3', - # #substeps1 - # 'kitchen_micro_open-v3', - # 'kitchen_micro_close-v3', - # 'kitchen_rdoor_open-v3', - # 'kitchen_rdoor_close-v3', - # 'kitchen_ldoor_open-v3', - # 'kitchen_ldoor_close-v3', - # 'kitchen_sdoor_open-v3', - # 'kitchen_sdoor_close-v3', - # 'kitchen_light_on-v3', - # 'kitchen_light_off-v3', - # 'kitchen_knob4_on-v3', - # 'kitchen_knob4_off-v3', - # 'kitchen_knob3_on-v3', - # 'kitchen_knob3_off-v3', - # 'kitchen_knob2_on-v3', - # 'kitchen_knob2_off-v3', - # 'kitchen_knob1_on-v3', - # 'kitchen_knob1_off-v3', - # 'franka_micro_open-v3','franka_micro_close-v3','franka_micro_random-v3', - # 'franka_slide_open-v3','franka_slide_close-v3','franka_slide_random-v3', - # #substeps2 - # 'kitchen_micro_slide_close-v3','' - # #substeps9 - # 'kitchen_openall-v3', 'kitchen_closeall-v3', - # ] - # self.check_envs('Kitchen', env_names, lite=False) - def test_kitchen_v4(self): - env_names = [ - 'FK1_RelaxFixed-v4', 'FK1_MicroOpenFixed-v4', 'FK1_MicroCloseFixed-v4', 'FK1_RdoorOpenFixed-v4', 'FK1_RdoorCloseFixed-v4', 'FK1_LdoorOpenFixed-v4', 'FK1_LdoorCloseFixed-v4', 'FK1_SdoorOpenFixed-v4', 'FK1_SdoorCloseFixed-v4', 'FK1_LightOnFixed-v4', 'FK1_LightOffFixed-v4', 'FK1_Knob4OnFixed-v4', 'FK1_Knob4OffFixed-v4', 'FK1_Knob3OnFixed-v4', 'FK1_Knob3OffFixed-v4', 'FK1_Knob2OnFixed-v4', 'FK1_Knob2OffFixed-v4', 'FK1_Knob1OnFixed-v4', 'FK1_Knob1OffFixed-v4', 'FK1_Stove1KettleFixed-v4', 'FK1_Stove4KettleFixed-v4' - ] - self.check_envs('KitchenFixed-v4', env_names, lite=False) - - env_names = [ - 'FK1_RelaxRandom-v4', 'FK1_MicroOpenRandom-v4', 'FK1_MicroCloseRandom-v4', 'FK1_RdoorOpenRandom-v4', 'FK1_RdoorCloseRandom-v4', 'FK1_LdoorOpenRandom-v4', 'FK1_LdoorCloseRandom-v4', 'FK1_SdoorOpenRandom-v4', 'FK1_SdoorCloseRandom-v4', 'FK1_LightOnRandom-v4', 'FK1_LightOffRandom-v4', 'FK1_Knob4OnRandom-v4', 'FK1_Knob4OffRandom-v4', 'FK1_Knob3OnRandom-v4', 'FK1_Knob3OffRandom-v4', 'FK1_Knob2OnRandom-v4', 'FK1_Knob2OffRandom-v4', 'FK1_Knob1OnRandom-v4', 'FK1_Knob1OffRandom-v4', 'FK1_Stove1KettleRandom-v4', 'FK1_Stove4KettleRandom-v4' - ] - self.check_envs('KitchenRandom-v4', env_names, lite=False) - - env_names = [ - 'FK1_RelaxRandom_v2d-v4', 'FK1_MicroOpenRandom_v2d-v4', 'FK1_MicroCloseRandom_v2d-v4', 'FK1_RdoorOpenRandom_v2d-v4', 'FK1_RdoorCloseRandom_v2d-v4', 'FK1_LdoorOpenRandom_v2d-v4', 'FK1_LdoorCloseRandom_v2d-v4', 'FK1_SdoorOpenRandom_v2d-v4', 'FK1_SdoorCloseRandom_v2d-v4', 'FK1_LightOnRandom_v2d-v4', 'FK1_LightOffRandom_v2d-v4', 'FK1_Knob4OnRandom_v2d-v4', 'FK1_Knob4OffRandom_v2d-v4', 'FK1_Knob3OnRandom_v2d-v4', 'FK1_Knob3OffRandom_v2d-v4', 'FK1_Knob2OnRandom_v2d-v4', 'FK1_Knob2OffRandom_v2d-v4', 'FK1_Knob1OnRandom_v2d-v4', 'FK1_Knob1OffRandom_v2d-v4', 'FK1_Stove1KettleRandom_v2d-v4', 'FK1_Stove4KettleRandom_v2d-v4' - ] - self.check_envs('KitchenRandom_v2d-v4', env_names, lite=False) + def test_envs(self): + self.check_envs('Multi-Task Suite', robohive_multitask_suite) if __name__ == '__main__': unittest.main() \ No newline at end of file diff --git a/robohive/tests/test_myo.py b/robohive/tests/test_myo.py index 17267d1b..5c9aecdc 100644 --- a/robohive/tests/test_myo.py +++ b/robohive/tests/test_myo.py @@ -5,42 +5,11 @@ import unittest from robohive.tests.test_envs import TestEnvs +from robohive import robohive_myo_suite class TestMyo(TestEnvs): - def test_myo(self): - env_names = [ - 'motorFingerReachFixed-v0', 'motorFingerReachRandom-v0', - 'myoFingerReachFixed-v0', 'myoFingerReachRandom-v0', - 'myoHandReachFixed-v0', 'myoHandReachRandom-v0', - - 'motorFingerPoseFixed-v0', 'motorFingerPoseRandom-v0', - 'myoFingerPoseFixed-v0', 'myoFingerPoseRandom-v0', - - 'myoElbowPose1D6MFixed-v0', 'myoElbowPose1D6MRandom-v0', - 'myoElbowPose1D6MExoRandom-v0', 'myoElbowPose1D6MExoRandom-v0', - 'myoHandPoseFixed-v0', 'myoHandPoseRandom-v0', - - 'myoHandKeyTurnFixed-v0', 'myoHandKeyTurnRandom-v0', - 'myoHandObjHoldFixed-v0', 'myoHandObjHoldRandom-v0', - 'myoHandPenTwirlFixed-v0', 'myoHandPenTwirlRandom-v0', - - 'myoHandDieReorientFixed-v0', 'myoHandDieReorientRandom-v0', - - 'myoHandBaodingFixed-v1', 'myoHandBaodingRandom-v1', - 'myoHandBaodingFixed4th-v1','myoHandBaodingFixed8th-v1', - ] - for k in range(10): env_names+=['myoHandPose'+str(k)+'Fixed-v0'] - - env_names+="myoSarcFingerReachFixed-v0, myoFatiFingerReachFixed-v0, myoSarcFingerReachRandom-v0, myoFatiFingerReachRandom-v0, myoSarcElbowPose1D6MFixed-v0, myoFatiElbowPose1D6MFixed-v0, myoSarcElbowPose1D6MRandom-v0, myoFatiElbowPose1D6MRandom-v0, myoSarcElbowPose1D6MExoFixed-v0, myoFatiElbowPose1D6MExoFixed-v0, myoSarcElbowPose1D6MExoRandom-v0, myoFatiElbowPose1D6MExoRandom-v0, myoSarcFingerPoseFixed-v0, myoFatiFingerPoseFixed-v0, myoSarcFingerPoseRandom-v0, myoFatiFingerPoseRandom-v0, myoSarcHandPoseFixed-v0, myoFatiHandPoseFixed-v0, myoReafHandPoseFixed-v0, myoSarcHandPose0Fixed-v0, myoFatiHandPose0Fixed-v0, myoReafHandPose0Fixed-v0, myoSarcHandPose1Fixed-v0, myoFatiHandPose1Fixed-v0, myoReafHandPose1Fixed-v0, myoSarcHandPose2Fixed-v0, myoFatiHandPose2Fixed-v0, myoReafHandPose2Fixed-v0, myoSarcHandPose3Fixed-v0, myoFatiHandPose3Fixed-v0, myoReafHandPose3Fixed-v0, myoSarcHandPose4Fixed-v0, myoFatiHandPose4Fixed-v0, myoReafHandPose4Fixed-v0, myoSarcHandPose5Fixed-v0, myoFatiHandPose5Fixed-v0, myoReafHandPose5Fixed-v0, myoSarcHandPose6Fixed-v0, myoFatiHandPose6Fixed-v0, myoReafHandPose6Fixed-v0, myoSarcHandPose7Fixed-v0, myoFatiHandPose7Fixed-v0, myoReafHandPose7Fixed-v0, myoSarcHandPose8Fixed-v0, myoFatiHandPose8Fixed-v0, myoReafHandPose8Fixed-v0, myoSarcHandPose9Fixed-v0, myoFatiHandPose9Fixed-v0, myoReafHandPose9Fixed-v0, myoSarcHandPoseRandom-v0, myoFatiHandPoseRandom-v0, myoReafHandPoseRandom-v0, myoSarcHandReachFixed-v0, myoFatiHandReachFixed-v0, myoReafHandReachFixed-v0, myoSarcHandReachRandom-v0, myoFatiHandReachRandom-v0, myoReafHandReachRandom-v0, myoSarcHandKeyTurnFixed-v0, myoFatiHandKeyTurnFixed-v0, myoReafHandKeyTurnFixed-v0, myoSarcHandKeyTurnRandom-v0, myoFatiHandKeyTurnRandom-v0, myoReafHandKeyTurnRandom-v0, myoSarcHandObjHoldFixed-v0, myoFatiHandObjHoldFixed-v0, myoReafHandObjHoldFixed-v0, myoSarcHandObjHoldRandom-v0, myoFatiHandObjHoldRandom-v0, myoReafHandObjHoldRandom-v0, myoSarcHandPenTwirlFixed-v0, myoFatiHandPenTwirlFixed-v0, myoReafHandPenTwirlFixed-v0, myoSarcHandPenTwirlRandom-v0, myoFatiHandPenTwirlRandom-v0, myoReafHandPenTwirlRandom-v0, myoSarcHandBaodingFixed-v1, myoFatiHandBaodingFixed-v1, myoReafHandBaodingFixed-v1, myoSarcHandBaodingRandom-v1, myoFatiHandBaodingRandom-v1, myoReafHandBaodingRandom-v1, myoSarcHandBaodingFixed4th-v1, myoFatiHandBaodingFixed4th-v1, myoReafHandBaodingFixed4th-v1, myoSarcHandBaodingFixed8th-v1, myoFatiHandBaodingFixed8th-v1, myoReafHandBaodingFixed8th-v1".split(", ") - - self.check_envs('Myo', env_names) - - def test_myochallenge(self): - env_names = [ - 'myoChallengeDieReorientDemo-v0', 'myoChallengeDieReorientP1-v0', 'myoChallengeDieReorientP2-v0', - 'myoChallengeBaodingP1-v1', 'myoChallengeBaodingP2-v1' - ] - self.check_envs('MyoChallenge', env_names) + def test_envs(self): + self.check_envs('Myo Suite', robohive_myo_suite) if __name__ == '__main__': unittest.main() \ No newline at end of file diff --git a/robohive/tests/test_quads.py b/robohive/tests/test_quads.py new file mode 100644 index 00000000..648c8cb8 --- /dev/null +++ b/robohive/tests/test_quads.py @@ -0,0 +1,10 @@ +import unittest +from robohive.tests.test_envs import TestEnvs +from robohive import robohive_quad_suite + +class TestQuads(TestEnvs): + def test_envs(self): + self.check_envs('Quadruped Suite', robohive_quad_suite) + +if __name__ == '__main__': + unittest.main() \ No newline at end of file diff --git a/robohive/tests/test_robot.py b/robohive/tests/test_robot.py new file mode 100644 index 00000000..e1cf046b --- /dev/null +++ b/robohive/tests/test_robot.py @@ -0,0 +1,12 @@ +import unittest +from robohive.robot.robot import demo_robot + +class TestRobot(unittest.TestCase): + def test_robot(self): + # Call your function and test its output/assertions + demo_robot() + +if __name__ == '__main__': + print("\n=================================", flush=True) + print("Testing Robot") + unittest.main() \ No newline at end of file diff --git a/robohive/tutorials/1_env_registration_and_customization.ipynb b/robohive/tutorials/1_env_registration_and_customization.ipynb index ca3dca02..8d541ab4 100644 --- a/robohive/tutorials/1_env_registration_and_customization.ipynb +++ b/robohive/tutorials/1_env_registration_and_customization.ipynb @@ -136,7 +136,7 @@ "### 2. Registering env variants\n", "(note: there is a seperate tutorial specifically on how to customize obs/proprio/extero of an env)\n", "\n", - "While every RoboHive env are packaged with care, research projects often require flexibility to customize the prepackaed environments. RoboHive provides functionality to easily create variants of preregistered environments. We have found this functionality to be really useful when multiple closely related envs are required. For example -- env variations during hyper parameter sweeps, testing a policy on diffeernt env conditions, system identification." + "While every RoboHive env is packaged with care, research projects often require flexibility to customize the prepackaged environments. RoboHive provides functionality to easily create variants of pre-registered environments. We have found this functionality to be really useful when multiple closely related envs are required. For example -- env variations during hyper parameter sweeps, testing a policy on different env conditions, system identification." ] }, { @@ -158,12 +158,13 @@ ] }, { + "attachments": {}, "cell_type": "markdown", "id": "4d4cd8aa", "metadata": {}, "source": [ - "Lets inspect the two env variants. Pay attention to two details -- name and the updated specs. \n", - "1. Note that env-variant picks up a new unique name to distinguish itself from the original env. Unique name can also be expliciely provided using `variant_id`\n", + "Lets inspect the two env variants. Pay attention to two details -- name and the updated specs.\n", + "1. Note that env-variant picks up a new unique name to distinguish itself from the original env. Unique names can also be explicitly provided using `variant_id`\n", "2. Variant env picks up the missing details from the base env. Information is merged by defaults. `override_keys` can be used to fully override any keys" ] }, @@ -231,12 +232,12 @@ "source": [ "In the output you will note that the two envs have different settings but have the same id. This is confusing/misleading.\n", "\n", - "**NOTE:** Passing kwargs during make is not advisable as the updated env have the same id as the original env. This leads to two potential issues \n", + "**NOTE:** Passing kwargs during make is not advisable as the updated env has the same id as the original env. This leads to two potential issues \n", "\n", - "1. *Confusion while reporting results* - Reporting results using the env's name while its configurations have been changed leads to reproducibility issues. If env's configuration changes are needed, it is recommended to instead to use env_variant to register the altered name with its own unique id. This is a very common mistake in the field. Lets fix this! \n", + "1. *Confusion while reporting results* - Reporting results using the env's name while its configurations have been changed leads to reproducibility issues. If env's configuration changes are needed, it is recommended to instead to use env_variant to register the altered name with its own unique id. This is a very common mistake in the field. Let's fix this! \n", "**Recommendation**: For reporting results RoboHive recommends projects/papers to use `register_env_variant` at top of their scripts to create a unique env name `_FrankaReachRandom-v0` if default envs are customized in any way.\n", "\n", - "2. *Confusion while usage* - two env with the same id but different properties can lead to confusion during development/usage. " + "2. *Confusion while usage* - two env with the same id but different properties can lead to confusion during development/usage. " ] }, { diff --git a/robohive/tutorials/3_get_obs_proprio_extero.ipynb b/robohive/tutorials/3_get_obs_proprio_extero.ipynb index 829ab9fb..d39e8436 100644 --- a/robohive/tutorials/3_get_obs_proprio_extero.ipynb +++ b/robohive/tutorials/3_get_obs_proprio_extero.ipynb @@ -5,13 +5,13 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# How to get details from RoboHive Environments \n", - "RoboHive supports three ways to query the environment based on the nature of inforamtion required. Following information can be queried from an active environment\n", + "# How to get details from RoboHive Environments\n", + "RoboHive supports three ways to query the environment based on the nature of information required. Following information can be queried from an active environment\n", "1. Get observations (configured using `obs_keys`)\n", "2. Get proprioception (configured via `proprio_keys`)\n", "3. Get exteroception (configured via `visual_keys`)\n", "\n", - "Lets go through them in details one at a time. First we laod an environment, preconfigured with respective keys, and step it a few times - \n", + "Let's go through them in detail one at a time. First we load an environment, preconfigured with respective keys, and step it a few times -\n", "(we will go through the key configuration details in another tutorial)." ] }, @@ -39,10 +39,10 @@ "metadata": {}, "source": [ "### 1. get observations\n", - "This is the most commonly used method to get the current (state based) observation from the environment. Observation are customized using `env.obs_keys` and can contain any details that the environment has access to. RoboHive's doesn't put any restrictions on the information that can be provided by the environment.\n", + "This is the most commonly used method to get the current (state based) observation from the environment. Observations are customized using `env.obs_keys` and can contain any details that the environment has access to. RoboHive doesn't put any restrictions on the information that can be provided by the environment.\n", "\n", - "RoboHive env provides an `env.get_obs()` method to query observations. It triggers following sequence of events -\n", - "1. Uses robot (sim/hardware) to get sensors\n", + "RoboHive env provides an `env.get_obs()` method to query observations. It triggers the following sequence of events -\n", + "1. Uses a robot (sim/hardware) to get sensors\n", "2. Reconstructs (partially) observed-sim `env.sim_obsd` using (noisy) sensor data\n", "3. Build the full observation dictionary `env.obs_dict` using the observed-sim\n", "4. Build obs vector from the obs_dict (using the `env.obs_keys`)" @@ -64,7 +64,7 @@ "metadata": {}, "source": [ "### 2. get proprioception\n", - "This method is used to get *only proprioceptive signals* from the robohive environments. Proprioceptive signals are customized using `env.proprio_keys` and typically consists of proprioceptive sensors that are available to the agent/robots. Proprioception can be acquired in two ways \n", + "This method is used to get *only proprioceptive signals* from the robohive environments. Proprioceptive signals are customized using `env.proprio_keys` and typically consist of proprioceptive sensors that are available to the agent/robots. Proprioception can be acquired in two ways \n", "1. Recover from *existing* observation dictionary\n", "2. Update observation and get proprioception alongside (default behavior)" ] @@ -75,7 +75,7 @@ "metadata": {}, "outputs": [], "source": [ - "# Recover from existing observaton dictionary \n", + "# Recover from existing observation dictionary \n", "time, proprio_vec, proprio_dict = env.get_proprioception(env.obs_dict)\n", "\n", "# Update observation and get proprioception alongside\n", @@ -90,7 +90,7 @@ "metadata": {}, "source": [ "### 3. get exteroception\n", - "This method is used to get *only exteroceptive signals* from the robohive environments. exteroceptive signals typically consists of exteroceptive sensors that are available to the agent/robots. Currently we are fosuing primarily on cameras as exteroceptive inputs. It can be customized using `env.visual_keys`. Expanding to other exteroceptive modalities while possible and within scope, is not a tested functionality yet.\n", + "This method is used to get *only exteroceptive signals* from the robohive environments. exteroceptive signals typically consist of exteroceptive sensors that are available to the agent/robots. Currently we are focusing primarily on cameras as exteroceptive inputs. It can be customized using `env.visual_keys`. Expanding to other exteroceptive modalities while possible and within scope, is not a tested functionality yet.\n", "\n", "Note that exteroceptive sensors are expensive (compute + bandwidth), therefore its requires users to make explicit calls for an update. It can be acquired in two ways -\n", "\n", @@ -107,7 +107,7 @@ "# Make an explicit call to update\n", "extero_dict = env.get_exteroception()\n", "\n", - "# Update obdervation and get exteroception alongside\n", + "# Update observation and get exteroception alongside\n", "obs = env.get_obs(update_exteroception=True)\n", "print(env.visual_dict.keys())" ] @@ -146,7 +146,7 @@ "Note the observation vector - it looks too simple for such an environment of this complexity. This seems a little weird!\n", "It is a common practice in *RoboHive* to use ['time', 'time'] as observation for envs designed for visual diversity. This is for two reasons \n", "1. To avoid leaking oracle information via obs to the agents studying visual generalization \n", - "2. Single ['time'] key leads to silent singleton exansion when inputs of higher dimenstions are required. Replcicating the `time` keys twice helps to catch/expose this bug.\n", + "2. Single ['time'] key leads to silent singleton expansion when inputs of higher dimensions are required. Replicating the `time` keys twice helps to catch/expose this bug.\n", "\n", "### 2. Ways to ask for obs, proprioception, exteroception?\n", "The flexibility of RoboHive allows multiple ways to ask for information from the env. We outline a of options below - " diff --git a/robohive/tutorials/examine_robot.py b/robohive/tutorials/examine_robot.py index 0659aad9..74d0c401 100644 --- a/robohive/tutorials/examine_robot.py +++ b/robohive/tutorials/examine_robot.py @@ -13,7 +13,6 @@ $ python tutorials/examine_robot.py --sim_path envs/arms/franka/assets/franka_reach_v0.xml --config_path envs/arms/franka/assets/franka_reach_v0.config ''' -from mujoco_py import MjViewer from robohive.robot.robot import Robot import numpy as np import click @@ -32,11 +31,7 @@ def main(sim_path, config_path, is_hardware, jnt_amp, frame_skip, traj_horizon, # start robots and visualizers robot = Robot(robot_name="Robot Demo", model_path=sim_path, config_path=config_path, act_mode='pos', is_hardware=is_hardware) sim = robot.sim - if live_render: - viewer = MjViewer(sim) - render_cbk = viewer.render - else: - render_cbk = None + render_cbk = sim.renderer.render_to_window if live_render else None # derived variables traj_dt = frame_skip*sim.model.opt.timestep @@ -46,7 +41,8 @@ def main(sim_path, config_path, is_hardware, jnt_amp, frame_skip, traj_horizon, jnt_rng = 0.5*(sim.model.jnt_range[:,1]-sim.model.jnt_range[:,0]) # Goto joint targets - while True: + for iloop in range(10): + print(f"Loop ID:{iloop}") # Sample a new desired position des_jnt_pos = jnt_mean + jnt_amp*np.random.uniform(high=jnt_rng, low=-jnt_rng) @@ -56,7 +52,7 @@ def main(sim_path, config_path, is_hardware, jnt_amp, frame_skip, traj_horizon, robot.reset(reset_pos=jnt_mean, reset_vel=djnt_mean) sensors = robot.get_sensors() # gets latest sensors and propage it in the sim for _ in range(traj_nsteps): - robot.step(ctrl_desired=act, step_duration=traj_dt, ctrl_normalized=False, realTimeSim=True, render_cbk=render_cbk) + robot.step(ctrl_desired=act, step_duration=traj_dt, ctrl_normalized=False, realTimeSim=(live_render==True), render_cbk=render_cbk) sensors = robot.get_sensors() # gets current sensors and propage it in the sim # Report progress diff --git a/robohive/utils/dict_utils.py b/robohive/utils/dict_utils.py index 6be7e674..0913d93b 100644 --- a/robohive/utils/dict_utils.py +++ b/robohive/utils/dict_utils.py @@ -1,4 +1,5 @@ import numpy as np +import unittest def dict_numpify(data:dict, u_res=np.uint8, i_res=np.int8, f_res=np.float16)->dict: """ @@ -10,7 +11,7 @@ def dict_numpify(data:dict, u_res=np.uint8, i_res=np.int8, f_res=np.float16)->di for key, val in data.items(): # non iteratables if np.isscalar(val): - if isinstance(val, (bool, np.bool)): + if isinstance(val, (bool, np.bool_)): val = np.array([val], dtype=np.bool_) elif isinstance(val, (np.unsignedinteger,)): val = np.array([val], dtype=u_res) @@ -85,8 +86,8 @@ def flatten_dict(data:dict, name:str='', delimiter:str='/')->dict: return flat_dict -if __name__ == '__main__': +def demo_dict_util(): data = { 'none': None, 'bool': True, @@ -123,12 +124,18 @@ def flatten_dict(data:dict, name:str='', delimiter:str='/')->dict: print("Original data") print_dtype(data) - # print("\nFlattened data") - # print_dtype(flatten_dict(data)) + print("\nFlattened data") + print_dtype(flatten_dict(data)) print("\nNumpy-fied data") data = dict_numpify(data) print_dtype(data) +class TestMain(unittest.TestCase): + def test_main(self): + # Call your function and test its output/assertions + self.assertEqual(demo_dict_util(), None) +if __name__ == '__main__': + unittest.main() diff --git a/robohive/utils/examine_env.py b/robohive/utils/examine_env.py index ea0dc03a..d70f6b41 100644 --- a/robohive/utils/examine_env.py +++ b/robohive/utils/examine_env.py @@ -16,10 +16,12 @@ DESC = ''' Helper script to examine an environment and associated policy for behaviors; \n - either onscreen, or offscreen, or just rollout without rendering.\n -- save resulting paths as pickle or as 2D plots +- save resulting paths as pickle or as 2D plots \n +- rollout either learned policies or scripted policies (e.g. see rand_policy class below) \n USAGE:\n - $ python examine_env.py --env_name door-v0 \n - $ python examine_env.py --env_name door-v0 --policy my_policy.pickle --mode evaluation --episodes 10 \n + $ python examine_env.py --env_name door-v1 \n + $ python examine_env.py --env_name door-v1 --policy_path robohive.utils.examine_env.rand_policy \n + $ python examine_env.py --env_name door-v1 --policy_path my_policy.pickle --mode evaluation --episodes 10 \n ''' # Random policy @@ -30,7 +32,14 @@ def __init__(self, env, seed): def get_action(self, obs): # return self.env.np_random.uniform(high=self.env.action_space.high, low=self.env.action_space.low) - return self.env.action_space.sample(), {'mode': 'random samples'} + return self.env.action_space.sample(), {'mode': 'random samples', 'evaluation':self.env.action_space.sample()} + +def load_class_from_str(module_name, class_name): + try: + m = __import__(module_name, globals(), locals(), class_name) + return getattr(m, class_name) + except (ImportError, AttributeError): + return None # MAIN ========================================================= @click.command(help=DESC) @@ -57,17 +66,23 @@ def main(env_name, policy_path, mode, seed, num_episodes, render, camera_name, o # resolve policy and outputs if policy_path is not None: - pi = pickle.load(open(policy_path, 'rb')) - if output_dir == './': # overide the default - output_dir, pol_name = os.path.split(policy_path) - output_name = os.path.splitext(pol_name)[0] - if output_name is None: - pol_name = os.path.split(policy_path)[1] - output_name = os.path.splitext(pol_name)[0] + policy_tokens = policy_path.split('.') + pi = load_class_from_str('.'.join(policy_tokens[:-1]), policy_tokens[-1]) + + if pi is not None: + pi = pi(env, seed) + else: + pi = pickle.load(open(policy_path, 'rb')) + if output_dir == './': # overide the default + output_dir, pol_name = os.path.split(policy_path) + output_name = os.path.splitext(pol_name)[0] + if output_name is None: + pol_name = os.path.split(policy_path)[1] + output_name = os.path.splitext(pol_name)[0] else: pi = rand_policy(env, seed) mode = 'exploration' - output_name ='random_policy' + output_name ='random_policy' if output_name is None else output_name # resolve directory if (os.path.isdir(output_dir) == False) and (render=='offscreen' or save_paths or plot_paths is not None): diff --git a/robohive/utils/import_utils.py b/robohive/utils/import_utils.py index bcc77302..dbbb493b 100644 --- a/robohive/utils/import_utils.py +++ b/robohive/utils/import_utils.py @@ -1,88 +1,89 @@ +import importlib + def mujoco_py_isavailable(): - try: - import mujoco_py - except ImportError as e: - help = """ + help = """ Options: (1) follow setup instructions here: https://github.com/openai/mujoco-py/ (2) install mujoco_py via pip (pip install mujoco_py) (3) install free_mujoco_py via pip (pip install free-mujoco-py) """ - raise ModuleNotFoundError(f"{e}. {help}") + if importlib.util.find_spec("mujoco_py") is None: + raise ModuleNotFoundError(help) + def mujoco_isavailable(): - try: - import mujoco - except ImportError as e: - help = """ - Options: - (1) install robohive with encoders (pip install robohive['mujoco']) - (2) follow setup instructions here: https://github.com/deepmind/mujoco - (3) install mujoco via pip (pip install mujoco) + help = """ + Options: + (1) install robohive with encoders (pip install robohive['mujoco']) + (2) follow setup instructions here: https://github.com/deepmind/mujoco + (3) install mujoco via pip (pip install mujoco) - """ - raise ModuleNotFoundError(f"{e}. {help}") + """ + if importlib.util.find_spec("mujoco") is None: + raise ModuleNotFoundError(help) def dm_control_isavailable(): - try: - import dm_control - except ImportError as e: - help = """ - Options: - (1) install robohive with encoders (pip install robohive['mujoco']) - (2) follow setup instructions here: https://github.com/deepmind/dm_control - (3) install dm-control via pip (pip install dm-control) - """ - raise ModuleNotFoundError(f"{e}. {help}") + help = """ + Options: + (1) install robohive with encoders (pip install robohive['mujoco']) + (2) follow setup instructions here: https://github.com/deepmind/dm_control + (3) install dm-control via pip (pip install dm-control) + """ + if importlib.util.find_spec("dm_control") is None: + raise ModuleNotFoundError(help) def torch_isavailable(): - try: - import torch - except ImportError as e: - help = """ - To use visual keys, RoboHive requires torch - Options: - (1) install robohive with encoders (pip install robohive['encoder']) - (2) directly install torch via pip (pip install torch) - """ - raise ModuleNotFoundError(f"{e}. {help}") + help = """ + To use visual keys, RoboHive requires torch + Options: + (1) install robohive with encoders (pip install robohive['encoder']) + (2) directly install torch via pip (pip install torch) + """ + if importlib.util.find_spec("torch") is None: + raise ModuleNotFoundError(help) def torchvision_isavailable(): - try: - import torchvision - except ImportError as e: - help = """ - To use visual keys, RoboHive requires torchvision - Options: - (1) install robohive with encoders (pip install robohive['encoder']) - (2) directly install torchvision via pip (pip install torchvision) - """ - raise ModuleNotFoundError(f"{e}. {help}") + help = """ + To use visual keys, RoboHive requires torchvision + Options: + (1) install robohive with encoders (pip install robohive['encoder']) + (2) directly install torchvision via pip (pip install torchvision) + """ + if importlib.util.find_spec("torchvision") is None: + raise ModuleNotFoundError(help) def r3m_isavailable(): - try: - import r3m - except ImportError as e: - help = """ - To use R3M as encodes in visual keys, RoboHive requires R3M installation - Options: - (1) follow install instructions at https://sites.google.com/view/robot-r3m/ - (2) pip install 'r3m@git+https://github.com/facebookresearch/r3m.git' - """ - raise ModuleNotFoundError(f"{e}. {help}") + help = """ + To use R3M as encodes in visual keys, RoboHive requires R3M installation + Options: + (1) follow install instructions at https://sites.google.com/view/robot-r3m/ + (2) pip install 'r3m@git+https://github.com/facebookresearch/r3m.git' + """ + if importlib.util.find_spec("r3m") is None: + raise ModuleNotFoundError(help) + def vc_isavailable(): - try: - import vc_models - except ImportError as e: - help = """ - To use VC1 as encodes in visual keys, RoboHive requires VC1 installation - Options: - (1) follow install instructions at https://eai-vc.github.io/ - (2) pip install 'vc_models@git+https://github.com/facebookresearch/eai-vc.git@9958b278666bcbde193d665cc0df9ccddcdb8a5a#egg=vc_models&subdirectory=vc_models' - """ - raise ModuleNotFoundError(f"{e}. {help}") \ No newline at end of file + help = """ + To use VC1 as encodes in visual keys, RoboHive requires VC1 installation + Options: + (1) follow install instructions at https://eai-vc.github.io/ + (2) pip install 'vc_models@git+https://github.com/facebookresearch/eai-vc.git@9958b278666bcbde193d665cc0df9ccddcdb8a5a#egg=vc_models&subdirectory=vc_models' + """ + if importlib.util.find_spec("vc_models") is None: + raise ModuleNotFoundError(help) + + +if __name__ == '__main__': + mujoco_py_isavailable() + mujoco_isavailable() + dm_control_isavailable() + torch_isavailable() + torchvision_isavailable() + r3m_isavailable() + vc_isavailable() + diff --git a/robohive/utils/paths_utils.py b/robohive/utils/paths_utils.py index 3e481e1c..7b2e5f36 100644 --- a/robohive/utils/paths_utils.py +++ b/robohive/utils/paths_utils.py @@ -146,7 +146,7 @@ def plot(paths, env=None, fileName_prefix=''): if env and hasattr(env.env, "rwd_keys_wt"): ax = plt.subplot(nplt2, 2, 6) ax.set_prop_cycle(None) - for key in env.env.rwd_keys_wt.keys(): + for key in sorted(env.env.rwd_keys_wt.keys()): plt.plot( path['env_infos']['time'], path['env_infos']['rwd_dict'][key]*env.env.rwd_keys_wt[key], diff --git a/robohive/utils/prompt_utils.py b/robohive/utils/prompt_utils.py index 77e52d22..8801f316 100644 --- a/robohive/utils/prompt_utils.py +++ b/robohive/utils/prompt_utils.py @@ -1,23 +1,76 @@ +""" ================================================= +Copyright (C) 2018 Vikash Kumar +Author :: Vikash Kumar (vikashplus@gmail.com) +Source :: https://github.com/vikashplus/robohive +License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. +================================================= """ + +""" +Utility script to help with information verbosity produced by RoboHive +To control verbosity set env variable ROBOHIVE_VERBOSITY=NONE(default)/INFO/WARN/ERROR/ONCE/ALWAYS +""" + from termcolor import cprint import enum +import os + +# Define verbosity levels class Prompt(enum.IntEnum): """Prompt verbosity types""" - INFO = 0 - WARN = 1 - ERROR = 2 - NONE = 4 + NONE = 0 # default (lowest priority) + INFO = 1 + WARN = 2 + ERROR = 3 + ONCE = 4 # print only once + ALWAYS = 5 # print always (highest priority) + + +# Prompt Cache (to track for Prompt.ONCE messages) +PROMPT_CACHE = [] -# Global verbose mode -VERBOSE_MODE = Prompt.INFO -def set_prompt_verbosity(verbose_mode:Prompt=Prompt.INFO): +# Infer verbose mode to be used +VERBOSE_MODE = os.getenv('ROBOHIVE_VERBOSITY') +if VERBOSE_MODE==None: + VERBOSE_MODE = Prompt.WARN +else: + VERBOSE_MODE = VERBOSE_MODE.upper() + if VERBOSE_MODE == 'ALWAYS': + VERBOSE_MODE = Prompt.ALWAYS + elif VERBOSE_MODE == 'ERROR': + VERBOSE_MODE = Prompt.ERROR + elif VERBOSE_MODE == 'WARN': + VERBOSE_MODE = Prompt.WARN + elif VERBOSE_MODE == 'INFO': + VERBOSE_MODE = Prompt.INFO + elif VERBOSE_MODE == 'NONE': + VERBOSE_MODE = Prompt.NONE + else: + raise TypeError("Unknown ROBOHIVE_VERBOSITY option") + + +# Programatically override the verbosity +def set_prompt_verbosity(verbose_mode:Prompt=Prompt.NONE): global VERBOSE_MODE VERBOSE_MODE = verbose_mode +# Print information respecting the verbosty mode def prompt(data, color=None, on_color=None, flush=False, end="\n", type:Prompt=Prompt.INFO): + global PROMPT_CACHE + + # Resolve if we need to print + if type == Prompt.ONCE: + data_hash = hash(data) + if data_hash in PROMPT_CACHE: + type = Prompt.INFO + else: + PROMPT_CACHE.append(data_hash) + type = Prompt.ALWAYS + + # resolve print colors if on_color == None: if type==Prompt.WARN: color = "black" @@ -25,5 +78,8 @@ def prompt(data, color=None, on_color=None, flush=False, end="\n", type:Prompt=P elif type==Prompt.ERROR: on_color = "on_red" + # resolve printing if type>=VERBOSE_MODE: + if not isinstance(data, str): + data = data.__str__() cprint(data, color=color, on_color=on_color, flush=flush, end=end) diff --git a/robohive/utils/quat_math.py b/robohive/utils/quat_math.py index d0f6d916..aed7920e 100644 --- a/robohive/utils/quat_math.py +++ b/robohive/utils/quat_math.py @@ -172,4 +172,27 @@ def quat2mat(quat): mat[..., 2, 0] = xZ - wY mat[..., 2, 1] = yZ + wX mat[..., 2, 2] = 1.0 - (xX + yY) - return np.where((Nq > _FLOAT_EPS)[..., np.newaxis, np.newaxis], mat, np.eye(3)) \ No newline at end of file + return np.where((Nq > _FLOAT_EPS)[..., np.newaxis, np.newaxis], mat, np.eye(3)) + + + +# multiply vector by 3D rotation matrix transpose +def rotVecMatT(vec, mat): + return np.array([ + mat[0,0]*vec[0] + mat[1,0]*vec[1] + mat[2,0]*vec[2], + mat[0,1]*vec[0] + mat[1,1]*vec[1] + mat[2,1]*vec[2], + mat[0,2]*vec[0] + mat[1,2]*vec[1] + mat[2,2]*vec[2] + ]) + +# multiply vector by 3D rotation matrix +def rotVecMat(vec, mat): + return np.array([ + mat[0,0]*vec[0] + mat[0,1]*vec[1] + mat[0,2]*vec[2], + mat[1,0]*vec[0] + mat[1,1]*vec[1] + mat[1,2]*vec[2], + mat[2,0]*vec[0] + mat[2,1]*vec[1] + mat[2,2]*vec[2] + ]) + +# multiply vector by quat +def rotVecQuat(vec, quat): + mat = quat2mat(quat) + return rotVecMat(vec,mat) \ No newline at end of file diff --git a/setup.py b/setup.py index 14675b0e..552a8b66 100644 --- a/setup.py +++ b/setup.py @@ -27,7 +27,7 @@ def package_files(directory): setup( name='robohive', - version='0.5.0', + version='0.6.0', license='Apache 2.0', packages=find_packages(), package_data={"": extra_files}, @@ -40,7 +40,8 @@ def package_files(directory): install_requires=[ 'click', 'gym==0.13', - 'free-mujoco-py', + 'mujoco==2.3.6', + 'dm-control==1.0.11', 'termcolor', 'sk-video', 'flatten_dict', @@ -52,9 +53,8 @@ def package_files(directory): ], extras_require={ # To use mujoco bindings, run (pip install -e ".[mujoco]") and set sim_backend=MUJOCO - 'mujoco':[ - 'mujoco==2.3.3', - 'dm-control==1.0.11' + 'mujoco_py':[ + 'free-mujoco-py', ], # To use hardware dependencies, run (pip install -e ".[a0]") and follow install instructions inside robot 'a0': [ diff --git a/setup/README.md b/setup/README.md index b304bb37..db37993f 100644 --- a/setup/README.md +++ b/setup/README.md @@ -8,12 +8,12 @@ 1. Robohive can be installed directly from the [PyPi](https://pypi.org/project/robohive/). Additional flags can be supplied to use additional features ``` bash - # only mujoco_py as available sim_backend + # only mujoco as available sim_backend pip install robohive # mujoco_py+mujoco as available sim_backend - pip install robohive[mujoco] + pip install robohive[mujoco_py] # mujoco_py+mujoco+visual encoders - pip install robohive[mujoco, encoders] + pip install robohive[mujoco_py, encoder] ``` RoboHive will throw informative errors if any of these packages are invokes but not installed @@ -24,27 +24,68 @@ Note: RoboHive agressively uses [git-submodules](https://git-scm.com/book/en/v2/Git-Tools-Submodules). Therefore, it is important to use the exact command above for installation. Basic understanding of how to work with submodules is expected. ```bash - $ cd robohive - # only mujoco_py as available sim_backend - $ pip install -e . + # only mujoco as available sim_backend + $ pip install -e robohive # mujoco_py+mujoco as available sim_backend - $ pip install -e ".[mujoco]" - # mujoco_py+mujoco+visual encoders - pip install robohive[mujoco, encoders] + $ pip install -e robohive[mujoco_py] + # mujoco_py+mujoco+visual encoder + pip install -e robohive[mujoco_py, encoder] # with a0 binding for realworld robot - $ pip install -e ".[a0]" + $ pip install -e robohive[a0] ``` **OR** Add repo to pythonpath by updating `~/.bashrc` or `~/.bash_profile` ``` export PYTHONPATH=":$PYTHONPATH" ``` - To use `R3M`, `RRL`, `VC` as visual encoders. Open [setup.py](/setup.py), uncomment the `r3m` and `vc_models` dependency under `encoder` and then run +3. To use `R3M`, `RRL`, `VC` as visual encoders. + - For PyPI installation: Encoders can't be directly installated from PyPi as these encoders don't have PyPi packages. (we are working with the authors for a fix). To use the encoders, simply try using the encoders and follow the instructions on the console + - For editable installation + Open [setup.py](/setup.py), uncomment the `r3m` and `vc_models` dependency under `encoder` and then run ``` - # all visual encoders - pip install robohive[encoders] + # all visual encoders + pip install robohive[encoder] ``` -3. You can visualize the environments with random controls using the below command +4. You can visualize the environments with random controls using the below command ``` $ python robohive/utils/examine_env.py -e FrankaReachRandom-v0 - ``` \ No newline at end of file + ``` + + +# Installation FAQs +The package relies on `mujoco-py` which might be the trickiest part of the installation. See `known issues` below and also instructions from the mujoco-py [page](https://github.com/openai/mujoco-py) if you are stuck with mujoco-py installation. + +## Linux +- Install osmesa related dependencies: +``` +$ sudo apt-get install libgl1-mesa-dev libgl1-mesa-glx libglew-dev libosmesa6-dev build-essential libglfw3 +``` + +- Visualization in linux: If the linux system has a GPU, then mujoco-py does not automatically preload the correct drivers. We added an alias `MJPL` in bashrc (see instructions) which stands for mujoco pre-load. When runing any python script that requires rendering, prepend the execution with MJPL. + + - Update `bashrc` by adding the following lines and source it + ``` + alias MJPL='LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libGLEW.so:/usr/lib/nvidia-384/libGL.so' + ``` + - Use this preload when launching your scripts + ``` + $ MJPL python script.py + ``` +- If there are issues with install of pytorch, please follow instructions from the [pytorch website](https://pytorch.org/) to install it properly based on the specific version of CUDA (or CPU-only) you have. + +- If you encounter a patchelf error in mujoco_py install, you can fix this with the following command when inside the anaconda env: `conda install -c anaconda patchelf`. See this [page](https://github.com/openai/mujoco-py/issues/147) for additional info. + +## Mac OS + +- If there are issues with install of pytorch, please follow instructions from the [pytorch website](https://pytorch.org/) to install it properly. + +- If you encounter a patchelf error in mujoco_py install, you can fix this with the following command when inside the anaconda env: `conda install -c anaconda patchelf`. See this [page](https://github.com/openai/mujoco-py/issues/147) for additional info. + +- GCC error in Mac OS: If you get a GCC error from mujoco-py, you can get the correct version mujoco-py expects with `brew install gcc --without-multilib`. This may require uninstalling other versions of GCC that may have been previously installed with `brew remove gcc@6` for example. You can see which brew packages were already installed with `brew list`. + + +## Known Issues +- Errors related to osmesa during installation. This is a `mujoco-py` build error and would likely go away if the following command is used before creating the conda environment. If the problem still persists, please contact the developers of mujoco-py +``` +$ sudo apt-get install libgl1-mesa-dev libgl1-mesa-glx libglew-dev libosmesa6-dev +```