├── README.md ├── car_racing ├── models.py ├── tools.py ├── vmail.py └── wrappers.py ├── dm_control ├── models.py ├── tools.py ├── vmail.py └── wrappers.py ├── images └── VMAIL.png ├── robel_claw ├── models.py ├── robel │ ├── CONTRIBUTING.md │ ├── LICENSE │ ├── README.md │ ├── conda_envs │ │ ├── base.yml │ │ ├── mjrl.yml │ │ └── softlearning.yml │ ├── media │ │ └── cover.png │ ├── requirements.dev.txt │ ├── requirements.hardware.txt │ ├── requirements.txt │ ├── robel │ │ ├── __init__.py │ │ ├── components │ │ │ ├── __init__.py │ │ │ ├── base.py │ │ │ ├── base_test.py │ │ │ ├── builder.py │ │ │ ├── builder_test.py │ │ │ ├── robot │ │ │ │ ├── __init__.py │ │ │ │ ├── builder.py │ │ │ │ ├── dynamixel_client.py │ │ │ │ ├── dynamixel_client_test.py │ │ │ │ ├── dynamixel_robot.py │ │ │ │ ├── dynamixel_robot_test.py │ │ │ │ ├── dynamixel_utils.py │ │ │ │ ├── group_config.py │ │ │ │ ├── group_config_test.py │ │ │ │ ├── hardware_robot.py │ │ │ │ ├── hardware_robot_test.py │ │ │ │ ├── robot.py │ │ │ │ └── robot_test.py │ │ │ └── tracking │ │ │ │ ├── __init__.py │ │ │ │ ├── builder.py │ │ │ │ ├── group_config.py │ │ │ │ ├── hardware_tracker.py │ │ │ │ ├── phasespace_tracker.py │ │ │ │ ├── tracker.py │ │ │ │ ├── utils │ │ │ │ ├── __init__.py │ │ │ │ └── coordinate_system.py │ │ │ │ ├── virtual_reality │ │ │ │ ├── __init__.py │ │ │ │ ├── client.py │ │ │ │ ├── device.py │ │ │ │ └── poses.py │ │ │ │ └── vr_tracker.py │ │ ├── dclaw │ │ │ ├── __init__.py │ │ │ ├── assets │ │ │ │ ├── dclaw3xh_valve3_v0.xml │ │ │ │ ├── dclaw3xh_valve3_v1.xml │ │ │ │ ├── dclaw3xh_valve4_v0.xml │ │ │ │ └── dclaw3xh_valve4_v1.xml │ │ │ ├── base_env.py │ │ │ ├── pose.py │ │ │ ├── pose_test.py │ │ │ ├── screw.py │ │ │ ├── screw_test.py │ │ │ ├── scripted_reset.py │ │ │ ├── turn.py │ │ │ └── turn_test.py │ │ ├── dkitty │ │ │ ├── __init__.py │ │ │ ├── assets │ │ │ │ ├── dkitty_avoid-v0.xml │ │ │ │ ├── dkitty_orient-v0.xml │ │ │ │ ├── dkitty_play-v0.xml │ │ │ │ ├── dkitty_push-v0.xml │ │ │ │ ├── dkitty_stand-v0.xml │ │ │ │ └── dkitty_walk-v0.xml │ │ │ ├── avoid.py │ │ │ ├── base_env.py │ │ │ ├── orient.py │ │ │ ├── orient_test.py │ │ │ ├── push.py │ │ │ ├── stand.py │ │ │ ├── stand_test.py │ │ │ ├── utils │ │ │ │ ├── __init__.py │ │ │ │ ├── manual_reset.py │ │ │ │ └── scripted_reset.py │ │ │ ├── walk.py │ │ │ └── walk_test.py │ │ ├── robel-scenes │ │ │ ├── .gitignore │ │ │ ├── CONTRIBUTING.md │ │ │ ├── LICENSE │ │ │ ├── README.md │ │ │ ├── dclaw │ │ │ │ ├── assets │ │ │ │ │ ├── chain3xh.xml │ │ │ │ │ ├── chain3xh_overlay.xml │ │ │ │ │ └── dependencies3xh.xml │ │ │ │ ├── dclaw3xh.xml │ │ │ │ └── meshes │ │ │ │ │ ├── acrylic_circle.stl │ │ │ │ │ ├── xh_4_clamping_high_poly.stl │ │ │ │ │ ├── xh_4_finger_tip.stl │ │ │ │ │ ├── xh_base_high_poly.stl │ │ │ │ │ ├── xh_base_hull.stl │ │ │ │ │ ├── xh_base_hull_top.stl │ │ │ │ │ ├── xh_circle_high_poly.stl │ │ │ │ │ ├── xh_clamping_high_poly.stl │ │ │ │ │ ├── xh_clamping_small_high_poly.stl │ │ │ │ │ ├── xh_finger_tip.stl │ │ │ │ │ └── xh_white_top_high_poly.stl │ │ │ ├── dclaw_stations │ │ │ │ ├── assets │ │ │ │ │ ├── dependencies.xml │ │ │ │ │ ├── valve3.xml │ │ │ │ │ ├── valve3_0.py │ │ │ │ │ ├── valve3_0.xml │ │ │ │ │ └── valve4.xml │ │ │ │ ├── meshes │ │ │ │ │ ├── station_metal_base.stl │ │ │ │ │ ├── station_plastic_base.stl │ │ │ │ │ ├── station_plastic_walls.stl │ │ │ │ │ ├── station_surface.stl │ │ │ │ │ ├── valve_3_high_poly.stl │ │ │ │ │ └── valve_4_high_poly.stl │ │ │ │ ├── valve3_station.xml │ │ │ │ └── valve4_station.xml │ │ │ ├── dkitty │ │ │ │ ├── assets │ │ │ │ │ ├── chainA_xh-v2.1.xml │ │ │ │ │ ├── chainB_xh-v2.1.xml │ │ │ │ │ ├── dependenciesA_xh-v2.1.xml │ │ │ │ │ └── dependenciesB_xh-v2.1.xml │ │ │ │ ├── kitty-v2.1.xml │ │ │ │ └── meshes │ │ │ │ │ ├── D_kitty_plates_V2.stl │ │ │ │ │ ├── bulkhead.stl │ │ │ │ │ ├── bulkhead_low_poly.stl │ │ │ │ │ ├── bulkhead_low_poly1.stl │ │ │ │ │ ├── bulkhead_low_poly2.stl │ │ │ │ │ ├── bulkhead_low_poly3.stl │ │ │ │ │ ├── bulkhead_low_poly4.stl │ │ │ │ │ ├── bulkhead_low_poly5.stl │ │ │ │ │ ├── fr12_h103_3.stl │ │ │ │ │ ├── screws.stl │ │ │ │ │ ├── torso.stl │ │ │ │ │ ├── x430_dkitty_2.stl │ │ │ │ │ ├── xh_clamping_small.stl │ │ │ │ │ └── xh_finger_tip.stl │ │ │ └── scenes │ │ │ │ ├── basic_scene.xml │ │ │ │ └── textures │ │ │ │ └── white_marble_tile2.png │ │ ├── robot_env.py │ │ ├── robot_env_test.py │ │ ├── scripts │ │ │ ├── __init__.py │ │ │ ├── check_mujoco_deps.py │ │ │ ├── data │ │ │ │ ├── mjrl │ │ │ │ │ ├── DClawPoseMotion-v0-policy.pkl │ │ │ │ │ ├── DKittyOrientRandomDynamics-v0-policy.pkl │ │ │ │ │ ├── DKittyStandRandomDynamics-v0-policy.pkl │ │ │ │ │ ├── DKittyWalkFixedOld-v0-policy.pkl │ │ │ │ │ └── DKittyWalkRandomDynamics-v0-policy.pkl │ │ │ │ └── softlearning │ │ │ │ │ ├── DClawScrewFixed-v0-policy.pkl │ │ │ │ │ ├── DClawScrewRandom-v0-policy.pkl │ │ │ │ │ ├── DClawTurnFixed-v0-policy.pkl │ │ │ │ │ └── DClawTurnRandom-v0-policy.pkl │ │ │ ├── enjoy_mjrl.py │ │ │ ├── enjoy_softlearning.py │ │ │ ├── find_vr_devices.py │ │ │ ├── play.py │ │ │ ├── reset_hardware.py │ │ │ ├── rollout.py │ │ │ └── utils.py │ │ ├── simulation │ │ │ ├── __init__.py │ │ │ ├── dm_renderer.py │ │ │ ├── dm_sim_scene.py │ │ │ ├── mjpy_renderer.py │ │ │ ├── mjpy_sim_scene.py │ │ │ ├── randomize.py │ │ │ ├── renderer.py │ │ │ ├── sim_scene.py │ │ │ └── sim_scene_test.py │ │ └── utils │ │ │ ├── __init__.py │ │ │ ├── configurable.py │ │ │ ├── configurable_test.py │ │ │ ├── math_utils.py │ │ │ ├── math_utils_test.py │ │ │ ├── plotting.py │ │ │ ├── registration.py │ │ │ ├── reset_procedure.py │ │ │ ├── resources.py │ │ │ ├── resources_test.py │ │ │ └── testing │ │ │ ├── __init__.py │ │ │ ├── mock_dynamixel_sdk.py │ │ │ ├── mock_dynamixel_sdk_test.py │ │ │ ├── mock_sim_scene.py │ │ │ ├── mock_sim_scene_test.py │ │ │ └── mock_time.py │ ├── setup.cfg │ └── setup.py ├── tools.py ├── vmail.py └── wrappers.py └── vmail.yaml /README.md: -------------------------------------------------------------------------------- 1 | # Visual Adversarial Imitation Learning using Variational Models (VMAIL) 2 | This is the official implementation of the NeurIPS 2021 paper. 3 | 4 | - [Project website][website] 5 | - [Research paper][paper] 6 | - [Datasets used in the paper][data] 7 | 8 | [website]: https://sites.google.com/view/variational-mail 9 | [paper]: https://arxiv.org/abs/2107.08829 10 | [data]: https://drive.google.com/drive/folders/1JZmOVmlCqScqu0DDmn7857D5FtHZr6Un 11 | 12 | 13 | ## Method 14 | 15 | ![VMAIL](/images/VMAIL.png) 16 | 17 | VMAIL simultaneously learns a variational dynamics model and trains an on-policy 18 | adversarial imitation learning algorithm in the latent space using only model-based 19 | rollouts. This allows for stable and sample efficient training, as well as zero-shot 20 | imitation learning by transfering the learned dynamics model 21 | 22 | 23 | 24 | ## Instructions 25 | 26 | Get dependencies: 27 | 28 | ``` 29 | conda env create -f vmail.yml 30 | conda activate vmail 31 | cd robel_claw/robel 32 | pip install -e . 33 | ``` 34 | 35 | To train agents for each environmnet download the expert data from the provided link and run: 36 | 37 | ``` 38 | python3 -u vmail.py --logdir .logdir --expert_datadir expert_datadir 39 | ``` 40 | 41 | The training will generate tensorabord plots and GIFs in the log folder: 42 | 43 | ``` 44 | tensorboard --logdir ./logdir 45 | ``` 46 | 47 | ## Citation 48 | 49 | If you find this code useful, please reference in your paper: 50 | 51 | ``` 52 | @article{rafailov2021visual, 53 | title={Visual Adversarial Imitation Learning using Variational Models}, 54 | author={Rafael Rafailov and Tianhe Yu and Aravind Rajeswaran and Chelsea Finn}, 55 | year={2021}, 56 | journal={Neural Information Processing Systems} 57 | } 58 | ``` -------------------------------------------------------------------------------- /images/VMAIL.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/images/VMAIL.png -------------------------------------------------------------------------------- /robel_claw/robel/CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # How to Contribute 2 | 3 | We'd love to accept your patches and contributions to this project. There are 4 | just a few small guidelines you need to follow. 5 | 6 | ## Contributor License Agreement 7 | 8 | Contributions to this project must be accompanied by a Contributor License 9 | Agreement. You (or your employer) retain the copyright to your contribution; 10 | this simply gives us permission to use and redistribute your contributions as 11 | part of the project. Head over to to see 12 | your current agreements on file or to sign a new one. 13 | 14 | You generally only need to submit a CLA once, so if you've already submitted one 15 | (even if it was for a different project), you probably don't need to do it 16 | again. 17 | 18 | ## Code reviews 19 | 20 | All submissions, including submissions by project members, require review. We 21 | use GitHub pull requests for this purpose. Consult 22 | [GitHub Help](https://help.github.com/articles/about-pull-requests/) for more 23 | information on using pull requests. 24 | 25 | ## Community Guidelines 26 | 27 | This project follows 28 | [Google's Open Source Community Guidelines](https://opensource.google.com/conduct/). 29 | -------------------------------------------------------------------------------- /robel_claw/robel/conda_envs/base.yml: -------------------------------------------------------------------------------- 1 | name: robel 2 | channels: 3 | - defaults 4 | - conda-forge 5 | dependencies: 6 | - python=3.6 7 | - pip>=19.0 8 | - pip: 9 | - gym>=0.11.0,<0.14.0 10 | - mujoco-py>2.0,<2.1 11 | - numpy>=1.13.3 12 | - transforms3d>=0.3.0<0.4 13 | - dynamixel_sdk @ https://github.com/ROBOTIS-GIT/DynamixelSDK/archive/3.7.0.zip#subdirectory=python 14 | - openvr==1.3.2201 15 | -------------------------------------------------------------------------------- /robel_claw/robel/conda_envs/mjrl.yml: -------------------------------------------------------------------------------- 1 | name: robel-mjrl 2 | channels: 3 | - pytorch 4 | - defaults 5 | dependencies: 6 | - python=3.7 7 | - pip>=19.1 8 | - pytorch-cpu=1.1.0 9 | - tabulate=0.8.3 10 | - termcolor=1.1.0 11 | - torchvision-cpu=0.3.0 12 | - pip: 13 | - click 14 | - cloudpickle 15 | - gym==0.12 16 | - ipdb 17 | - matplotlib 18 | - mujoco-py<2.1,>=2.0 19 | - pyyaml 20 | - tqdm 21 | - wheel 22 | - scipy 23 | - git+git://github.com/deepmind/dm_control.git 24 | - transforms3d==0.3.1 25 | - dynamixel_sdk @ https://github.com/ROBOTIS-GIT/DynamixelSDK/archive/3.7.0.zip#subdirectory=python 26 | - openvr==1.3.2201 27 | -------------------------------------------------------------------------------- /robel_claw/robel/conda_envs/softlearning.yml: -------------------------------------------------------------------------------- 1 | name: robel-softlearning 2 | channels: 3 | - defaults 4 | - conda-forge 5 | dependencies: 6 | - python=3.7.3 7 | - pip>=19.1 8 | - conda>=4.6 9 | - pip: 10 | - deepdiff==3.3.0 11 | - flatten-dict==0.0.3.post1 12 | - gtimer==1.0.0b5 13 | - tensorflow==1.14.0rc0 14 | - tensorflow-probability==0.7.0rc0 15 | - numpy==1.16.3 16 | - gym==0.12.1 17 | - git+https://github.com/hartikainen/serializable.git@76516385a3a716ed4a2a9ad877e2d5cbcf18d4e6 18 | - matplotlib==3.0.2 19 | - Cython==0.29.1 20 | - mujoco-py==2.0.2.2 21 | - ray[debug]==0.7.0 22 | - git+https://github.com/deepmind/dm_control.git@e681949dda5ef1b05acf4fad99a134101469d8e7 23 | - pandas==0.23.4 24 | - transforms3d==0.3.1 25 | - dynamixel_sdk @ https://github.com/ROBOTIS-GIT/DynamixelSDK/archive/3.7.0.zip#subdirectory=python 26 | - openvr==1.3.2201 27 | -------------------------------------------------------------------------------- /robel_claw/robel/media/cover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/media/cover.png -------------------------------------------------------------------------------- /robel_claw/robel/requirements.dev.txt: -------------------------------------------------------------------------------- 1 | absl-py 2 | pylint 3 | pytype 4 | yapf 5 | -------------------------------------------------------------------------------- /robel_claw/robel/requirements.hardware.txt: -------------------------------------------------------------------------------- 1 | git+https://github.com/ROBOTIS-GIT/DynamixelSDK.git#subdirectory=python 2 | openvr==1.3.2201 3 | -------------------------------------------------------------------------------- /robel_claw/robel/requirements.txt: -------------------------------------------------------------------------------- 1 | gym>=0.11.0,<0.14.0 2 | mujoco-py>2.0,<2.1 3 | numpy>=1.10.4 4 | transforms3d>=0.3.0<0.4 5 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Import all robot submodules.""" 16 | 17 | import robel.dclaw 18 | import robel.dkitty 19 | 20 | from robel.utils.configurable import set_env_params 21 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/base.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Base API for Components. 16 | 17 | A Component provides a unified API between simulation and hardware. 18 | """ 19 | 20 | import abc 21 | import logging 22 | import sys 23 | from typing import Any, Dict, Optional, Sequence, Union 24 | 25 | import numpy as np 26 | 27 | from robel.simulation.sim_scene import SimScene 28 | 29 | # Type definition for a group configuration. 30 | GroupConfig = Any # pylint: disable=invalid-name 31 | GroupState = Any # pylint: disable=invalid-name 32 | 33 | 34 | class BaseComponent(abc.ABC): 35 | """Base class for all components.""" 36 | def __init__( 37 | self, 38 | sim_scene: SimScene, 39 | groups: Dict[str, Dict], 40 | random_state: Optional[np.random.RandomState] = None, 41 | ): 42 | """Initializes a new component. 43 | 44 | Args: 45 | sim_scene: The simulation to control. 46 | groups: Group configurations for reading/writing state. 47 | random_state: A random state to use for generating noise. 48 | """ 49 | self.sim_scene = sim_scene 50 | self.random_state = random_state 51 | 52 | if self.random_state is None: 53 | logging.info( 54 | 'Random state not given; observation noise will be ignored') 55 | 56 | # Process all groups. 57 | self.groups = {} 58 | for group_name, group_config in groups.items(): 59 | try: 60 | config = self._process_group(**group_config) 61 | except Exception as e: 62 | raise type(e)('[{}] Error parsing group "{}": {}'.format( 63 | self.__class__.__name__, 64 | group_name, 65 | str(e), 66 | )).with_traceback(sys.exc_info()[2]) 67 | self.groups[group_name] = config 68 | 69 | @property 70 | def is_hardware(self) -> bool: 71 | """Returns True if this is a hardware component.""" 72 | return False 73 | 74 | def close(self): 75 | """Cleans up any resources used by the component.""" 76 | 77 | def get_state(self, groups: Union[str, Sequence[str]], 78 | **kwargs) -> Union[GroupState, Sequence[GroupState]]: 79 | """Returns the state of the given groups. 80 | 81 | Args: 82 | groups: Either a single group name or a list of group names of the 83 | groups to retrieve the state of. 84 | 85 | Returns: 86 | If `groups` is a string, returns a single state object. Otherwise, 87 | returns a list of state objects. 88 | """ 89 | if isinstance(groups, str): 90 | states = self._get_group_states([self.get_config(groups)], **kwargs) 91 | else: 92 | states = self._get_group_states( 93 | [self.get_config(name) for name in groups], **kwargs) 94 | 95 | if isinstance(groups, str): 96 | return states[0] 97 | return states 98 | 99 | def get_config(self, group_name: str) -> GroupConfig: 100 | """Returns the configuration for a group.""" 101 | if group_name not in self.groups: 102 | raise ValueError( 103 | 'Group "{}" is not in the configured groups: {}'.format( 104 | group_name, sorted(self.groups.keys()))) 105 | return self.groups[group_name] 106 | 107 | @abc.abstractmethod 108 | def _process_group(self, **config_kwargs) -> GroupConfig: 109 | """Processes the configuration for a group. 110 | 111 | This should be overridden by subclasses to define and validate the group 112 | configuration. 113 | 114 | Args: 115 | **config_kwargs: Keyword arguments from the group configuration. 116 | 117 | Returns: 118 | An object that defines the group. 119 | e.g. A class that stores the group parameters. 120 | """ 121 | 122 | @abc.abstractmethod 123 | def _get_group_states(self, configs: Sequence[GroupConfig], 124 | **kwargs) -> Sequence[GroupState]: 125 | """Returns the states for the given group configurations.""" 126 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/base_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for BaseComponent.""" 16 | 17 | from typing import Any 18 | 19 | from absl.testing import absltest 20 | 21 | from robel.components.base import BaseComponent 22 | from robel.utils.testing.mock_sim_scene import MockSimScene 23 | 24 | 25 | class DummyComponent(BaseComponent): 26 | """Mock component for testing BaseComponent.""" 27 | 28 | def __init__(self, **kwargs): 29 | sim_scene = MockSimScene(nq=1) # type: Any 30 | super().__init__(sim_scene=sim_scene, **kwargs) 31 | 32 | def _process_group(self, **config_kwargs): 33 | return {} 34 | 35 | def _get_group_states(self, configs): 36 | return [0 for group in configs] 37 | 38 | 39 | class BaseComponentTest(absltest.TestCase): 40 | """Unit test class for BaseComponent.""" 41 | 42 | def test_get_state(self): 43 | """Tests retrieving state from a single group.""" 44 | component = DummyComponent(groups={'foo': {}}) 45 | state = component.get_state('foo') 46 | self.assertEqual(state, 0) 47 | 48 | def test_get_states(self): 49 | """Tests retrieving state from multiple groups.""" 50 | component = DummyComponent(groups={'foo': {}, 'bar': {}}) 51 | foo_state, bar_state = component.get_state(['foo', 'bar']) 52 | self.assertEqual(foo_state, 0) 53 | self.assertEqual(bar_state, 0) 54 | 55 | 56 | if __name__ == '__main__': 57 | absltest.main() 58 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/builder.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Shared logic for a component builder. 16 | """ 17 | 18 | import abc 19 | from typing import List 20 | 21 | from robel.components.base import BaseComponent 22 | 23 | 24 | class ComponentBuilder(metaclass=abc.ABCMeta): 25 | """Base class for a component configuration. 26 | 27 | This wraps a dictionary of parameters that is used to initialize a 28 | Component. 29 | """ 30 | 31 | def __init__(self): 32 | self.group_configs = {} 33 | 34 | @property 35 | def group_names(self) -> List[str]: 36 | """Returns the sorted list of current group names.""" 37 | return sorted(self.group_configs.keys()) 38 | 39 | @abc.abstractmethod 40 | def build(self, *args, **kwargs) -> BaseComponent: 41 | """Builds the component.""" 42 | 43 | def add_group(self, group_name: str, **group_kwargs): 44 | """Adds a group configuration. 45 | 46 | Args: 47 | group_name: The name of the group. 48 | **group_kwargs: Key-value pairs to configure the group with. 49 | """ 50 | if group_name in self.group_configs: 51 | raise ValueError( 52 | 'Group with name `{}` already exists in component config.' 53 | .format(group_name)) 54 | self.group_configs[group_name] = group_kwargs 55 | 56 | def update_group(self, group_name: str, **group_kwargs): 57 | """Updates a group configuration. 58 | 59 | Args: 60 | group_name: The name of the group. 61 | **group_kwargs: Key-value pairs to configure the group with. 62 | """ 63 | self._check_group_exists(group_name) 64 | self.group_configs[group_name].update(group_kwargs) 65 | 66 | def _check_group_exists(self, name: str): 67 | """Raises an error if a group with the given name doesn't exist.""" 68 | if name not in self.group_configs: 69 | raise ValueError( 70 | ('No group with name "{}" was added to the builder. Currently ' 71 | 'added groups: {}').format(name, self.group_names)) 72 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/builder_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for ComponentBuilder.""" 16 | 17 | from absl.testing import absltest 18 | 19 | from robel.components.builder import ComponentBuilder 20 | 21 | 22 | class DummyBuilder(ComponentBuilder): 23 | """Mock component for testing ComponentBuilder.""" 24 | 25 | def build(self, *args, **kwargs): 26 | """Builds the component.""" 27 | 28 | 29 | class ComponentBuilderTest(absltest.TestCase): 30 | """Unit test class for ComponentBuilder.""" 31 | 32 | def test_add_group(self): 33 | """Tests adding a group.""" 34 | builder = DummyBuilder() 35 | builder.add_group('test', a=1, b=2) 36 | self.assertDictEqual(builder.group_configs, {'test': dict(a=1, b=2)}) 37 | self.assertListEqual(builder.group_names, ['test']) 38 | 39 | def test_add_multiple_groups(self): 40 | """Tests adding multiple groups.""" 41 | builder = DummyBuilder() 42 | builder.add_group('test1', a=1, b=2) 43 | builder.add_group('test2', b=2, c=3) 44 | self.assertDictEqual(builder.group_configs, { 45 | 'test1': dict(a=1, b=2), 46 | 'test2': dict(b=2, c=3), 47 | }) 48 | self.assertListEqual(builder.group_names, ['test1', 'test2']) 49 | 50 | def test_add_group_conflict(self): 51 | """Tests adding a duplicate group.""" 52 | builder = DummyBuilder() 53 | builder.add_group('test') 54 | with self.assertRaises(ValueError): 55 | builder.add_group('test') 56 | self.assertListEqual(builder.group_names, ['test']) 57 | 58 | def test_update_group(self): 59 | """Tests updating an existing group.""" 60 | builder = DummyBuilder() 61 | builder.add_group('test', a=1, b=2) 62 | builder.update_group('test', b=3, c=4) 63 | self.assertDictEqual(builder.group_configs, 64 | {'test': dict(a=1, b=3, c=4)}) 65 | 66 | def test_update_nonexistent_group(self): 67 | """Tests updating an non-existing group.""" 68 | builder = DummyBuilder() 69 | builder.add_group('test', a=1, b=2) 70 | with self.assertRaises(ValueError): 71 | builder.update_group('nottest', b=3, c=4) 72 | self.assertDictEqual(builder.group_configs, {'test': dict(a=1, b=2)}) 73 | 74 | 75 | if __name__ == '__main__': 76 | absltest.main() 77 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/robot/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Robot component module.""" 16 | 17 | from .robot import ControlMode, RobotState 18 | from .builder import RobotComponentBuilder 19 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/robot/builder.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Builder-specific logic for creating RobotComponents.""" 16 | 17 | from robel.components.builder import ComponentBuilder 18 | from robel.components.robot.dynamixel_utils import CalibrationMap 19 | 20 | 21 | class RobotComponentBuilder(ComponentBuilder): 22 | """Builds a RobotComponent.""" 23 | 24 | def __init__(self): 25 | super().__init__() 26 | self._dxl_device_path = None 27 | self._calibration_map = None 28 | 29 | def build(self, *args, **kwargs): 30 | """Builds the component.""" 31 | if self._dxl_device_path: 32 | if self._calibration_map is not None: 33 | self._calibration_map.update_group_configs(self.group_configs) 34 | from robel.components.robot.dynamixel_robot import ( 35 | DynamixelRobotComponent) 36 | return DynamixelRobotComponent( 37 | *args, 38 | groups=self.group_configs, 39 | device_path=self._dxl_device_path, 40 | **kwargs) 41 | from robel.components.robot.robot import RobotComponent 42 | return RobotComponent(*args, groups=self.group_configs, **kwargs) 43 | 44 | def set_dynamixel_device_path(self, device_path: str): 45 | """Sets the device path for a hardware robot component. 46 | 47 | If set, the builder will build a DynamixelRobotComponent. 48 | 49 | Args: 50 | device_path: The device path to the Dynamixel device. 51 | """ 52 | self._dxl_device_path = device_path 53 | 54 | def set_hardware_calibration_map(self, calibration_map: CalibrationMap): 55 | """Sets the calibration map for hardware.""" 56 | self._calibration_map = calibration_map 57 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/robot/dynamixel_client_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for DynamixelClient.""" 16 | 17 | from absl.testing import absltest 18 | 19 | from robel.components.robot.dynamixel_client import DynamixelClient 20 | from robel.utils.testing.mock_dynamixel_sdk import patch_dynamixel 21 | 22 | 23 | class DynamixelClientTest(absltest.TestCase): 24 | """Unit test class for DynamixelClient.""" 25 | 26 | @patch_dynamixel(test=[1, 2, 3, 4]) 27 | def test_connect(self, sdk): 28 | client = DynamixelClient([1, 2, 3, 4], port='test') 29 | self.assertFalse(client.is_connected) 30 | 31 | client.connect() 32 | self.assertIn('test', sdk.used_ports) 33 | self.assertListEqual(sdk.get_enabled_motors('test'), [1, 2, 3, 4]) 34 | client.disconnect() 35 | 36 | @patch_dynamixel(test=[1, 2, 3, 4]) 37 | def test_torque_enabled(self, sdk): 38 | client = DynamixelClient([1, 2, 3, 4], port='test') 39 | client.connect() 40 | self.assertListEqual(sdk.get_enabled_motors('test'), [1, 2, 3, 4]) 41 | 42 | client.set_torque_enabled([1, 3], False) 43 | self.assertListEqual(sdk.get_enabled_motors('test'), [2, 4]) 44 | 45 | client.set_torque_enabled([1, 2], True) 46 | self.assertListEqual(sdk.get_enabled_motors('test'), [1, 2, 4]) 47 | 48 | client.disconnect() 49 | self.assertListEqual(sdk.get_enabled_motors('test'), []) 50 | 51 | 52 | if __name__ == '__main__': 53 | absltest.main() 54 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/robot/dynamixel_utils.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Helper classes and methods for working with Dynamixel robots.""" 16 | 17 | from typing import Any, Dict, Iterable, Tuple 18 | 19 | 20 | class CalibrationMap: 21 | """Mapping of motor ID to (scale, offset) tuples.""" 22 | 23 | def __init__(self, mapping: Dict[int, Tuple[float, float]]): 24 | """Initializes a new calibration mapping.""" 25 | self.mapping = mapping 26 | 27 | def get_parameters(self, motor_ids: Iterable[int]) -> Dict[str, Any]: 28 | """Returns a dictionary of calibration parameters. 29 | 30 | Args: 31 | motor_ids: The motor IDs to get calibration parameters for. 32 | """ 33 | return { 34 | 'calib_scale': [self.mapping[i][0] for i in motor_ids], 35 | 'calib_offset': [self.mapping[i][1] for i in motor_ids], 36 | } 37 | 38 | def make_group_config(self, motor_ids: Iterable[int], 39 | **group_kwargs) -> Dict[str, Any]: 40 | """Creates a Dynamixel group configuration for the given motor IDs. 41 | 42 | Args: 43 | motor_ids: The motor IDs to create a group for. 44 | **group_kwargs: Additional configuration to set for the group. 45 | """ 46 | return { 47 | 'motor_ids': motor_ids, 48 | **self.get_parameters(motor_ids), 49 | **group_kwargs 50 | } 51 | 52 | def update_group_configs(self, configs: Dict[str, Any]): 53 | """Updates the calibration values for groups in the configuration. 54 | 55 | Args: 56 | config: The component configuration to update. 57 | *group_names: One or more group names to update. 58 | """ 59 | for group_config in configs.values(): 60 | if 'motor_ids' not in group_config: 61 | continue 62 | group_config.update(self.get_parameters(group_config['motor_ids'])) 63 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/robot/hardware_robot_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for RobotComponent and RobotGroupConfig.""" 16 | 17 | from typing import Any 18 | 19 | from absl.testing import absltest 20 | import numpy as np 21 | 22 | from robel.components.robot.hardware_robot import (HardwareRobotComponent, 23 | RobotState) 24 | from robel.utils.testing.mock_sim_scene import MockSimScene 25 | from robel.utils.testing.mock_time import patch_time 26 | 27 | 28 | class DummyHardwareRobotComponent(HardwareRobotComponent): 29 | """Test implementation of HardwareRobotComponent.""" 30 | 31 | def calibrate_state(self, state: RobotState, group_name: str): 32 | self._calibrate_state(state, self.get_config(group_name)) 33 | 34 | def decalibrate_qpos(self, qpos: np.ndarray, group_name: str): 35 | return self._decalibrate_qpos(qpos, self.get_config(group_name)) 36 | 37 | def do_timestep(self): 38 | self._synchronize_timestep() 39 | 40 | 41 | class HardwareRobotComponentTest(absltest.TestCase): 42 | """Unit test class for HardwareRobotComponent.""" 43 | 44 | def test_calibrate_state(self): 45 | """Converts a state to component space.""" 46 | sim_scene = MockSimScene(nq=1) # type: Any 47 | robot = DummyHardwareRobotComponent( 48 | sim_scene, 49 | groups={ 50 | 'a': { 51 | 'calib_scale': [0.5, -1, 1], 52 | 'calib_offset': [2, 0, -0.5], 53 | } 54 | }) 55 | state = RobotState( 56 | qpos=np.array([1., 1., 1.]), qvel=np.array([1., 1., 1.])) 57 | robot.calibrate_state(state, 'a') 58 | np.testing.assert_allclose(state.qpos, [2.5, -1, 0.5]) 59 | np.testing.assert_allclose(state.qvel, [0.5, -1, 1]) 60 | 61 | def test_decalibrate_qpos(self): 62 | """Converts a component state qpos to hardware space.""" 63 | sim_scene = MockSimScene(nq=1) # type: Any 64 | robot = DummyHardwareRobotComponent( 65 | sim_scene, 66 | groups={ 67 | 'a': { 68 | 'calib_scale': [0.5, -1, 1], 69 | 'calib_offset': [2, 0, -0.5], 70 | } 71 | }) 72 | qpos = robot.decalibrate_qpos(np.array([1., 2., 3.]), 'a') 73 | np.testing.assert_allclose(qpos, [-2, -2, 3.5]) 74 | 75 | def test_timestep(self): 76 | """Tests advancement of time when doing timesteps.""" 77 | with patch_time( 78 | 'robel.components.robot.hardware_robot.time', 79 | initial_time=0) as mock_time: 80 | sim_scene = MockSimScene(nq=1, step_duration=0.5) # type: Any 81 | robot = DummyHardwareRobotComponent(sim_scene, groups={}) 82 | 83 | self.assertEqual(robot.time, 0) 84 | robot.do_timestep() 85 | self.assertAlmostEqual(robot.time, 0.5) 86 | mock_time.sleep(0.25) 87 | robot.do_timestep() 88 | self.assertAlmostEqual(robot.time, 1.0) 89 | mock_time.sleep(0.6) 90 | robot.do_timestep() 91 | self.assertAlmostEqual(robot.time, 1.6) 92 | robot.reset_time() 93 | self.assertEqual(robot.time, 0) 94 | 95 | 96 | if __name__ == '__main__': 97 | absltest.main() 98 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/robot/robot_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for RobotComponent and RobotGroupConfig.""" 16 | 17 | from typing import Any 18 | 19 | from absl.testing import absltest 20 | import numpy as np 21 | 22 | from robel.components.robot.robot import ControlMode, RobotComponent 23 | from robel.utils.testing.mock_sim_scene import MockSimScene 24 | 25 | 26 | class RobotComponentTest(absltest.TestCase): 27 | """Unit test class for RobotComponent.""" 28 | 29 | def test_get_state(self): 30 | """Tests querying the state of multiple groups.""" 31 | sim_scene = MockSimScene(nq=10) # type: Any 32 | sim_scene.data.qpos[:] = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10] 33 | sim_scene.data.qvel[:] = [11, 12, 13, 14, 15, 16, 17, 18, 19, 20] 34 | robot = RobotComponent( 35 | sim_scene, 36 | groups={ 37 | 'a': { 38 | 'qpos_indices': [0, 1, 3, 5], 39 | }, 40 | 'b': { 41 | 'qpos_indices': [2, 6], 42 | 'qvel_indices': [7, 8, 9], 43 | }, 44 | }) 45 | a_state, b_state = robot.get_state(['a', 'b']) 46 | np.testing.assert_array_equal(a_state.qpos, [1, 2, 4, 6]) 47 | np.testing.assert_array_equal(a_state.qvel, [11, 12, 14, 16]) 48 | np.testing.assert_array_equal(b_state.qpos, [3, 7]) 49 | np.testing.assert_array_equal(b_state.qvel, [18, 19, 20]) 50 | 51 | def test_step(self): 52 | """Tests stepping with an action for multiple groups.""" 53 | sim_scene = MockSimScene(nq=10, ctrl_range=[-1, 1]) # type: Any 54 | robot = RobotComponent( 55 | sim_scene, 56 | groups={ 57 | 'a': { 58 | 'qpos_indices': [0, 1, 3, 5], 59 | }, 60 | 'b': { 61 | 'actuator_indices': [7, 8, 9], 62 | }, 63 | }) 64 | robot.step({ 65 | 'a': np.array([.2, .4, .6, .8]), 66 | 'b': np.array([.1, .3, .5]) 67 | }) 68 | np.testing.assert_allclose(sim_scene.data.ctrl, 69 | [.2, .4, 0, .6, 0, .8, 0, .1, .3, .5]) 70 | 71 | self.assertEqual(sim_scene.advance.call_count, 1) 72 | 73 | def test_step_denormalize(self): 74 | """Tests denormalizing the actions to the sim control range.""" 75 | sim_scene = MockSimScene(nq=5, ctrl_range=[0, 10]) # type: Any 76 | robot = RobotComponent( 77 | sim_scene, groups={'a': { 78 | 'qpos_indices': [0, 1, 2, 3, 4], 79 | }}) 80 | robot.step({ 81 | 'a': np.array([-1, 1, -0.5, 0.5, 0]), 82 | }) 83 | np.testing.assert_allclose(sim_scene.data.ctrl, [0, 10, 2.5, 7.5, 5]) 84 | 85 | def test_step_position_control_bounds(self): 86 | """Tests action clamping when doing position control.""" 87 | sim_scene = MockSimScene(nq=5, ctrl_range=[-1, 1]) # type: Any 88 | sim_scene.data.qpos[:] = [-0.4, -0.2, 0, 0.2, 0.4] 89 | robot = RobotComponent( 90 | sim_scene, 91 | groups={ 92 | 'a': { 93 | 'qpos_indices': [0, 1, 2, 3, 4], 94 | 'qpos_range': [(-0.5, 0.5)] * 5, 95 | 'qvel_range': [(-0.2, 0.2)] * 5, 96 | } 97 | }) 98 | robot.step({'a': np.array([-1, -1, 0.2, 1, 1])}) 99 | np.testing.assert_allclose(sim_scene.data.ctrl, 100 | [-0.5, -0.4, 0.1, 0.4, 0.5]) 101 | 102 | def test_step_velocity_control_bounds(self): 103 | """Tests action clamping when doing velocity control.""" 104 | sim_scene = MockSimScene(nq=3, ctrl_range=[-10, 10]) # type: Any 105 | robot = RobotComponent( 106 | sim_scene, 107 | groups={ 108 | 'a': { 109 | 'control_mode': ControlMode.JOINT_VELOCITY, 110 | 'qpos_indices': [0, 1, 2], 111 | 'qvel_range': [(-2, 2), (-1, 5), (-7, -4)], 112 | } 113 | }) 114 | robot.step({'a': np.array([-0.5, 1, -1])}) 115 | np.testing.assert_allclose(sim_scene.data.ctrl, [-1, 5, -7]) 116 | 117 | 118 | if __name__ == '__main__': 119 | absltest.main() 120 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/tracking/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Tracking component module.""" 16 | 17 | from .tracker import TrackerState 18 | from .builder import TrackerComponentBuilder, TrackerType 19 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/tracking/phasespace_tracker.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Implementation of TrackerComponent using OpenVR to track devices.""" 16 | 17 | import time 18 | 19 | import numpy as np 20 | from transforms3d.quaternions import quat2mat 21 | 22 | from robel.components.tracking.hardware_tracker import ( 23 | DeviceId, HardwareTrackerComponent, HardwareTrackerGroupConfig, 24 | TrackerState) 25 | 26 | # Seconds to sleep to ensure PhaseSpace can start getting data. 27 | PHASESPACE_INIT_TIME = 4 28 | 29 | 30 | class PhaseSpaceTrackerGroupConfig(HardwareTrackerGroupConfig): 31 | """Stores group configuration for a PhaseSpaceTrackerComponent.""" 32 | 33 | 34 | class PhaseSpaceTrackerComponent(HardwareTrackerComponent): 35 | """Component for reading tracking data from PhaseSpace.""" 36 | 37 | # Cached client that is shared for the application lifetime. 38 | _PS_CLIENT = None 39 | 40 | def __init__(self, *args, server_address: str, **kwargs): 41 | """Initializes a VrTrackerComponent.""" 42 | super().__init__(*args, **kwargs) 43 | if self._PS_CLIENT is None: 44 | import phasespace 45 | print('Connecting to PhaseSpace at: {}'.format(server_address)) 46 | self._PS_CLIENT = phasespace.PhaseSpaceClient(server_address) 47 | print('Connected! Waiting for initialization...') 48 | time.sleep(PHASESPACE_INIT_TIME) 49 | self._position_scale = 1e-3 50 | self._poses = None 51 | self._state_cache = {} 52 | 53 | def _process_group(self, **config_kwargs): 54 | """Processes the configuration for a group.""" 55 | return PhaseSpaceTrackerGroupConfig(self.sim_scene, **config_kwargs) 56 | 57 | def _refresh_poses(self): 58 | """Refreshes the pose state.""" 59 | self._poses = self._PS_CLIENT.get_state() 60 | 61 | def _get_raw_tracker_state(self, device_id: DeviceId): 62 | """Returns the tracker state.""" 63 | try: 64 | rigid_data = self._poses.get_rigid(device_id) 65 | state = TrackerState( 66 | pos=rigid_data.position * self._position_scale, 67 | rot=quat2mat(rigid_data.rotation), 68 | vel=np.zeros(3), 69 | angular_vel=np.zeros(3)) 70 | self._state_cache[device_id] = state 71 | except: 72 | if device_id not in self._state_cache: 73 | raise 74 | state = self._state_cache[device_id] 75 | return state 76 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/tracking/utils/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/tracking/virtual_reality/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/tracking/virtual_reality/client.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Client to communicate with a VR device using OpenVR. 16 | 17 | Example usage: 18 | >>> client = VrClient() 19 | >>> client.set_devices({'tracker': 1}) 20 | """ 21 | 22 | from typing import List, Union 23 | 24 | import openvr 25 | 26 | from robel.components.tracking.virtual_reality.device import VrDevice 27 | from robel.components.tracking.virtual_reality.poses import VrPoseBatch 28 | 29 | 30 | class VrClient: 31 | """Communicates with a VR device.""" 32 | 33 | def __init__(self): 34 | self._vr_system = None 35 | self._devices = [] 36 | self._device_serial_lookup = {} 37 | self._device_index_lookup = {} 38 | self._last_pose_batch = None 39 | self._plot = None 40 | 41 | # Attempt to start OpenVR. 42 | if not openvr.isRuntimeInstalled(): 43 | raise OSError('OpenVR runtime not installed.') 44 | 45 | self._vr_system = openvr.init(openvr.VRApplication_Other) 46 | 47 | def close(self): 48 | """Cleans up any resources used by the client.""" 49 | if self._vr_system is not None: 50 | openvr.shutdown() 51 | self._vr_system = None 52 | 53 | def get_device(self, identifier: Union[int, str]) -> VrDevice: 54 | """Returns the device with the given name.""" 55 | identifier = str(identifier) 56 | if identifier in self._device_index_lookup: 57 | return self._device_index_lookup[identifier] 58 | if identifier in self._device_serial_lookup: 59 | return self._device_serial_lookup[identifier] 60 | 61 | self.discover_devices() 62 | if (identifier not in self._device_index_lookup 63 | and identifier not in self._device_serial_lookup): 64 | raise ValueError( 65 | 'Could not find device with name or index: {} (Available: {})' 66 | .format(identifier, sorted(self._device_serial_lookup.keys()))) 67 | 68 | if identifier in self._device_index_lookup: 69 | return self._device_index_lookup[identifier] 70 | return self._device_serial_lookup[identifier] 71 | 72 | def discover_devices(self) -> List[VrDevice]: 73 | """Returns and caches all connected devices.""" 74 | self._device_index_lookup.clear() 75 | self._device_serial_lookup.clear() 76 | devices = [] 77 | for device_index in range(openvr.k_unMaxTrackedDeviceCount): 78 | device = VrDevice(self._vr_system, device_index) 79 | if not device.is_connected(): 80 | continue 81 | devices.append(device) 82 | self._device_index_lookup[str(device.index)] = device 83 | self._device_serial_lookup[device.get_serial()] = device 84 | self._devices = devices 85 | return devices 86 | 87 | def get_poses(self, time_from_now: float = 0.0, 88 | update_plot: bool = True) -> VrPoseBatch: 89 | """Returns a batch of poses that can be queried per device. 90 | 91 | Args: 92 | time_from_now: The seconds into the future to read poses. 93 | update_plot: If True, updates an existing plot. 94 | """ 95 | pose_batch = VrPoseBatch(self._vr_system, time_from_now) 96 | self._last_pose_batch = pose_batch 97 | if update_plot and self._plot and self._plot.is_open: 98 | self._plot.refresh() 99 | return pose_batch 100 | 101 | def __enter__(self): 102 | """Enables use as a context manager.""" 103 | return self 104 | 105 | def __exit__(self, *args): 106 | """Enables use as a context manager.""" 107 | self.close() 108 | 109 | def __del__(self): 110 | """Automatically disconnect on destruction.""" 111 | self.close() 112 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/tracking/virtual_reality/device.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Device-related logic from OpenVR devices.""" 16 | 17 | import openvr 18 | 19 | 20 | class VrDevice: 21 | """Represents an OpenVR device.""" 22 | 23 | def __init__(self, vr_system, index: int): 24 | """Initializes a new VrDevice with the given device index.""" 25 | self._vr_system = vr_system 26 | self._index = index 27 | self._value_cache = {} 28 | 29 | @property 30 | def index(self) -> int: 31 | """Returns the device index.""" 32 | return self._index 33 | 34 | def is_connected(self) -> bool: 35 | """Returns whether the device is connected.""" 36 | return self._vr_system.isTrackedDeviceConnected(self._index) 37 | 38 | def get_serial(self) -> str: 39 | """Returns the serial number of the device.""" 40 | return self._get_string(openvr.Prop_SerialNumber_String) 41 | 42 | def get_model(self) -> str: 43 | """Returns the model number of the device.""" 44 | return self._get_string(openvr.Prop_ModelNumber_String) 45 | 46 | def get_model_name(self) -> str: 47 | """Returns the model name of the device.""" 48 | return self._get_string(openvr.Prop_RenderModelName_String) 49 | 50 | def get_summary(self) -> str: 51 | """Returns a summary of information about the device.""" 52 | connected = self.is_connected() 53 | info = '[{} - {}]'.format(self._index, 54 | 'Connected' if connected else 'Disconnected') 55 | if connected: 56 | info += ' {} ({})'.format(self.get_model_name(), self.get_serial()) 57 | return info 58 | 59 | def _get_string(self, prop_type) -> str: 60 | """Returns a string property of the device.""" 61 | if prop_type in self._value_cache: 62 | return self._value_cache[prop_type] 63 | value = self._vr_system.getStringTrackedDeviceProperty( 64 | self._index, prop_type).decode('utf-8') 65 | self._value_cache[prop_type] = value 66 | return value 67 | 68 | def _get_bool(self, prop_type) -> bool: 69 | """Returns a boolean property of the device.""" 70 | if prop_type in self._value_cache: 71 | return self._value_cache[prop_type] 72 | value = self._vr_system.getBoolTrackedDeviceProperty( 73 | self._index, prop_type)[0] 74 | self._value_cache[prop_type] = value 75 | return value 76 | 77 | def _get_float(self, prop_type) -> float: 78 | """Returns a float property of the device.""" 79 | if prop_type in self._value_cache: 80 | return self._value_cache[prop_type] 81 | value = self._vr_system.getFloatTrackedDeviceProperty( 82 | self._index, prop_type)[0] 83 | self._value_cache[prop_type] = value 84 | return value 85 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/tracking/virtual_reality/poses.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Pose-related logic for OpenVR devices.""" 16 | 17 | import numpy as np 18 | import openvr 19 | 20 | from robel.components.tracking.tracker import TrackerState 21 | from robel.components.tracking.virtual_reality.device import VrDevice 22 | 23 | 24 | class VrPoseBatch: 25 | """Represents a batch of poses calculated by the OpenVR system.""" 26 | 27 | def __init__(self, vr_system, time_from_now: float = 0.0): 28 | """Initializes a new pose batch.""" 29 | self._vr_system = vr_system 30 | # Query poses for all devices. 31 | self.poses = self._vr_system.getDeviceToAbsoluteTrackingPose( 32 | openvr.TrackingUniverseStanding, time_from_now, 33 | openvr.k_unMaxTrackedDeviceCount) 34 | 35 | def get_state(self, device: VrDevice) -> TrackerState: 36 | """Returns the tracking state for the given device.""" 37 | if not self.poses[device.index].bPoseIsValid: 38 | state = TrackerState() 39 | else: 40 | vr_pose = np.ctypeslib.as_array( 41 | self.poses[device.index].mDeviceToAbsoluteTracking[:], 42 | shape=(3, 4)) 43 | # Check that the pose is valid. 44 | # If all of the translations are 0, get from the cache. 45 | assert vr_pose.shape == (3, 4) 46 | state = TrackerState(pos=vr_pose[:, 3], rot=vr_pose[:, :3]) 47 | return state 48 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/components/tracking/vr_tracker.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Implementation of TrackerComponent using OpenVR to track devices.""" 16 | 17 | import numpy as np 18 | 19 | from robel.components.tracking.hardware_tracker import ( 20 | DeviceId, HardwareTrackerComponent, HardwareTrackerGroupConfig, 21 | TrackerState) 22 | 23 | 24 | class VrTrackerGroupConfig(HardwareTrackerGroupConfig): 25 | """Stores group configuration for a VrTrackerComponent.""" 26 | 27 | 28 | class VrTrackerComponent(HardwareTrackerComponent): 29 | """Component for reading tracking data from a HTC Vive.""" 30 | 31 | # Cached VR client that is shared for the application lifetime. 32 | _VR_CLIENT = None 33 | 34 | # Transform from OpenVR space (y upwards) to simulation space (z upwards). 35 | GLOBAL_TRANSFORM = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]], 36 | dtype=np.float32) 37 | 38 | def __init__(self, *args, **kwargs): 39 | """Initializes a VrTrackerComponent.""" 40 | super().__init__(*args, **kwargs) 41 | self._coord_system.set_coordinate_transform(self.GLOBAL_TRANSFORM) 42 | self._poses = None 43 | 44 | if self._VR_CLIENT is None: 45 | from robel.components.tracking.virtual_reality.client import ( 46 | VrClient) 47 | self._VR_CLIENT = VrClient() 48 | 49 | # Check that all devices exist. 50 | for name, group in self.groups.items(): 51 | if group.device_identifier is None: 52 | continue 53 | device = self._VR_CLIENT.get_device(group.device_identifier) 54 | print('Assigning group "{}" with device: {}'.format( 55 | name, device.get_summary())) 56 | 57 | def _process_group(self, **config_kwargs): 58 | """Processes the configuration for a group.""" 59 | return VrTrackerGroupConfig(self.sim_scene, **config_kwargs) 60 | 61 | def _refresh_poses(self): 62 | """Refreshes the pose state.""" 63 | self._poses = self._VR_CLIENT.get_poses() 64 | 65 | def _get_raw_tracker_state(self, device_id: DeviceId) -> TrackerState: 66 | """Returns the tracker state.""" 67 | device = self._VR_CLIENT.get_device(device_id) 68 | return self._poses.get_state(device) 69 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dclaw/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Gym environment registration for DClaw environments.""" 16 | 17 | from robel.utils.registration import register 18 | 19 | #=============================================================================== 20 | # Pose tasks 21 | #=============================================================================== 22 | 23 | # Default number of steps per episode. 24 | _POSE_EPISODE_LEN = 80 # 80*20*2.5ms = 4s 25 | 26 | register( 27 | env_id='DClawPoseFixed-v0', 28 | class_path='robel.dclaw.pose:DClawPoseFixed', 29 | max_episode_steps=_POSE_EPISODE_LEN) 30 | 31 | register( 32 | env_id='DClawPoseRandom-v0', 33 | class_path='robel.dclaw.pose:DClawPoseRandom', 34 | max_episode_steps=_POSE_EPISODE_LEN) 35 | 36 | register( 37 | env_id='DClawPoseRandomDynamics-v0', 38 | class_path='robel.dclaw.pose:DClawPoseRandomDynamics', 39 | max_episode_steps=_POSE_EPISODE_LEN) 40 | 41 | #=============================================================================== 42 | # Turn tasks 43 | #=============================================================================== 44 | 45 | # Default number of steps per episode. 46 | _TURN_EPISODE_LEN = 40 # 40*40*2.5ms = 4s 47 | 48 | register( 49 | env_id='DClawTurnFixed-v0', 50 | class_path='robel.dclaw.turn:DClawTurnFixed', 51 | max_episode_steps=_TURN_EPISODE_LEN) 52 | 53 | register( 54 | env_id='DClawTurnRandom-v0', 55 | class_path='robel.dclaw.turn:DClawTurnRandom', 56 | max_episode_steps=_TURN_EPISODE_LEN) 57 | 58 | register( 59 | env_id='DClawTurnRandomDynamics-v0', 60 | class_path='robel.dclaw.turn:DClawTurnRandomDynamics', 61 | max_episode_steps=_TURN_EPISODE_LEN) 62 | 63 | 64 | #=============================================================================== 65 | # Screw tasks 66 | #=============================================================================== 67 | 68 | # Default number of steps per episode. 69 | _SCREW_EPISODE_LEN = 80 # 80*40*2.5ms = 8s 70 | 71 | register( 72 | env_id='DClawScrewFixed-v0', 73 | class_path='robel.dclaw.screw:DClawScrewFixed', 74 | max_episode_steps=_SCREW_EPISODE_LEN) 75 | 76 | 77 | 78 | register( 79 | env_id='DClawScrewVel-v0', 80 | class_path='robel.dclaw.screw:DClawScrewVelv0', 81 | max_episode_steps=_SCREW_EPISODE_LEN) 82 | 83 | register( 84 | env_id='DClawScrewVel-v1', 85 | class_path='robel.dclaw.screw:DClawScrewVelv1', 86 | max_episode_steps=_SCREW_EPISODE_LEN) 87 | 88 | 89 | 90 | register( 91 | env_id='DClawScrewVelp3-v0', 92 | class_path='robel.dclaw.screw:DClawScrewVelp3v0', 93 | max_episode_steps=_SCREW_EPISODE_LEN) 94 | 95 | register( 96 | env_id='DClawScrewVelp3-v1', 97 | class_path='robel.dclaw.screw:DClawScrewVelp3v1', 98 | max_episode_steps=_SCREW_EPISODE_LEN) 99 | 100 | register( 101 | env_id='DClawScrewVelp4-v0', 102 | class_path='robel.dclaw.screw:DClawScrewVelp4v0', 103 | max_episode_steps=_SCREW_EPISODE_LEN) 104 | 105 | register( 106 | env_id='DClawScrewVelp4-v1', 107 | class_path='robel.dclaw.screw:DClawScrewVelp4v1', 108 | max_episode_steps=_SCREW_EPISODE_LEN) 109 | 110 | 111 | register( 112 | env_id='DClawScrewRandom-v0', 113 | class_path='robel.dclaw.screw:DClawScrewRandom', 114 | max_episode_steps=_SCREW_EPISODE_LEN) 115 | 116 | register( 117 | env_id='DClawScrewRandomDynamics-v0', 118 | class_path='robel.dclaw.screw:DClawScrewRandomDynamics', 119 | max_episode_steps=_SCREW_EPISODE_LEN) 120 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dclaw/assets/dclaw3xh_valve3_v0.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dclaw/assets/dclaw3xh_valve3_v1.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dclaw/assets/dclaw3xh_valve4_v0.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dclaw/assets/dclaw3xh_valve4_v1.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dclaw/pose_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for D'Claw pose tasks.""" 16 | 17 | from absl.testing import absltest 18 | from absl.testing import parameterized 19 | import gym 20 | import numpy as np 21 | 22 | from robel.dclaw.pose import (DClawPoseFixed, DClawPoseRandom, 23 | DClawPoseRandomDynamics) 24 | # pylint: disable=no-member 25 | 26 | 27 | @parameterized.parameters( 28 | ('DClawPoseFixed-v0', DClawPoseFixed), 29 | ('DClawPoseRandom-v0', DClawPoseRandom), 30 | ('DClawPoseRandomDynamics-v0', DClawPoseRandomDynamics), 31 | ) 32 | class DClawPoseTest(absltest.TestCase): 33 | """Unit test class for RobotEnv.""" 34 | 35 | def test_gym_make(self, env_id, env_cls): 36 | """Accesses the sim, model, and data properties.""" 37 | env = gym.make(env_id) 38 | self.assertIsInstance(env.unwrapped, env_cls) 39 | 40 | def test_spaces(self, _, env_cls): 41 | """Checks the observation, action, and state spaces.""" 42 | env = env_cls() 43 | observation_size = np.sum([ 44 | 9, # qpos 45 | 9, # last_action 46 | 9, # qpos_error 47 | ]) 48 | self.assertEqual(env.observation_space.shape, (observation_size,)) 49 | self.assertEqual(env.action_space.shape, (9,)) 50 | self.assertEqual(env.state_space['qpos'].shape, (9,)) 51 | self.assertEqual(env.state_space['qvel'].shape, (9,)) 52 | 53 | def test_reset_step(self, _, env_cls): 54 | """Checks that resetting and stepping works.""" 55 | env = env_cls() 56 | env.reset() 57 | env.step(env.action_space.sample()) 58 | 59 | 60 | if __name__ == '__main__': 61 | absltest.main() 62 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dclaw/screw_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for D'Claw screw tasks.""" 16 | 17 | from absl.testing import absltest 18 | from absl.testing import parameterized 19 | import gym 20 | import numpy as np 21 | 22 | from robel.dclaw.screw import (DClawScrewFixed, DClawScrewRandom, 23 | DClawScrewRandomDynamics) 24 | # pylint: disable=no-member 25 | 26 | 27 | @parameterized.parameters( 28 | ('DClawScrewFixed-v0', DClawScrewFixed), 29 | ('DClawScrewRandom-v0', DClawScrewRandom), 30 | ('DClawScrewRandomDynamics-v0', DClawScrewRandomDynamics), 31 | ) 32 | class DClawScrewTest(absltest.TestCase): 33 | """Unit test class for RobotEnv.""" 34 | 35 | def test_gym_make(self, env_id, env_cls): 36 | """Accesses the sim, model, and data properties.""" 37 | env = gym.make(env_id) 38 | self.assertIsInstance(env.unwrapped, env_cls) 39 | 40 | def test_spaces(self, _, env_cls): 41 | """Checks the observation, action, and state spaces.""" 42 | env = env_cls() 43 | observation_size = np.sum([ 44 | 9, # claw_qpos 45 | 1, # object_x 46 | 1, # object_y 47 | 9, # last_action 48 | 1, # target_error 49 | ]) 50 | self.assertEqual(env.observation_space.shape, (observation_size,)) 51 | self.assertEqual(env.action_space.shape, (9,)) 52 | self.assertEqual(env.state_space['claw_qpos'].shape, (9,)) 53 | self.assertEqual(env.state_space['claw_qvel'].shape, (9,)) 54 | self.assertEqual(env.state_space['object_qpos'].shape, (1,)) 55 | self.assertEqual(env.state_space['object_qvel'].shape, (1,)) 56 | 57 | def test_reset_step(self, _, env_cls): 58 | """Checks that resetting and stepping works.""" 59 | env = env_cls() 60 | env.reset() 61 | env.step(env.action_space.sample()) 62 | 63 | 64 | if __name__ == '__main__': 65 | absltest.main() 66 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dclaw/turn_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for D'Claw turn tasks.""" 16 | 17 | from absl.testing import absltest 18 | from absl.testing import parameterized 19 | import gym 20 | import numpy as np 21 | 22 | from robel.dclaw.turn import (DClawTurnFixed, DClawTurnRandom, 23 | DClawTurnRandomDynamics) 24 | # pylint: disable=no-member 25 | 26 | 27 | @parameterized.parameters( 28 | ('DClawTurnFixed-v0', DClawTurnFixed), 29 | ('DClawTurnRandom-v0', DClawTurnRandom), 30 | ('DClawTurnRandomDynamics-v0', DClawTurnRandomDynamics), 31 | ) 32 | class DClawTurnTest(absltest.TestCase): 33 | """Unit test class for RobotEnv.""" 34 | 35 | def test_gym_make(self, env_id, env_cls): 36 | """Accesses the sim, model, and data properties.""" 37 | env = gym.make(env_id) 38 | self.assertIsInstance(env.unwrapped, env_cls) 39 | 40 | def test_spaces(self, _, env_cls): 41 | """Checks the observation, action, and state spaces.""" 42 | env = env_cls() 43 | observation_size = np.sum([ 44 | 9, # claw_qpos 45 | 1, # object_x 46 | 1, # object_y 47 | 9, # last_action 48 | 1, # target_error 49 | ]) 50 | self.assertEqual(env.observation_space.shape, (observation_size,)) 51 | self.assertEqual(env.action_space.shape, (9,)) 52 | self.assertEqual(env.state_space['claw_qpos'].shape, (9,)) 53 | self.assertEqual(env.state_space['claw_qvel'].shape, (9,)) 54 | self.assertEqual(env.state_space['object_qpos'].shape, (1,)) 55 | self.assertEqual(env.state_space['object_qvel'].shape, (1,)) 56 | 57 | def test_reset_step(self, _, env_cls): 58 | """Checks that resetting and stepping works.""" 59 | env = env_cls() 60 | env.reset() 61 | env.step(env.action_space.sample()) 62 | 63 | 64 | if __name__ == '__main__': 65 | absltest.main() 66 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dkitty/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Gym environment registration for DKitty environments.""" 16 | 17 | from robel.utils.registration import register 18 | 19 | #=============================================================================== 20 | # Stand tasks 21 | #=============================================================================== 22 | 23 | # Default number of steps per episode. 24 | _STAND_EPISODE_LEN = 80 # 80*40*2.5ms = 8s 25 | 26 | register( 27 | env_id='DKittyStandFixed-v0', 28 | class_path='robel.dkitty.stand:DKittyStandFixed', 29 | max_episode_steps=_STAND_EPISODE_LEN) 30 | 31 | register( 32 | env_id='DKittyStandRandom-v0', 33 | class_path='robel.dkitty.stand:DKittyStandRandom', 34 | max_episode_steps=_STAND_EPISODE_LEN) 35 | 36 | register( 37 | env_id='DKittyStandRandomDynamics-v0', 38 | class_path='robel.dkitty.stand:DKittyStandRandomDynamics', 39 | max_episode_steps=_STAND_EPISODE_LEN) 40 | 41 | #=============================================================================== 42 | # Orient tasks 43 | #=============================================================================== 44 | 45 | # Default number of steps per episode. 46 | _ORIENT_EPISODE_LEN = 80 # 80*40*2.5ms = 8s 47 | 48 | register( 49 | env_id='DKittyOrientFixed-v0', 50 | class_path='robel.dkitty.orient:DKittyOrientFixed', 51 | max_episode_steps=_ORIENT_EPISODE_LEN) 52 | 53 | register( 54 | env_id='DKittyOrientRandom-v0', 55 | class_path='robel.dkitty.orient:DKittyOrientRandom', 56 | max_episode_steps=_ORIENT_EPISODE_LEN) 57 | 58 | register( 59 | env_id='DKittyOrientRandomDynamics-v0', 60 | class_path='robel.dkitty.orient:DKittyOrientRandomDynamics', 61 | max_episode_steps=_ORIENT_EPISODE_LEN) 62 | 63 | #=============================================================================== 64 | # Walk tasks 65 | #=============================================================================== 66 | 67 | # Default number of steps per episode. 68 | _WALK_EPISODE_LEN = 160 # 160*40*2.5ms = 16s 69 | 70 | register( 71 | env_id='DKittyWalk-v0', 72 | class_path='robel.dkitty.walk:DKittyWalk', 73 | max_episode_steps=_WALK_EPISODE_LEN) 74 | 75 | register( 76 | env_id='DKittyWalkFixed-v0', 77 | class_path='robel.dkitty.walk:DKittyWalkFixed', 78 | max_episode_steps=_WALK_EPISODE_LEN) 79 | 80 | register( 81 | env_id='DKittyWalkRandom-v0', 82 | class_path='robel.dkitty.walk:DKittyWalkRandom', 83 | max_episode_steps=_WALK_EPISODE_LEN) 84 | 85 | register( 86 | env_id='DKittyWalkRandomDynamics-v0', 87 | class_path='robel.dkitty.walk:DKittyWalkRandomDynamics', 88 | max_episode_steps=_WALK_EPISODE_LEN) 89 | 90 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dkitty/assets/dkitty_avoid-v0.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dkitty/assets/dkitty_orient-v0.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dkitty/assets/dkitty_play-v0.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dkitty/assets/dkitty_push-v0.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dkitty/assets/dkitty_stand-v0.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dkitty/assets/dkitty_walk-v0.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dkitty/orient_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for DKitty walk tasks.""" 16 | 17 | from absl.testing import absltest 18 | from absl.testing import parameterized 19 | import gym 20 | import numpy as np 21 | 22 | from robel.dkitty.orient import (DKittyOrientFixed, DKittyOrientRandom, 23 | DKittyOrientRandomDynamics) 24 | # pylint: disable=no-member 25 | 26 | 27 | @parameterized.parameters( 28 | ('DKittyOrientFixed-v0', DKittyOrientFixed), 29 | ('DKittyOrientRandom-v0', DKittyOrientRandom), 30 | ('DKittyOrientRandomDynamics-v0', DKittyOrientRandomDynamics), 31 | ) 32 | class DKittyOrientTest(absltest.TestCase): 33 | """Unit test class for RobotEnv.""" 34 | 35 | def test_gym_make(self, env_id, env_cls): 36 | """Accesses the sim, model, and data properties.""" 37 | env = gym.make(env_id) 38 | self.assertIsInstance(env.unwrapped, env_cls) 39 | 40 | def test_spaces(self, _, env_cls): 41 | """Checks the observation, action, and state spaces.""" 42 | env = env_cls() 43 | observation_size = np.sum([ 44 | 3, # root_pos 45 | 3, # root_euler 46 | 12, # kitty_qpos 47 | 3, # root_vel 48 | 3, # root_angular_vel 49 | 12, # kitty_qvel 50 | 12, # last_action 51 | 1, # upright 52 | 2, # current_facing 53 | 2, # desired_facing 54 | ]) 55 | self.assertEqual(env.observation_space.shape, (observation_size,)) 56 | self.assertEqual(env.action_space.shape, (12,)) 57 | self.assertEqual(env.state_space['root_pos'].shape, (3,)) 58 | self.assertEqual(env.state_space['root_euler'].shape, (3,)) 59 | self.assertEqual(env.state_space['root_vel'].shape, (3,)) 60 | self.assertEqual(env.state_space['root_angular_vel'].shape, (3,)) 61 | self.assertEqual(env.state_space['kitty_qpos'].shape, (12,)) 62 | self.assertEqual(env.state_space['kitty_qvel'].shape, (12,)) 63 | 64 | def test_reset_step(self, _, env_cls): 65 | """Checks that resetting and stepping works.""" 66 | env = env_cls() 67 | env.reset() 68 | env.step(env.action_space.sample()) 69 | 70 | 71 | if __name__ == '__main__': 72 | absltest.main() 73 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dkitty/stand_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for DKitty walk tasks.""" 16 | 17 | from absl.testing import absltest 18 | from absl.testing import parameterized 19 | import gym 20 | import numpy as np 21 | 22 | from robel.dkitty.stand import (DKittyStandFixed, DKittyStandRandom, 23 | DKittyStandRandomDynamics) 24 | # pylint: disable=no-member 25 | 26 | 27 | @parameterized.parameters( 28 | ('DKittyStandFixed-v0', DKittyStandFixed), 29 | ('DKittyStandRandom-v0', DKittyStandRandom), 30 | ('DKittyStandRandomDynamics-v0', DKittyStandRandomDynamics), 31 | ) 32 | class DKittyStandTest(absltest.TestCase): 33 | """Unit test class for RobotEnv.""" 34 | 35 | def test_gym_make(self, env_id, env_cls): 36 | """Accesses the sim, model, and data properties.""" 37 | env = gym.make(env_id) 38 | self.assertIsInstance(env.unwrapped, env_cls) 39 | 40 | def test_spaces(self, _, env_cls): 41 | """Checks the observation, action, and state spaces.""" 42 | env = env_cls() 43 | observation_size = np.sum([ 44 | 3, # root_qpos 45 | 3, # root_euler 46 | 12, # kitty_qpos 47 | 3, # root_vel 48 | 3, # root_angular_vel 49 | 12, # kitty_qvel 50 | 12, # last_action 51 | 1, # upright 52 | 12, # pose error 53 | ]) 54 | self.assertEqual(env.observation_space.shape, (observation_size,)) 55 | self.assertEqual(env.action_space.shape, (12,)) 56 | self.assertEqual(env.state_space['root_pos'].shape, (3,)) 57 | self.assertEqual(env.state_space['root_euler'].shape, (3,)) 58 | self.assertEqual(env.state_space['root_vel'].shape, (3,)) 59 | self.assertEqual(env.state_space['root_angular_vel'].shape, (3,)) 60 | self.assertEqual(env.state_space['kitty_qpos'].shape, (12,)) 61 | self.assertEqual(env.state_space['kitty_qvel'].shape, (12,)) 62 | 63 | def test_reset_step(self, _, env_cls): 64 | """Checks that resetting and stepping works.""" 65 | env = env_cls() 66 | env.reset() 67 | env.step(env.action_space.sample()) 68 | 69 | 70 | if __name__ == '__main__': 71 | absltest.main() 72 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dkitty/utils/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dkitty/utils/manual_reset.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Hardware reset functions for the D'Kitty.""" 16 | 17 | import time 18 | 19 | from robel.components.builder import ComponentBuilder 20 | from robel.components.robot import RobotComponentBuilder 21 | from robel.components.robot.dynamixel_robot import DynamixelRobotComponent 22 | from robel.components.tracking import TrackerComponentBuilder 23 | from robel.components.tracking.tracker import TrackerComponent 24 | from robel.utils.reset_procedure import ResetProcedure 25 | 26 | 27 | class ManualAutoDKittyResetProcedure(ResetProcedure): 28 | """Manual reset procedure for D'Kitty. 29 | 30 | This waits until the D'Kitty is placed upright and automatically starts the 31 | episode. 32 | """ 33 | 34 | def __init__(self, 35 | upright_threshold: float = 0.9, 36 | max_height: float = 0.35, 37 | min_successful_checks: int = 5, 38 | check_interval_sec: float = 0.1, 39 | print_interval_sec: float = 1.0, 40 | episode_start_delay_sec: float = 1.0): 41 | super().__init__() 42 | self._upright_threshold = upright_threshold 43 | self._max_height = max_height 44 | self._min_successful_checks = min_successful_checks 45 | self._check_interval_sec = check_interval_sec 46 | self._print_interval_sec = print_interval_sec 47 | self._episode_start_delay_sec = episode_start_delay_sec 48 | self._last_print_time = 0 49 | self._robot = None 50 | self._tracker = None 51 | 52 | def configure_reset_groups(self, builder: ComponentBuilder): 53 | """Configures the component groups needed for reset.""" 54 | if isinstance(builder, RobotComponentBuilder): 55 | assert 'dkitty' in builder.group_configs 56 | elif isinstance(builder, TrackerComponentBuilder): 57 | assert 'torso' in builder.group_configs 58 | 59 | def reset(self, robot: DynamixelRobotComponent, tracker: TrackerComponent): 60 | """Performs the reset procedure.""" 61 | self._robot = robot 62 | self._tracker = tracker 63 | 64 | def finish(self): 65 | """Called when the reset is complete.""" 66 | # Wait until the robot is sufficiently upright. 67 | self._wait_until_upright() 68 | 69 | def _wait_until_upright(self): 70 | """Waits until the D'Kitty is upright.""" 71 | upright_checks = 0 72 | self._last_print_time = 0 # Start at 0 so print happens first time. 73 | while True: 74 | if self._is_dkitty_upright(): 75 | upright_checks += 1 76 | else: 77 | upright_checks = 0 78 | if upright_checks > self._min_successful_checks: 79 | break 80 | time.sleep(self._check_interval_sec) 81 | 82 | print('Reset complete, starting episode...') 83 | time.sleep(self._episode_start_delay_sec) 84 | 85 | def _is_dkitty_upright(self) -> bool: 86 | """Checks if the D'Kitty is currently upright.""" 87 | state = self._tracker.get_state('torso') 88 | height = state.pos[2] 89 | upright = state.rot[2, 2] 90 | 91 | cur_time = time.time() 92 | if cur_time - self._last_print_time >= self._print_interval_sec: 93 | self._last_print_time = cur_time 94 | print(('Waiting for D\'Kitty to be upright (upright: {:.2f}, ' 95 | 'height: {:.2f})').format(upright, height)) 96 | 97 | if upright < self._upright_threshold: 98 | return False 99 | if height > self._max_height: 100 | return False 101 | return True 102 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/dkitty/walk_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for DKitty walk tasks.""" 16 | 17 | from absl.testing import absltest 18 | from absl.testing import parameterized 19 | import gym 20 | import numpy as np 21 | 22 | from robel.dkitty.walk import (DKittyWalkFixed, DKittyWalkRandom, 23 | DKittyWalkRandomDynamics) 24 | # pylint: disable=no-member 25 | 26 | 27 | @parameterized.parameters( 28 | ('DKittyWalkFixed-v0', DKittyWalkFixed), 29 | ('DKittyWalkRandom-v0', DKittyWalkRandom), 30 | ('DKittyWalkRandomDynamics-v0', DKittyWalkRandomDynamics), 31 | ) 32 | class DKittyWalkTest(absltest.TestCase): 33 | """Unit test class for RobotEnv.""" 34 | 35 | def test_gym_make(self, env_id, env_cls): 36 | """Accesses the sim, model, and data properties.""" 37 | env = gym.make(env_id) 38 | self.assertIsInstance(env.unwrapped, env_cls) 39 | 40 | def test_spaces(self, _, env_cls): 41 | """Checks the observation, action, and state spaces.""" 42 | env = env_cls() 43 | observation_size = np.sum([ 44 | 3, # root_qpos 45 | 3, # root_euler 46 | 12, # kitty_qpos 47 | 3, # root_vel 48 | 3, # root_angular_vel 49 | 12, # kitty_qvel 50 | 12, # last_action 51 | 1, # upright 52 | 1, # heading 53 | 2, # target error 54 | ]) 55 | self.assertEqual(env.observation_space.shape, (observation_size,)) 56 | self.assertEqual(env.action_space.shape, (12,)) 57 | self.assertEqual(env.state_space['root_pos'].shape, (3,)) 58 | self.assertEqual(env.state_space['root_euler'].shape, (3,)) 59 | self.assertEqual(env.state_space['root_vel'].shape, (3,)) 60 | self.assertEqual(env.state_space['root_angular_vel'].shape, (3,)) 61 | self.assertEqual(env.state_space['kitty_qpos'].shape, (12,)) 62 | self.assertEqual(env.state_space['kitty_qvel'].shape, (12,)) 63 | 64 | def test_reset_step(self, _, env_cls): 65 | """Checks that resetting and stepping works.""" 66 | env = env_cls() 67 | env.reset() 68 | env.step(env.action_space.sample()) 69 | 70 | 71 | if __name__ == '__main__': 72 | absltest.main() 73 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/.gitignore: -------------------------------------------------------------------------------- 1 | # General 2 | .DS_Store 3 | *.swp 4 | *.profraw 5 | 6 | # Editors 7 | .vscode 8 | .idea 9 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # How to Contribute 2 | 3 | We'd love to accept your patches and contributions to this project. There are 4 | just a few small guidelines you need to follow. 5 | 6 | ## Contributor License Agreement 7 | 8 | Contributions to this project must be accompanied by a Contributor License 9 | Agreement. You (or your employer) retain the copyright to your contribution; 10 | this simply gives us permission to use and redistribute your contributions as 11 | part of the project. Head over to to see 12 | your current agreements on file or to sign a new one. 13 | 14 | You generally only need to submit a CLA once, so if you've already submitted one 15 | (even if it was for a different project), you probably don't need to do it 16 | again. 17 | 18 | ## Code reviews 19 | 20 | All submissions, including submissions by project members, require review. We 21 | use GitHub pull requests for this purpose. Consult 22 | [GitHub Help](https://help.github.com/articles/about-pull-requests/) for more 23 | information on using pull requests. 24 | 25 | ## Community Guidelines 26 | 27 | This project follows 28 | [Google's Open Source Community Guidelines](https://opensource.google.com/conduct/). 29 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/README.md: -------------------------------------------------------------------------------- 1 | # ROBEL Scenes 2 | 3 | This repository is a collection of shared [MuJoCo](http://www.mujoco.org/) 4 | simulation scenes and assets for ROBEL environments. 5 | 6 | See [google-research/robel](https://github.com/google-research/robel) for more 7 | details. 8 | 9 | ## Contributing 10 | 11 | We designed ROBEL to be an easily extensible platform for new robots, tasks, and 12 | benchmarks. See [`CONTRIBUTING.md`](CONTRIBUTING.md) for a guide 13 | on how to contribute. 14 | 15 | ## Disclaimer 16 | 17 | This is not an official Google product. 18 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/assets/chain3xh_overlay.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/assets/dependencies3xh.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 91 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/dclaw3xh.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/meshes/acrylic_circle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw/meshes/acrylic_circle.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_4_clamping_high_poly.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_4_clamping_high_poly.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_4_finger_tip.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_4_finger_tip.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_base_high_poly.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_base_high_poly.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_base_hull.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_base_hull.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_base_hull_top.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_base_hull_top.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_circle_high_poly.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_circle_high_poly.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_clamping_high_poly.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_clamping_high_poly.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_clamping_small_high_poly.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_clamping_small_high_poly.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_finger_tip.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_finger_tip.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_white_top_high_poly.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw/meshes/xh_white_top_high_poly.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/assets/dependencies.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 80 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/assets/valve3.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/assets/valve3_0.py: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/assets/valve3_0.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/assets/valve4.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/meshes/station_metal_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw_stations/meshes/station_metal_base.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/meshes/station_plastic_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw_stations/meshes/station_plastic_base.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/meshes/station_plastic_walls.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw_stations/meshes/station_plastic_walls.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/meshes/station_surface.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw_stations/meshes/station_surface.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/meshes/valve_3_high_poly.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw_stations/meshes/valve_3_high_poly.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/meshes/valve_4_high_poly.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dclaw_stations/meshes/valve_4_high_poly.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/valve3_station.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dclaw_stations/valve4_station.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/assets/dependenciesB_xh-v2.1.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/kitty-v2.1.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/D_kitty_plates_V2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/D_kitty_plates_V2.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead_low_poly.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead_low_poly.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead_low_poly1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead_low_poly1.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead_low_poly2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead_low_poly2.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead_low_poly3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead_low_poly3.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead_low_poly4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead_low_poly4.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead_low_poly5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/bulkhead_low_poly5.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/fr12_h103_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/fr12_h103_3.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/screws.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/screws.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/torso.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/torso.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/x430_dkitty_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/x430_dkitty_2.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/xh_clamping_small.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/xh_clamping_small.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/dkitty/meshes/xh_finger_tip.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/dkitty/meshes/xh_finger_tip.stl -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/scenes/basic_scene.xml: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/robel-scenes/scenes/textures/white_marble_tile2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/robel-scenes/scenes/textures/white_marble_tile2.png -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/check_mujoco_deps.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Checks if the given MuJoCo XML file has valid dependencies. 16 | 17 | Example usage: 18 | python -m robel.scripts.check_mujoco_deps path/to/mujoco.xml 19 | """ 20 | 21 | import argparse 22 | import logging 23 | import os 24 | 25 | from robel.utils.resources import AssetBundle 26 | 27 | 28 | def main(): 29 | parser = argparse.ArgumentParser() 30 | parser.add_argument('path', nargs=1, help='The MuJoCo XML to parse.') 31 | args = parser.parse_args() 32 | model_path = args.path[0] 33 | 34 | if not os.path.exists(model_path): 35 | raise ValueError('Path does not exist: ' + model_path) 36 | 37 | logging.basicConfig(level=logging.INFO) 38 | with AssetBundle(dry_run=True, verbose=True) as bundle: 39 | bundle.add_mujoco(model_path) 40 | 41 | 42 | if __name__ == '__main__': 43 | main() 44 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/data/mjrl/DClawPoseMotion-v0-policy.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/scripts/data/mjrl/DClawPoseMotion-v0-policy.pkl -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/data/mjrl/DKittyOrientRandomDynamics-v0-policy.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/scripts/data/mjrl/DKittyOrientRandomDynamics-v0-policy.pkl -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/data/mjrl/DKittyStandRandomDynamics-v0-policy.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/scripts/data/mjrl/DKittyStandRandomDynamics-v0-policy.pkl -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/data/mjrl/DKittyWalkFixedOld-v0-policy.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/scripts/data/mjrl/DKittyWalkFixedOld-v0-policy.pkl -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/data/mjrl/DKittyWalkRandomDynamics-v0-policy.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/scripts/data/mjrl/DKittyWalkRandomDynamics-v0-policy.pkl -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/data/softlearning/DClawScrewFixed-v0-policy.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/scripts/data/softlearning/DClawScrewFixed-v0-policy.pkl -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/data/softlearning/DClawScrewRandom-v0-policy.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/scripts/data/softlearning/DClawScrewRandom-v0-policy.pkl -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/data/softlearning/DClawTurnFixed-v0-policy.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/scripts/data/softlearning/DClawTurnFixed-v0-policy.pkl -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/data/softlearning/DClawTurnRandom-v0-policy.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rmrafailov/VMAIL/8c2d2d98064f8d190aed0c26c6239108621fea6d/robel_claw/robel/robel/scripts/data/softlearning/DClawTurnRandom-v0-policy.pkl -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/enjoy_mjrl.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Runs an MJRL policy. 16 | 17 | Example usage: 18 | python -m robel.scripts.enjoy_mjrl --device /dev/ttyUSB0 19 | 20 | This runs the DClawTurnFixed-v0 environment by default. To run other 21 | environments, pass in the environment name with `-e/--env_name` 22 | 23 | python -m robel.scripts.enjoy_mjrl \ 24 | --env_name DClawScrewFixed-v0 \ 25 | --device /dev/ttyUSB0 26 | """ 27 | 28 | import argparse 29 | import os 30 | import pickle 31 | 32 | from robel.scripts.rollout import rollout_script 33 | 34 | POLICY_DIR = os.path.join( 35 | os.path.abspath(os.path.dirname(__file__)), 'data/mjrl') 36 | 37 | DEFAULT_POLICY_FORMAT = os.path.join(POLICY_DIR, '{}-policy.pkl') 38 | 39 | 40 | def policy_factory(args: argparse.Namespace): 41 | """Creates the policy.""" 42 | # Get default policy path from the environment name. 43 | policy_path = args.policy 44 | if not policy_path: 45 | policy_path = DEFAULT_POLICY_FORMAT.format(args.env_name) 46 | 47 | # Load the policy 48 | with open(policy_path, 'rb') as f: 49 | policy = pickle.load(f) 50 | 51 | def policy_fn(obs): 52 | _, info = policy.get_action(obs) 53 | return info['evaluation'] 54 | 55 | return policy_fn 56 | 57 | 58 | if __name__ == '__main__': 59 | rollout_script(policy_factory=policy_factory, add_policy_arg=True) 60 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/enjoy_softlearning.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Runs a softlearning policy. 16 | 17 | Example usage: 18 | python -m robel.scripts.enjoy_softlearning --device /dev/ttyUSB0 19 | 20 | This runs the DClawTurnFixed-v0 environment by default. To run other 21 | environments, pass in the environment name with `-e/--env_name` 22 | 23 | python -m robel.scripts.enjoy_softlearning \ 24 | --env_name DClawScrewFixed-v0 \ 25 | --device /dev/ttyUSB0 26 | """ 27 | 28 | import argparse 29 | import os 30 | import pickle 31 | 32 | import numpy as np 33 | 34 | from robel.scripts.rollout import rollout_script 35 | 36 | POLICY_DIR = os.path.join( 37 | os.path.abspath(os.path.dirname(__file__)), 'data/softlearning') 38 | 39 | DEFAULT_POLICY_FORMAT = os.path.join(POLICY_DIR, '{}.pkl') 40 | 41 | 42 | def policy_factory(args: argparse.Namespace): 43 | """Creates the policy.""" 44 | # Get default policy path from the environment name. 45 | policy_path = args.policy 46 | if not policy_path: 47 | policy_path = DEFAULT_POLICY_FORMAT.format(args.env_name) 48 | 49 | # Load the policy 50 | with open(policy_path, 'rb') as f: 51 | policy_data = pickle.load(f) 52 | policy = policy_data['policy'] 53 | policy.set_weights(policy_data['weights']) 54 | 55 | def policy_fn(obs): 56 | with policy.set_deterministic(True): 57 | actions = policy.actions_np(np.expand_dims(obs, axis=0)) 58 | return actions[0] 59 | 60 | return policy_fn 61 | 62 | 63 | if __name__ == '__main__': 64 | rollout_script(policy_factory=policy_factory, add_policy_arg=True) 65 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/find_vr_devices.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Allows the user to interact with the OpenVR client.""" 16 | 17 | import cmd 18 | import logging 19 | 20 | from transforms3d.euler import euler2quat 21 | 22 | from robel.components.tracking.virtual_reality.client import VrClient 23 | 24 | INTRODUCTION = """Interactive shell for using the OpenVR client. 25 | 26 | Type `help` or `?` to list commands. 27 | """ 28 | 29 | 30 | class VrDeviceShell(cmd.Cmd): 31 | """Implements a command-line interface for using the OpenVR client.""" 32 | 33 | intro = INTRODUCTION 34 | prompt = '>>> ' 35 | 36 | def __init__(self, client: VrClient): 37 | super().__init__() 38 | self.client = client 39 | 40 | def do_list(self, unused_arg): 41 | """Lists the available devices on the machine.""" 42 | devices = self.client.discover_devices() 43 | if not devices: 44 | print('No devices found!') 45 | return 46 | for device in devices: 47 | print(device.get_summary()) 48 | 49 | def do_pose(self, args): 50 | """Prints the pose for the given device.""" 51 | names = args.split() 52 | devices = [self.client.get_device(name) for name in names] 53 | 54 | pose_batch = self.client.get_poses() 55 | for device in devices: 56 | state = pose_batch.get_state(device) 57 | print(device.get_summary()) 58 | print('> Pos: [{:.3f} {:.3f} {:.3f}]'.format(*state.pos)) 59 | print('> Rot: [{:.3f} {:.3f} {:.3f}]'.format(*state.rot_euler)) 60 | 61 | def emptyline(self): 62 | """Overrides behavior when an empty line is sent.""" 63 | 64 | 65 | if __name__ == '__main__': 66 | logging.basicConfig(level=logging.INFO) 67 | with VrClient() as vr_client: 68 | repl = VrDeviceShell(vr_client) 69 | repl.cmdloop() 70 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/scripts/reset_hardware.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Script to test resetting robot hardware environments. 16 | 17 | To run: 18 | python -m robel.scripts.reset_hardware \ 19 | -e DKittyWalkFixed-v0 -d /dev/ttyUSB0 20 | """ 21 | 22 | import argparse 23 | import logging 24 | import time 25 | 26 | import gym 27 | 28 | import robel 29 | from robel.scripts.utils import parse_env_args 30 | 31 | 32 | def main(): 33 | # Get command line arguments. 34 | parser = argparse.ArgumentParser() 35 | parser.add_argument( 36 | '-n', 37 | '--num_repeats', 38 | type=int, 39 | default=1, 40 | help='The number of resets to perform.') 41 | env_id, params, args = parse_env_args(parser) 42 | 43 | # Show INFO-level logs. 44 | logging.basicConfig(level=logging.INFO) 45 | 46 | # Create the environment and get the robot component. 47 | robel.set_env_params(env_id, params) 48 | env = gym.make(env_id).unwrapped 49 | assert env.robot.is_hardware 50 | 51 | for i in range(args.num_repeats): 52 | print('Starting reset #{}'.format(i)) 53 | 54 | # Disengage all of the motors and let the dkitty fall. 55 | env.robot.set_motors_engaged(None, engaged=False) 56 | 57 | print('Place the robot to a starting position.') 58 | input('Press Enter to start the reset...') 59 | 60 | # Start with all motors engaged. 61 | env.robot.set_motors_engaged(None, engaged=True) 62 | env.reset() 63 | 64 | print('Done reset! Turning off the robot in a few seconds.') 65 | time.sleep(2) 66 | 67 | 68 | if __name__ == '__main__': 69 | main() 70 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/simulation/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/simulation/mjpy_renderer.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Rendering simulation using mujoco_py.""" 16 | 17 | import mujoco_py 18 | import numpy as np 19 | 20 | from robel.simulation.renderer import Renderer, RenderMode 21 | 22 | 23 | class MjPyRenderer(Renderer): 24 | """Class for rendering mujoco_py simulations.""" 25 | 26 | def __init__(self, sim): 27 | assert isinstance(sim, mujoco_py.MjSim), \ 28 | 'MjPyRenderer takes a mujoco_py MjSim object.' 29 | super().__init__(sim) 30 | self._onscreen_renderer = None 31 | self._offscreen_renderer = None 32 | 33 | def render_to_window(self): 34 | """Renders the simulation to a window.""" 35 | if not self._onscreen_renderer: 36 | self._onscreen_renderer = mujoco_py.MjViewer(self._sim) 37 | self._update_camera_properties(self._onscreen_renderer.cam) 38 | 39 | self._onscreen_renderer.render() 40 | 41 | def refresh_window(self): 42 | """Refreshes the rendered window if one is present.""" 43 | if self._onscreen_renderer is None: 44 | return 45 | self._onscreen_renderer.render() 46 | 47 | def render_offscreen(self, 48 | width: int, 49 | height: int, 50 | mode: RenderMode = RenderMode.RGB, 51 | camera_id: int = -1) -> np.ndarray: 52 | """Renders the camera view as a numpy array of pixels. 53 | 54 | Args: 55 | width: The viewport width (pixels). 56 | height: The viewport height (pixels). 57 | mode: The rendering mode. 58 | camera_id: The ID of the camera to render from. By default, uses 59 | the free camera. 60 | 61 | Returns: 62 | A numpy array of the pixels. 63 | """ 64 | assert width > 0 and height > 0 65 | if not self._offscreen_renderer: 66 | self._offscreen_renderer = mujoco_py \ 67 | .MjRenderContextOffscreen(self._sim, device_id=-1) 68 | 69 | # Update the camera configuration for the free-camera. 70 | if camera_id == -1: 71 | self._update_camera_properties(self._offscreen_renderer.cam) 72 | 73 | self._offscreen_renderer.render(width, height, camera_id) 74 | if mode == RenderMode.RGB: 75 | data = self._offscreen_renderer.read_pixels( 76 | width, height, depth=False) 77 | # Original image is upside-down, so flip it 78 | return data[::-1, :, :] 79 | elif mode == RenderMode.DEPTH: 80 | data = self._offscreen_renderer.read_pixels( 81 | width, height, depth=True)[1] 82 | # Original image is upside-down, so flip it 83 | return data[::-1, :] 84 | else: 85 | raise NotImplementedError(mode) 86 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/simulation/mjpy_sim_scene.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Simulation using DeepMind Control Suite.""" 16 | 17 | import logging 18 | import os 19 | from typing import Any 20 | 21 | import mujoco_py 22 | from mujoco_py.builder import cymj, user_warning_raise_exception 23 | 24 | from robel.simulation.mjpy_renderer import MjPyRenderer 25 | from robel.simulation.sim_scene import SimScene 26 | 27 | 28 | # Custom handler for MuJoCo exceptions. 29 | def _mj_warning_fn(warn_data: bytes): 30 | """Warning function override for mujoco_py.""" 31 | try: 32 | user_warning_raise_exception(warn_data) 33 | except mujoco_py.MujocoException as e: 34 | logging.error('MuJoCo Exception: %s', str(e)) 35 | 36 | 37 | cymj.set_warning_callback(_mj_warning_fn) 38 | 39 | 40 | class MjPySimScene(SimScene): 41 | """Encapsulates a MuJoCo robotics simulation using mujoco_py.""" 42 | 43 | def _load_simulation(self, model_handle: Any) -> Any: 44 | """Loads the simulation from the given model handle. 45 | 46 | Args: 47 | model_handle: Path to the Mujoco XML file to load. 48 | 49 | Returns: 50 | A mujoco_py MjSim object. 51 | """ 52 | if isinstance(model_handle, str): 53 | if not os.path.isfile(model_handle): 54 | raise ValueError( 55 | '[MjPySimScene] Invalid model file path: {}'.format( 56 | model_handle)) 57 | 58 | model = mujoco_py.load_model_from_path(model_handle) 59 | sim = mujoco_py.MjSim(model) 60 | else: 61 | raise NotImplementedError(model_handle) 62 | return sim 63 | 64 | def _create_renderer(self, sim: Any) -> MjPyRenderer: 65 | """Creates a renderer for the given simulation.""" 66 | return MjPyRenderer(sim) 67 | 68 | def copy_model(self) -> Any: 69 | """Returns a copy of the MjModel object.""" 70 | null_model = self.get_mjlib().PyMjModel() 71 | model_copy = self.get_mjlib().mj_copyModel(null_model, self.model) 72 | return model_copy 73 | 74 | def save_binary(self, path: str) -> str: 75 | """Saves the loaded model to a binary .mjb file. 76 | 77 | Returns: 78 | The file path that the binary was saved to. 79 | """ 80 | if not path.endswith('.mjb'): 81 | path = path + '.mjb' 82 | self.get_mjlib().mj_saveModel(self.model, path.encode(), None, 0) 83 | return path 84 | 85 | def upload_height_field(self, hfield_id: int): 86 | """Uploads the height field to the rendering context.""" 87 | if not self.sim.render_contexts: 88 | logging.warning('No rendering context; not uploading height field.') 89 | return 90 | self.get_mjlib().mjr_uploadHField( 91 | self.model, self.sim.render_contexts[0].con, hfield_id) 92 | 93 | def get_mjlib(self) -> Any: 94 | """Returns an interface to the low-level MuJoCo API.""" 95 | return _MjlibWrapper(mujoco_py.cymj) 96 | 97 | def get_handle(self, value: Any) -> Any: 98 | """Returns a handle that can be passed to mjlib methods.""" 99 | return value 100 | 101 | 102 | class _MjlibWrapper: 103 | """Wrapper that forwards mjlib calls.""" 104 | 105 | def __init__(self, lib): 106 | self._lib = lib 107 | 108 | def __getattr__(self, name: str): 109 | if name.startswith('mj'): 110 | return getattr(self._lib, '_' + name) 111 | return getattr(self._lib, name) 112 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/simulation/renderer.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Rendering API for MuJoCo simulations.""" 16 | 17 | import abc 18 | import enum 19 | from typing import Any, Optional, Sequence 20 | 21 | import numpy as np 22 | 23 | 24 | class RenderMode(enum.Enum): 25 | """Rendering modes for offscreen rendering.""" 26 | RGB = 0 27 | DEPTH = 1 28 | SEGMENTATION = 2 29 | 30 | 31 | class Renderer(abc.ABC): 32 | """Base interface for rendering simulations.""" 33 | 34 | def __init__(self, sim): 35 | """Initializes a new renderer. 36 | 37 | Args: 38 | sim: A handle to the simulation. 39 | """ 40 | self._sim = sim 41 | self._camera_settings = {} 42 | 43 | @abc.abstractmethod 44 | def render_to_window(self): 45 | """Renders the simulation to a window.""" 46 | 47 | @abc.abstractmethod 48 | def refresh_window(self): 49 | """Refreshes the rendered window if one is present.""" 50 | 51 | @abc.abstractmethod 52 | def render_offscreen(self, 53 | width: int, 54 | height: int, 55 | mode: RenderMode = RenderMode.RGB, 56 | camera_id: int = -1) -> np.ndarray: 57 | """Renders the camera view as a numpy array of pixels. 58 | 59 | Args: 60 | width: The viewport width (pixels). 61 | height: The viewport height (pixels). 62 | mode: The rendering mode. 63 | camera_id: The ID of the camera to render from. By default, uses 64 | the free camera. 65 | 66 | Returns: 67 | A numpy array of the pixels. 68 | """ 69 | 70 | def set_free_camera_settings( 71 | self, 72 | distance: Optional[float] = None, 73 | azimuth: Optional[float] = None, 74 | elevation: Optional[float] = None, 75 | lookat: Sequence[float] = None, 76 | center: bool = True, 77 | ): 78 | """Sets the free camera parameters. 79 | 80 | Args: 81 | distance: The distance of the camera from the target. 82 | azimuth: Horizontal angle of the camera, in degrees. 83 | elevation: Vertical angle of the camera, in degrees. 84 | lookat: The (x, y, z) position in world coordinates to target. 85 | center: If True and `lookat` is not given, targets the camera at the 86 | median position of the simulation geometry. 87 | """ 88 | settings = {} 89 | if distance is not None: 90 | settings['distance'] = distance 91 | if azimuth is not None: 92 | settings['azimuth'] = azimuth 93 | if elevation is not None: 94 | settings['elevation'] = elevation 95 | if lookat is not None: 96 | settings['lookat'] = np.array(lookat, dtype=np.float32) 97 | elif center: 98 | # Calculate the center of the simulation geometry. 99 | settings['lookat'] = np.array( 100 | [np.median(self._sim.data.geom_xpos[:, i]) for i in range(3)], 101 | dtype=np.float32) 102 | 103 | self._camera_settings = settings 104 | 105 | def close(self): 106 | """Cleans up any resources being used by the renderer.""" 107 | 108 | def _update_camera_properties(self, camera: Any): 109 | """Updates the given camera object with the current camera settings.""" 110 | for key, value in self._camera_settings.items(): 111 | if key == 'lookat': 112 | getattr(camera, key)[:] = value 113 | else: 114 | setattr(camera, key, value) 115 | 116 | def __del__(self): 117 | """Automatically clean up when out of scope.""" 118 | self.close() 119 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/simulation/sim_scene_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for SimScene.""" 16 | 17 | import contextlib 18 | import tempfile 19 | from typing import Generator 20 | 21 | from absl.testing import absltest 22 | 23 | from robel.simulation.sim_scene import SimBackend, SimScene 24 | 25 | # Simple MuJoCo model XML. 26 | TEST_MODEL_XML = """ 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | """ 48 | 49 | 50 | @contextlib.contextmanager 51 | def test_model_file() -> Generator[str, None, None]: 52 | """Context manager that yields a temporary MuJoCo XML file.""" 53 | with tempfile.NamedTemporaryFile(mode='w+t', suffix='.xml') as f: 54 | f.write(TEST_MODEL_XML) 55 | f.flush() 56 | f.seek(0) 57 | yield f.name 58 | 59 | 60 | def mjpy_and_dm(fn): 61 | """Decorator that tests for both mujoco_py and dm_control.""" 62 | 63 | def test_fn(self: absltest.TestCase): 64 | with test_model_file() as test_file_path: 65 | with self.subTest('mujoco_py'): 66 | fn( 67 | self, 68 | SimScene.create( 69 | test_file_path, backend=SimBackend.MUJOCO_PY)) 70 | with self.subTest('dm_control'): 71 | fn( 72 | self, 73 | SimScene.create( 74 | test_file_path, backend=SimBackend.DM_CONTROL)) 75 | 76 | return test_fn 77 | 78 | 79 | class SimSceneTest(absltest.TestCase): 80 | """Unit test class for SimScene.""" 81 | 82 | @mjpy_and_dm 83 | def test_load(self, robot: SimScene): 84 | self.assertIsNotNone(robot.sim) 85 | self.assertIsNotNone(robot.model) 86 | self.assertIsNotNone(robot.data) 87 | 88 | @mjpy_and_dm 89 | def test_step(self, robot: SimScene): 90 | robot.sim.reset() 91 | robot.sim.forward() 92 | robot.sim.step() 93 | 94 | @mjpy_and_dm 95 | def test_accessors(self, robot: SimScene): 96 | self.assertTrue(robot.model.body_name2id('main') >= 0) 97 | self.assertTrue(robot.model.geom_name2id('base') >= 0) 98 | self.assertTrue(robot.model.site_name2id('end') >= 0) 99 | self.assertTrue(robot.model.joint_name2id('j1') >= 0) 100 | self.assertIsNotNone(robot.data.body_xpos[0]) 101 | self.assertIsNotNone(robot.data.body_xquat[0]) 102 | 103 | @mjpy_and_dm 104 | def test_copy_model(self, robot: SimScene): 105 | initial_pos = robot.model.body_pos[0].copy().tolist() 106 | 107 | model_copy = robot.copy_model() 108 | robot.model.body_pos[0, :] = [0.1, 0.2, 0.3] 109 | 110 | self.assertListEqual(model_copy.body_pos[0].tolist(), initial_pos) 111 | self.assertListEqual(robot.model.body_pos[0].tolist(), [0.1, 0.2, 0.3]) 112 | 113 | 114 | if __name__ == '__main__': 115 | absltest.main() 116 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/utils/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/utils/configurable_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Tests for configurable.""" 16 | 17 | import pickle 18 | import tempfile 19 | 20 | from absl.testing import absltest 21 | 22 | from robel.utils.configurable import configurable 23 | 24 | TEST_CONFIGS = {} 25 | 26 | 27 | @configurable(config_cache=TEST_CONFIGS) 28 | class DummyWithConfig(object): 29 | def __init__(self, a=1, b=2, c=3): 30 | self.a = a 31 | self.b = b 32 | self.c = c 33 | 34 | 35 | class ChildDummyWithConfig(DummyWithConfig): 36 | pass 37 | 38 | 39 | @configurable(pickleable=True, config_cache=TEST_CONFIGS) 40 | class DummyWithConfigPickleable(object): 41 | def __init__(self, a=1, b=2, c=3): 42 | self.a = a 43 | self.b = b 44 | self.c = c 45 | 46 | 47 | class TestConfigurable(absltest.TestCase): 48 | """Unit tests for configurable.""" 49 | 50 | def setUp(self): 51 | TEST_CONFIGS.clear() 52 | 53 | def test_instance(self): 54 | """Tests default values.""" 55 | d = DummyWithConfig() 56 | self.assertEqual(d.a, 1) 57 | self.assertEqual(d.b, 2) 58 | self.assertEqual(d.c, 3) 59 | 60 | def test_set_config(self): 61 | """Tests setting a config values.""" 62 | TEST_CONFIGS[DummyWithConfig] = {'a': 4, 'c': 5} 63 | 64 | d = DummyWithConfig() 65 | self.assertEqual(d.a, 4) 66 | self.assertEqual(d.b, 2) 67 | self.assertEqual(d.c, 5) 68 | 69 | def test_set_config_kwargs(self): 70 | """Tests overriding a config with kwargs.""" 71 | TEST_CONFIGS[DummyWithConfig] = {'a': 4, 'c': 5} 72 | 73 | d = DummyWithConfig(a=7) 74 | self.assertEqual(d.a, 7) 75 | self.assertEqual(d.b, 2) 76 | self.assertEqual(d.c, 5) 77 | 78 | def test_set_config_inheritance(self): 79 | """Tests config values for a child class.""" 80 | TEST_CONFIGS[ChildDummyWithConfig] = {'a': 4, 'c': 5} 81 | 82 | d1 = ChildDummyWithConfig() 83 | self.assertEqual(d1.a, 4) 84 | self.assertEqual(d1.b, 2) 85 | self.assertEqual(d1.c, 5) 86 | 87 | d2 = DummyWithConfig() 88 | self.assertEqual(d2.a, 1) 89 | self.assertEqual(d2.b, 2) 90 | self.assertEqual(d2.c, 3) 91 | 92 | def test_pickle(self): 93 | """Tests loading from a pickled object.""" 94 | TEST_CONFIGS[DummyWithConfigPickleable] = {'a': 4, 'c': 5} 95 | 96 | d = DummyWithConfigPickleable(b=8) 97 | TEST_CONFIGS.clear() 98 | 99 | with tempfile.TemporaryFile() as f: 100 | pickle.dump(d, f) 101 | f.seek(0) 102 | d2 = pickle.load(f) 103 | 104 | self.assertEqual(d2.a, 4) 105 | self.assertEqual(d2.b, 8) 106 | self.assertEqual(d2.c, 5) 107 | 108 | def test_pickle_override(self): 109 | """Tests overriding serialized parameters.""" 110 | TEST_CONFIGS[DummyWithConfigPickleable] = {'a': 4, 'c': 5} 111 | 112 | d = DummyWithConfigPickleable(c=1) 113 | self.assertEqual(d.a, 4) 114 | self.assertEqual(d.b, 2) 115 | self.assertEqual(d.c, 1) 116 | 117 | with tempfile.TemporaryFile() as f: 118 | pickle.dump(d, f) 119 | f.seek(0) 120 | 121 | TEST_CONFIGS[DummyWithConfigPickleable] = {'b': 5} 122 | 123 | d2 = pickle.load(f) 124 | 125 | self.assertEqual(d2.a, 4) 126 | self.assertEqual(d2.b, 5) 127 | self.assertEqual(d2.c, 1) 128 | 129 | 130 | if __name__ == '__main__': 131 | absltest.main() 132 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/utils/math_utils.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Utility functions relating to math.""" 16 | 17 | import logging 18 | from typing import Sequence 19 | 20 | import numpy as np 21 | 22 | 23 | def average_quaternions(quaternions: Sequence[np.ndarray]) -> np.ndarray: 24 | """Returns the average of the given quaternions. 25 | 26 | Args: 27 | quaternions: A list of quaternions to average. 28 | 29 | Returns: 30 | The averaged quaternion. 31 | """ 32 | # Implements the algorithm from: 33 | # Markley, F. L., Cheng, Y., Crassidis, J. L., & Oshman, Y. (2007). 34 | # Averaging quaternions. Journal of Guidance, Control, and Dynamics, 35 | # 30(4), 1193-1197. 36 | n_quat = len(quaternions) 37 | assert n_quat > 0, 'Must provide at least one quaternion.' 38 | weight = 1.0 / n_quat # Uniform weighting for all quaternions. 39 | q_matrix = np.vstack(quaternions) 40 | assert q_matrix.shape == (n_quat, 4) 41 | m_matrix = np.matmul(weight * np.transpose(q_matrix), q_matrix) 42 | _, eig_vecs = np.linalg.eigh(m_matrix) 43 | # The final eigenvector corresponds to the largest eigenvalue. 44 | return eig_vecs[:, -1] 45 | 46 | 47 | def calculate_cosine(vec1: np.ndarray, vec2: np.ndarray) -> np.ndarray: 48 | """Calculates the cosine angle between two vectors. 49 | 50 | This computes cos(theta) = dot(v1, v2) / (norm(v1) * norm(v2)) 51 | 52 | Args: 53 | vec1: The first vector. This can have a batch dimension. 54 | vec2: The second vector. This can have a batch dimension. 55 | 56 | Returns: 57 | The cosine angle between the two vectors, with the same batch dimension 58 | as the given vectors. 59 | """ 60 | if np.shape(vec1) != np.shape(vec2): 61 | raise ValueError('{} must have the same shape as {}'.format(vec1, vec2)) 62 | ndim = np.ndim(vec1) 63 | if ndim < 1 or ndim > 2: 64 | raise ValueError('{} must be 1 or 2 dimensions'.format(vec1)) 65 | axis = 1 if ndim == 2 else 0 66 | norm_product = ( 67 | np.linalg.norm(vec1, axis=axis) * np.linalg.norm(vec2, axis=axis)) 68 | zero_norms = norm_product == 0 69 | if np.any(zero_norms): 70 | logging.warning( 71 | '%s or %s is all 0s; this may be normal during initialization.', 72 | str(vec1), str(vec2)) 73 | if ndim == 2: 74 | norm_product[zero_norms] = 1 75 | else: 76 | norm_product = 1 77 | # Return the batched dot product. 78 | return np.einsum('...i,...i', vec1, vec2) / norm_product 79 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/utils/math_utils_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Tests for math_utils.""" 16 | 17 | from absl.testing import absltest 18 | import numpy as np 19 | from transforms3d.euler import euler2quat, quat2euler 20 | 21 | from robel.utils.math_utils import average_quaternions, calculate_cosine 22 | 23 | 24 | class AverageQuaternionsTest(absltest.TestCase): 25 | """Tests for `average_quaternions`.""" 26 | 27 | def test_identity(self): 28 | """Average one quaternion should equal itself.""" 29 | test_quat = euler2quat(np.pi / 4, np.pi / 4, np.pi / 4) 30 | avg_quat = average_quaternions([test_quat]) 31 | np.testing.assert_array_almost_equal(avg_quat, test_quat) 32 | 33 | def test_multiple_identity(self): 34 | """Average multiple copies of a quaternion should equal itself.""" 35 | test_quat = euler2quat(np.pi / 4, np.pi / 4, np.pi / 4) 36 | avg_quat = average_quaternions([test_quat, test_quat, test_quat]) 37 | np.testing.assert_array_almost_equal(avg_quat, test_quat) 38 | 39 | def test_average_two(self): 40 | """Averaging two different quaternions.""" 41 | quat1 = euler2quat(np.pi / 4, 0, 0) 42 | quat2 = euler2quat(-np.pi / 4, 0, 0) 43 | avg_quat = average_quaternions([quat1, quat2]) 44 | result = quat2euler(avg_quat) 45 | np.testing.assert_array_almost_equal(result, [0, 0, 0]) 46 | 47 | 48 | class CalculateCosineTest(absltest.TestCase): 49 | """Tests for `calculate_cosine`.""" 50 | 51 | def test_identical(self): 52 | """Two of the same vectors are completely aligned.""" 53 | v1 = np.array([1, 0]) 54 | self.assertAlmostEqual(calculate_cosine(v1, v1), 1) 55 | 56 | def test_parallel(self): 57 | """Two parallel vectors.""" 58 | v1 = np.array([1, 2]) 59 | v2 = np.array([2, 4]) 60 | self.assertAlmostEqual(calculate_cosine(v1, v2), 1) 61 | 62 | def test_opposite(self): 63 | """Two parallel vectors.""" 64 | v1 = np.array([1, 2]) 65 | v2 = np.array([-1, -2]) 66 | self.assertAlmostEqual(calculate_cosine(v1, v2), -1) 67 | 68 | def test_orthogonal(self): 69 | """Two orthogonal vectors.""" 70 | v1 = np.array([1, 1]) 71 | v2 = np.array([1, -1]) 72 | self.assertAlmostEqual(calculate_cosine(v1, v2), 0) 73 | 74 | def test_batched(self): 75 | """Multiple vectors.""" 76 | v1 = np.array([[1, 1], [2, 2]]) 77 | v2 = np.array([[1, -1], [3, 3]]) 78 | np.testing.assert_array_almost_equal(calculate_cosine(v1, v2), [0, 1]) 79 | 80 | def test_zero(self): 81 | """Tests when the norm is 0.""" 82 | v1 = np.array([1, 0]) 83 | v2 = np.array([0, 0]) 84 | self.assertAlmostEqual(calculate_cosine(v1, v2), 0) 85 | 86 | def test_zero_batched(self): 87 | """Tests when the norm is 0.""" 88 | v1 = np.array([[1, 0], [1, 1]]) 89 | v2 = np.array([[0, 0], [2, 2]]) 90 | np.testing.assert_array_almost_equal(calculate_cosine(v1, v2), [0, 1]) 91 | 92 | 93 | if __name__ == '__main__': 94 | absltest.main() 95 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/utils/plotting.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Helper methods and classes for plotting data.""" 16 | 17 | from typing import Optional, Sequence 18 | 19 | import matplotlib.pyplot as plt 20 | import matplotlib.animation as animation 21 | 22 | 23 | class AnimatedPlot: 24 | """Plots data that can be updated over time.""" 25 | 26 | def __init__(self, bounds: Optional[Sequence[float]] = None): 27 | """Initializes a new plot.""" 28 | self.fig, self.ax = plt.subplots() 29 | self.ani = None 30 | self.update_fn = None 31 | self.iteration = 0 32 | self.elements = [] 33 | 34 | if bounds: 35 | self.ax.axis(bounds) 36 | self.ax.grid() 37 | 38 | @property 39 | def is_open(self) -> bool: 40 | """Returns True if the figure window is open.""" 41 | return plt.fignum_exists(self.fig.number) 42 | 43 | def add(self, element): 44 | """Adds an element for animation.""" 45 | self.elements.append(element) 46 | 47 | def show(self, frame_rate: int = 30, blocking: bool = False): 48 | """Displays the plot.""" 49 | self.ani = animation.FuncAnimation( 50 | self.fig, 51 | self._update, 52 | interval=1000 // frame_rate, 53 | init_func=self._init, 54 | blit=False, 55 | ) 56 | plt.show(block=blocking) 57 | 58 | def refresh(self): 59 | """Allows the GUI to update.""" 60 | self.fig.canvas.draw() 61 | self.fig.canvas.start_event_loop(0.0001) 62 | 63 | def _init(self): 64 | return self.elements 65 | 66 | def _update(self, iteration): 67 | self.iteration = iteration 68 | if self.update_fn is not None: 69 | self.update_fn() # pylint: disable=not-callable 70 | return self.elements 71 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/utils/registration.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Helper methods for Gym environment registration.""" 16 | 17 | import logging 18 | 19 | from gym.envs import registration as gym_reg 20 | 21 | 22 | def register(env_id: str, class_path: str, **kwargs): 23 | """Registers the given class path as a Gym environment. 24 | 25 | Args: 26 | env_id: The ID to register the environment as. 27 | class_path: The fully-qualified class path of the environment. 28 | **kwargs: Key-word arguments to pass to gym's register function. 29 | """ 30 | if env_id in gym_reg.registry.env_specs: 31 | # This may happen during test discovery. 32 | logging.warning('Re-registering environment %s', env_id) 33 | del gym_reg.registry.env_specs[env_id] 34 | 35 | gym_reg.register(env_id, entry_point=class_path, **kwargs) 36 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/utils/reset_procedure.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Manages resetting functionality.""" 16 | 17 | import abc 18 | 19 | from robel.components.builder import ComponentBuilder 20 | 21 | 22 | class ResetProcedure(metaclass=abc.ABCMeta): 23 | """Implements a reset procedure for a robot.""" 24 | 25 | def __init__(self): 26 | """Creates a new reset procedure.""" 27 | 28 | @abc.abstractmethod 29 | def configure_reset_groups(self, builder: ComponentBuilder): 30 | """Configures the component groups needed for reset.""" 31 | 32 | @abc.abstractmethod 33 | def reset(self, **kwargs): 34 | """Performs the reset procedure.""" 35 | 36 | def finish(self): 37 | """Called when the reset is complete.""" 38 | 39 | 40 | class ManualResetProcedure(ResetProcedure): 41 | """Reset procedure that waits for the user to press enter.""" 42 | 43 | def configure_reset_groups(self, builder: ComponentBuilder): 44 | """Configures the component groups needed for reset.""" 45 | 46 | def reset(self, **kwargs): 47 | """Performs the reset procedure.""" 48 | 49 | def finish(self): 50 | """Called when the reset is complete.""" 51 | input('Press Enter to start the episode...') 52 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/utils/resources_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Tests for resources.""" 16 | 17 | from absl.testing import absltest 18 | 19 | from robel.utils.resources import AssetBundle 20 | 21 | 22 | class DummyResources: 23 | """Dummy cache of resources.""" 24 | 25 | def __init__(self, assets): 26 | self.assets = assets 27 | 28 | def get_resource(self, path: str): 29 | return self.assets[path] 30 | 31 | 32 | class TestAssetBundle(absltest.TestCase): 33 | """Unit tests for configurable.""" 34 | 35 | def test_add_mujoco(self): 36 | """Tests adding a MuJoCo file.""" 37 | resources = DummyResources({ 38 | 'a/b/main.xml': '', 39 | 'a/child1.xml': """ 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | """, 48 | 'a/c/hello.stl': 'Hello!', 49 | 'a/c/world.stl': 'World!', 50 | }) 51 | bundle = AssetBundle( 52 | dest_path='test', dry_run=True, resource_fn=resources.get_resource) 53 | 54 | transformed_path = bundle.add_mujoco('a/b/main.xml') 55 | self.assertEqual(transformed_path, 'test/a/b/main.xml') 56 | self.assertDictEqual( 57 | bundle.copied_paths, { 58 | 'a/b/main.xml': 'test/a/b/main.xml', 59 | 'a/child1.xml': 'test/a/child1.xml', 60 | 'a/c/hello.stl': 'test/a/c/hello.stl', 61 | 'a/c/world.stl': 'test/a/c/world.stl', 62 | }) 63 | 64 | 65 | if __name__ == '__main__': 66 | absltest.main() 67 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/utils/testing/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/utils/testing/mock_dynamixel_sdk_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for MockDynamixelSdk.""" 16 | 17 | from absl.testing import absltest 18 | 19 | from robel.utils.testing.mock_dynamixel_sdk import patch_dynamixel 20 | 21 | 22 | class MockDynamixelSdkTest(absltest.TestCase): 23 | """Tests MockDynamixelSdk.""" 24 | 25 | @patch_dynamixel(test=[1, 2, 3, 4]) 26 | def test_port_handler(self, sdk): 27 | handler = sdk.PortHandler('test') 28 | 29 | self.assertTrue(handler.openPort()) 30 | self.assertTrue(handler.setBaudRate(1000000)) 31 | handler.closePort() 32 | 33 | @patch_dynamixel(test=[1, 2, 3, 4], test1=[1, 2]) 34 | def test_port_handler_fault(self, sdk): 35 | with self.assertRaises(ValueError): 36 | sdk.PortHandler('another') 37 | 38 | handler = sdk.PortHandler('test') 39 | 40 | with self.assertRaises(ValueError): 41 | sdk.PortHandler('test') 42 | 43 | handler.faulty = True 44 | self.assertFalse(handler.openPort()) 45 | 46 | with self.assertRaises(ValueError): 47 | sdk.PortHandler('test1') 48 | 49 | self.assertFalse(handler.setBaudRate(1000000)) 50 | self.assertIn('test', sdk.used_ports) 51 | self.assertNotIn('test1', sdk.used_ports) 52 | 53 | @patch_dynamixel(test1=[1, 2, 3, 4], test2=[1, 2, 3, 4, 5, 6]) 54 | def test_port_handler_multi(self, sdk): 55 | handler1 = sdk.PortHandler('test1') 56 | handler2 = sdk.PortHandler('test2') 57 | 58 | self.assertTrue(handler1.openPort()) 59 | self.assertTrue(handler2.openPort()) 60 | 61 | @patch_dynamixel(test=[1]) 62 | def test_packet_handler(self, sdk): 63 | port = sdk.PortHandler('test') 64 | packet = sdk.PacketHandler() 65 | 66 | packet.write1ByteTxRx(port, 1, 64, 2) 67 | self.assertEqual(sdk.devices['test'][1][64], 2) 68 | 69 | @patch_dynamixel(test=[1, 2, 3, 4]) 70 | def test_read_write(self, sdk): 71 | motor_ids = [1, 2, 3, 4] 72 | port = sdk.PortHandler('test') 73 | packet = sdk.PacketHandler() 74 | 75 | self.assertTrue(port.openPort()) 76 | self.assertTrue(port.setBaudRate(1000000)) 77 | 78 | reader = sdk.GroupBulkRead(port, packet) 79 | writer = sdk.GroupSyncWrite(port, packet, 32, 4) 80 | 81 | for mid in motor_ids: 82 | self.assertTrue(reader.addParam(mid, 32, 4)) 83 | 84 | for mid in motor_ids: 85 | self.assertTrue(reader.isAvailable(mid, 32, 4)) 86 | 87 | for mid in motor_ids: 88 | self.assertEqual(reader.getData(mid, 32, 4), 0) 89 | 90 | payload = 12345678 91 | for mid in motor_ids: 92 | self.assertTrue(writer.addParam(mid, payload.to_bytes(4, 'little'))) 93 | 94 | self.assertTrue(writer.txPacket()) 95 | 96 | for mid in motor_ids: 97 | self.assertTrue(reader.getData(mid, 32, 4), payload) 98 | 99 | 100 | if __name__ == '__main__': 101 | absltest.main() 102 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/utils/testing/mock_sim_scene_test.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Unit tests for MockSimScene.""" 16 | 17 | from absl.testing import absltest 18 | import numpy as np 19 | 20 | from robel.utils.testing.mock_sim_scene import MockSimScene 21 | 22 | 23 | class MockSimSceneTest(absltest.TestCase): 24 | """Tests MockSimScene.""" 25 | 26 | def test_defaults(self): 27 | """Tests default initialization of the sim scene.""" 28 | scene = MockSimScene(nq=2) 29 | 30 | # Ensure that properties exist. 31 | self.assertIsNotNone(scene.sim) 32 | self.assertIsNotNone(scene.model) 33 | self.assertIsNotNone(scene.data) 34 | self.assertIsNotNone(scene.step_duration) 35 | self.assertIsNotNone(scene.close) 36 | self.assertIsNotNone(scene.advance) 37 | 38 | np.testing.assert_array_equal(scene.model.actuator_ctrlrange, [(-1, 1), 39 | (-1, 1)]) 40 | 41 | # Check that sizes are consistent. 42 | self.assertEqual(scene.model.nq, 2) 43 | self.assertEqual(scene.model.nv, 2) 44 | self.assertEqual(scene.model.nu, 2) 45 | self.assertEqual(scene.data.qpos.size, 2) 46 | self.assertEqual(scene.data.qvel.size, 2) 47 | self.assertEqual(scene.data.qacc.size, 2) 48 | self.assertEqual(scene.data.ctrl.size, 2) 49 | 50 | def test_explicit_init(self): 51 | """Tests initialization with explicit values.""" 52 | scene = MockSimScene( 53 | nq=2, 54 | nv=3, 55 | nu=4, 56 | ctrl_range=(-2, 2), 57 | body_names=['test0'], 58 | geom_names=['test0', 'test1'], 59 | site_names=['test0', 'test1', 'test2'], 60 | cam_names=['cam0'], 61 | step_duration=0.5) 62 | 63 | self.assertEqual(scene.data.qpos.size, 2) 64 | self.assertEqual(scene.data.qvel.size, 3) 65 | self.assertEqual(scene.data.qacc.size, 3) 66 | self.assertEqual(scene.data.ctrl.size, 4) 67 | 68 | np.testing.assert_array_equal(scene.model.actuator_ctrlrange, 69 | [(-2, 2)] * 4) 70 | np.testing.assert_array_equal(scene.model.body_pos, np.zeros((1, 3))) 71 | np.testing.assert_array_equal(scene.model.geom_pos, np.zeros((2, 3))) 72 | np.testing.assert_array_equal(scene.model.site_pos, np.zeros((3, 3))) 73 | np.testing.assert_array_equal(scene.model.cam_pos, np.zeros((1, 3))) 74 | 75 | self.assertEqual(scene.model.body_name2id('test0'), 0) 76 | self.assertEqual(scene.model.geom_name2id('test1'), 1) 77 | self.assertEqual(scene.model.site_name2id('test2'), 2) 78 | self.assertEqual(scene.model.camera_name2id('cam0'), 0) 79 | 80 | def test_render_offscreen(self): 81 | """Tests mock rendering.""" 82 | scene = MockSimScene(nq=1) 83 | image = scene.renderer.render_offscreen(16, 16) 84 | self.assertEqual(image.shape, (16, 16)) 85 | 86 | 87 | if __name__ == '__main__': 88 | absltest.main() 89 | -------------------------------------------------------------------------------- /robel_claw/robel/robel/utils/testing/mock_time.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Utilities to mock time-related methods.""" 16 | 17 | from absl.testing.absltest import mock 18 | 19 | 20 | class MockTime: 21 | """Class to mock the functionality of the time module.""" 22 | 23 | def __init__(self, initial_time: float = 0.0): 24 | self._time = initial_time 25 | 26 | def time(self) -> float: 27 | return self._time 28 | 29 | def sleep(self, duration: float): 30 | self._time += duration 31 | 32 | 33 | def patch_time(module_path: str, **kwargs): 34 | return mock.patch(module_path, MockTime(**kwargs)) 35 | -------------------------------------------------------------------------------- /robel_claw/robel/setup.cfg: -------------------------------------------------------------------------------- 1 | [pytype] 2 | inputs = adept_envs 3 | python_version = 3.5 4 | pythonpath = . 5 | 6 | -------------------------------------------------------------------------------- /robel_claw/robel/setup.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 The ROBEL Authors. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Installs robel. 16 | 17 | To install: 18 | pip install -e . 19 | """ 20 | 21 | import fnmatch 22 | import os 23 | 24 | import setuptools 25 | 26 | with open("README.md", "r") as fh: 27 | long_description = fh.read() 28 | 29 | 30 | def get_requirements(file_name): 31 | """Returns requirements from the given file.""" 32 | file_path = os.path.join(os.path.dirname(__file__), file_name) 33 | with open(file_path, 'r') as f: 34 | return [line.strip() for line in f] 35 | 36 | 37 | def get_data_files(package_dir, patterns): 38 | """Returns filepaths matching the given pattern.""" 39 | paths = set() 40 | for directory, _, filenames in os.walk(package_dir): 41 | for pattern in patterns: 42 | for filename in fnmatch.filter(filenames, pattern): 43 | base_path = os.path.relpath(directory, package_dir) 44 | paths.add(os.path.join(base_path, filename)) 45 | return list(paths) 46 | 47 | 48 | setuptools.setup( 49 | name="robel", 50 | version="0.1.2", 51 | license='Apache 2.0', 52 | description=('Robotics reinforcement learning benchmark tasks with ' 53 | 'cost-effective robots.'), 54 | long_description=long_description, 55 | long_description_content_type="text/markdown", 56 | packages=setuptools.find_packages(), 57 | package_data={ 58 | 'robel': get_data_files('robel', 59 | ['*.xml', '*.pkl', '*.stl', '*.png']), 60 | }, 61 | data_files=[ 62 | ('', ['requirements.txt', 'requirements.dev.txt']), 63 | ], 64 | install_requires=get_requirements('requirements.txt'), 65 | extra_requires={ 66 | 'dev': get_requirements('requirements.dev.txt'), 67 | }, 68 | tests_require=['absl-py'], 69 | python_requires='>=3.5.3', 70 | classifiers=[ 71 | 'Programming Language :: Python :: 3', 72 | 'License :: OSI Approved :: Apache Software License', 73 | 'Topic :: Scientific/Engineering :: Artificial Intelligence', 74 | 'Intended Audience :: Science/Research', 75 | ], 76 | ) 77 | --------------------------------------------------------------------------------