diff --git a/src/LyceumMuJoCo.jl b/src/LyceumMuJoCo.jl index d1cc78f..ac0ec4f 100644 --- a/src/LyceumMuJoCo.jl +++ b/src/LyceumMuJoCo.jl @@ -13,6 +13,7 @@ using StaticArrays using Distributions using Reexport using Distances +using Rotations using DocStringExtensions # Lyceum @@ -152,6 +153,7 @@ provided by LyceumMuJoCoViz. #### include("suite/pointmass.jl") +include("suite/adroit/penhand.jl") include("suite/armhand/armhandpickup.jl") include("gym/swimmer-v2.jl") diff --git a/src/suite/adroit/mj_envs/.gitignore b/src/suite/adroit/mj_envs/.gitignore new file mode 100644 index 0000000..e0f4f58 --- /dev/null +++ b/src/suite/adroit/mj_envs/.gitignore @@ -0,0 +1,106 @@ +*.DS_Store + +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +.hypothesis/ +.pytest_cache/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# pyenv +.python-version + +# celery beat schedule file +celerybeat-schedule + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ diff --git a/src/suite/adroit/mj_envs/LICENSE b/src/suite/adroit/mj_envs/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/src/suite/adroit/mj_envs/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/suite/adroit/mj_envs/README.md b/src/suite/adroit/mj_envs/README.md new file mode 100644 index 0000000..04976a8 --- /dev/null +++ b/src/suite/adroit/mj_envs/README.md @@ -0,0 +1 @@ +Fork of [vikashplus/mj_envs](https://github.com/vikashplus/mj_envs), changed to use git subtree's instead of submodules (for Julia Pkg compatability). diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/.gitignore b/src/suite/adroit/mj_envs/dependencies/Adroit/.gitignore new file mode 100644 index 0000000..5509140 --- /dev/null +++ b/src/suite/adroit/mj_envs/dependencies/Adroit/.gitignore @@ -0,0 +1 @@ +*.DS_Store diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/Adroit_hand.xml b/src/suite/adroit/mj_envs/dependencies/Adroit/Adroit_hand.xml new file mode 100644 index 0000000..d5a2a13 --- /dev/null +++ b/src/suite/adroit/mj_envs/dependencies/Adroit/Adroit_hand.xml @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/Adroit_hand_withOverlay.xml b/src/suite/adroit/mj_envs/dependencies/Adroit/Adroit_hand_withOverlay.xml new file mode 100644 index 0000000..ab87c68 --- /dev/null +++ b/src/suite/adroit/mj_envs/dependencies/Adroit/Adroit_hand_withOverlay.xml @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/LICENSE b/src/suite/adroit/mj_envs/dependencies/Adroit/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/src/suite/adroit/mj_envs/dependencies/Adroit/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/README.md b/src/suite/adroit/mj_envs/dependencies/Adroit/README.md new file mode 100644 index 0000000..ad5451c --- /dev/null +++ b/src/suite/adroit/mj_envs/dependencies/Adroit/README.md @@ -0,0 +1,29 @@ +# Adroit Manipulation Platform + +Adroit manipulation platform is reconfigurable, tendon-driven, pneumatically-actuated platform designed and developed by [Vikash Kumar](https://vikashplus.github.io/) during this Ph.D. ([Thesis: Manipulators and Manipulation in high dimensional spaces](https://digital.lib.washington.edu/researchworks/handle/1773/38104)) to study dynamic dexterous manipulation. Adroit is comprised of the [Shadow Hand](https://www.shadowrobot.com/products/dexterous-hand/) skeleton (developed by [Shadow Robot company](https://www.shadowrobot.com/)) and a custom arm, and is powered by a custom actuation sysem. This custom actuation system allows Adroit to move the ShadowHand skeleton faster than a human hand (70 msec limit-to-limit movement, 30 msec overall reflex latency), generate sufficient forces (40 N at each finger tendon, 125N at each wrist tendon), and achieve high compliance on the mechanism level (6 grams of external force at the fingertip displaces the finger when the system is powered.) This combination of speed, force, and compliance is a prerequisite for dexterous manipulation, yet it has never before been achieved with a tendon-driven system, let alone a system with 24 degrees of freedom and 40 tendons. + +## Mujoco Model +Adroit is a 28 degree of freedom system which consists of a 24 degrees of freedom **ShadowHand** and a 4 degree of freedom arm. This repository contains the Mujoco Models of the system developed with extreme care and great attention to the details. + + +## In Projects +Adroit has been used in a wide variety of project. A small list is appended below. Details of these projects can be found [here](https://vikashplus.github.io/). +[![projects](https://github.com/vikashplus/Adroit/blob/master/gallery/projects.JPG)](https://vikashplus.github.io/) +## In News and Media +Adroit has found quite some attention in the world media. Details can be found [here](https://vikashplus.github.io/news.html) + +[![News](https://github.com/vikashplus/Adroit/blob/master/gallery/news.JPG)](https://vikashplus.github.io/news.html) + + +## Citation +If the contents of this repo helped you, please consider citing + +``` +@phdthesis{Kumar2016thesis, + title = {Manipulators and Manipulation in high dimensional spaces}, + school = {University of Washington, Seattle}, + author = {Kumar, Vikash}, + year = {2016}, + url = {https://digital.lib.washington.edu/researchworks/handle/1773/38104} +} +``` diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/gallery/news.JPG b/src/suite/adroit/mj_envs/dependencies/Adroit/gallery/news.JPG new file mode 100644 index 0000000..e53b317 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/gallery/news.JPG differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/gallery/projects.JPG b/src/suite/adroit/mj_envs/dependencies/Adroit/gallery/projects.JPG new file mode 100644 index 0000000..1360a90 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/gallery/projects.JPG differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/assets.xml b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/assets.xml new file mode 100644 index 0000000..c2a85eb --- /dev/null +++ b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/assets.xml @@ -0,0 +1,345 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/chain.xml b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/chain.xml new file mode 100644 index 0000000..ba2353a --- /dev/null +++ b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/chain.xml @@ -0,0 +1,226 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/chain1.xml b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/chain1.xml new file mode 100644 index 0000000..7db7a10 --- /dev/null +++ b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/chain1.xml @@ -0,0 +1,227 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/joint_position_actuation.xml b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/joint_position_actuation.xml new file mode 100644 index 0000000..0a17b5c --- /dev/null +++ b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/joint_position_actuation.xml @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/F1.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/F1.stl new file mode 100644 index 0000000..515d3c9 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/F1.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/F2.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/F2.stl new file mode 100644 index 0000000..7bc5e20 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/F2.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/F3.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/F3.stl new file mode 100644 index 0000000..223f06f Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/F3.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/TH1_z.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/TH1_z.stl new file mode 100644 index 0000000..400ee2d Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/TH1_z.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/TH2_z.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/TH2_z.stl new file mode 100644 index 0000000..5ace838 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/TH2_z.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/TH3_z.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/TH3_z.stl new file mode 100644 index 0000000..23485ab Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/TH3_z.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/arm_base.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/arm_base.stl new file mode 100644 index 0000000..d9a26d5 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/arm_base.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/arm_trunk.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/arm_trunk.stl new file mode 100644 index 0000000..fb2ee17 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/arm_trunk.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/arm_trunk_asmbly.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/arm_trunk_asmbly.stl new file mode 100644 index 0000000..bafae4e Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/arm_trunk_asmbly.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/distal_ellipsoid.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/distal_ellipsoid.stl new file mode 100644 index 0000000..8906519 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/distal_ellipsoid.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/elbow_flex.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/elbow_flex.stl new file mode 100644 index 0000000..b004e74 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/elbow_flex.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/elbow_rotate_motor.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/elbow_rotate_motor.stl new file mode 100644 index 0000000..4e849bb Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/elbow_rotate_motor.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/elbow_rotate_muscle.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/elbow_rotate_muscle.stl new file mode 100644 index 0000000..ee6470c Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/elbow_rotate_muscle.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_Cy_PlateAsmbly(muscle_cone).stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_Cy_PlateAsmbly(muscle_cone).stl new file mode 100644 index 0000000..6ce05b9 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_Cy_PlateAsmbly(muscle_cone).stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_Cy_PlateAsmbly.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_Cy_PlateAsmbly.stl new file mode 100644 index 0000000..0945410 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_Cy_PlateAsmbly.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_PlateAsmbly.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_PlateAsmbly.stl new file mode 100644 index 0000000..85491bb Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_PlateAsmbly.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_electric.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_electric.stl new file mode 100644 index 0000000..80f6f3d Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_electric.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_electric_cvx.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_electric_cvx.stl new file mode 100644 index 0000000..3c30f57 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_electric_cvx.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_muscle.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_muscle.stl new file mode 100644 index 0000000..c47c510 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_muscle.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_simple.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_simple.stl new file mode 100644 index 0000000..888d2d3 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_simple.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_simple_cvx.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_simple_cvx.stl new file mode 100644 index 0000000..1f133ca Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_simple_cvx.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_weight.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_weight.stl new file mode 100644 index 0000000..515050a Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/forearm_weight.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/knuckle.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/knuckle.stl new file mode 100644 index 0000000..4faedd7 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/knuckle.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/lfmetacarpal.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/lfmetacarpal.stl new file mode 100644 index 0000000..535cf4d Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/lfmetacarpal.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/palm.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/palm.stl new file mode 100644 index 0000000..65e47eb Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/palm.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/upper_arm.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/upper_arm.stl new file mode 100644 index 0000000..5045e82 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/upper_arm.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/upper_arm_asmbl_shoulder.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/upper_arm_asmbl_shoulder.stl new file mode 100644 index 0000000..e6ffe6d Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/upper_arm_asmbl_shoulder.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/upper_arm_ass.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/upper_arm_ass.stl new file mode 100644 index 0000000..cbf0b74 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/upper_arm_ass.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/wrist.stl b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/wrist.stl new file mode 100644 index 0000000..420d5f9 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/meshes/wrist.stl differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/tendon_torque_actuation.xml b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/tendon_torque_actuation.xml new file mode 100644 index 0000000..1572f74 --- /dev/null +++ b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/tendon_torque_actuation.xml @@ -0,0 +1,123 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/darkwood.png b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/darkwood.png new file mode 100644 index 0000000..d5dcc5c Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/darkwood.png differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/dice.png b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/dice.png new file mode 100644 index 0000000..798a8e0 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/dice.png differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/foil.png b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/foil.png new file mode 100644 index 0000000..654cfe1 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/foil.png differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/marble.png b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/marble.png new file mode 100644 index 0000000..c50e8b9 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/marble.png differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/silverRaw.png b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/silverRaw.png new file mode 100644 index 0000000..13690e5 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/silverRaw.png differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/skin.png b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/skin.png new file mode 100644 index 0000000..54e528d Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/skin.png differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/square.png b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/square.png new file mode 100644 index 0000000..dbfd695 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/square.png differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/wood.png b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/wood.png new file mode 100644 index 0000000..c323cb9 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/wood.png differ diff --git a/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/woodb.png b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/woodb.png new file mode 100644 index 0000000..47f94a8 Binary files /dev/null and b/src/suite/adroit/mj_envs/dependencies/Adroit/resources/textures/woodb.png differ diff --git a/src/suite/adroit/mj_envs/mj_envs/__init__.py b/src/suite/adroit/mj_envs/mj_envs/__init__.py new file mode 100644 index 0000000..6b66789 --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/__init__.py @@ -0,0 +1 @@ +import mj_envs.hand_manipulation_suite \ No newline at end of file diff --git a/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/__init__.py b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/__init__.py new file mode 100644 index 0000000..913caf9 --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/__init__.py @@ -0,0 +1,34 @@ +from gym.envs.registration import register +from mjrl.envs.mujoco_env import MujocoEnv + +# Swing the door open +register( + id='door-v0', + entry_point='mj_envs.hand_manipulation_suite:DoorEnvV0', + max_episode_steps=200, +) +from mj_envs.hand_manipulation_suite.door_v0 import DoorEnvV0 + +# Hammer a nail into the board +register( + id='hammer-v0', + entry_point='mj_envs.hand_manipulation_suite:HammerEnvV0', + max_episode_steps=200, +) +from mj_envs.hand_manipulation_suite.hammer_v0 import HammerEnvV0 + +# Reposition a pen in hand +register( + id='pen-v0', + entry_point='mj_envs.hand_manipulation_suite:PenEnvV0', + max_episode_steps=100, +) +from mj_envs.hand_manipulation_suite.pen_v0 import PenEnvV0 + +# Relcoate an object to the target +register( + id='relocate-v0', + entry_point='mj_envs.hand_manipulation_suite:RelocateEnvV0', + max_episode_steps=200, +) +from mj_envs.hand_manipulation_suite.relocate_v0 import RelocateEnvV0 diff --git a/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_Adroit.xml b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_Adroit.xml new file mode 100644 index 0000000..3d00adc --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_Adroit.xml @@ -0,0 +1,170 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_assets.xml b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_assets.xml new file mode 100644 index 0000000..b4402be --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_assets.xml @@ -0,0 +1,344 @@ + + + \ No newline at end of file diff --git a/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_door.xml b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_door.xml new file mode 100644 index 0000000..831ff31 --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_door.xml @@ -0,0 +1,92 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_hammer.xml b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_hammer.xml new file mode 100644 index 0000000..f15ae28 --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_hammer.xml @@ -0,0 +1,111 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_pen.xml b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_pen.xml new file mode 100644 index 0000000..2abab7a --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_pen.xml @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_relocate.xml b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_relocate.xml new file mode 100644 index 0000000..8f2e10a --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_relocate.xml @@ -0,0 +1,88 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/tasks.jpg b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/tasks.jpg new file mode 100644 index 0000000..4b1499d Binary files /dev/null and b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/assets/tasks.jpg differ diff --git a/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/door_v0.py b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/door_v0.py new file mode 100644 index 0000000..f429ddd --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/door_v0.py @@ -0,0 +1,124 @@ +import numpy as np +from gym import utils +from mjrl.envs import mujoco_env +from mujoco_py import MjViewer +import os + +ADD_BONUS_REWARDS = True + +class DoorEnvV0(mujoco_env.MujocoEnv, utils.EzPickle): + def __init__(self): + self.door_hinge_did = 0 + self.door_bid = 0 + self.grasp_sid = 0 + self.handle_sid = 0 + curr_dir = os.path.dirname(os.path.abspath(__file__)) + mujoco_env.MujocoEnv.__init__(self, curr_dir+'/assets/DAPG_door.xml', 5) + + # change actuator sensitivity + self.sim.model.actuator_gainprm[self.sim.model.actuator_name2id('A_WRJ1'):self.sim.model.actuator_name2id('A_WRJ0')+1,:3] = np.array([10, 0, 0]) + self.sim.model.actuator_gainprm[self.sim.model.actuator_name2id('A_FFJ3'):self.sim.model.actuator_name2id('A_THJ0')+1,:3] = np.array([1, 0, 0]) + self.sim.model.actuator_biasprm[self.sim.model.actuator_name2id('A_WRJ1'):self.sim.model.actuator_name2id('A_WRJ0')+1,:3] = np.array([0, -10, 0]) + self.sim.model.actuator_biasprm[self.sim.model.actuator_name2id('A_FFJ3'):self.sim.model.actuator_name2id('A_THJ0')+1,:3] = np.array([0, -1, 0]) + + utils.EzPickle.__init__(self) + ob = self.reset_model() + self.act_mid = np.mean(self.model.actuator_ctrlrange, axis=1) + self.act_rng = 0.5*(self.model.actuator_ctrlrange[:,1]-self.model.actuator_ctrlrange[:,0]) + self.door_hinge_did = self.model.jnt_dofadr[self.model.joint_name2id('door_hinge')] + self.grasp_sid = self.model.site_name2id('S_grasp') + self.handle_sid = self.model.site_name2id('S_handle') + self.door_bid = self.model.body_name2id('frame') + + def step(self, a): + a = np.clip(a, -1.0, 1.0) + try: + a = self.act_mid + a*self.act_rng # mean center and scale + except: + a = a # only for the initialization phase + self.do_simulation(a, self.frame_skip) + ob = self.get_obs() + handle_pos = self.data.site_xpos[self.handle_sid].ravel() + palm_pos = self.data.site_xpos[self.grasp_sid].ravel() + door_pos = self.data.qpos[self.door_hinge_did] + + # get to handle + reward = -0.1*np.linalg.norm(palm_pos-handle_pos) + # open door + reward += -0.1*(door_pos - 1.57)*(door_pos - 1.57) + # velocity cost + reward += -1e-5*np.sum(self.data.qvel**2) + + if ADD_BONUS_REWARDS: + # Bonus + if door_pos > 0.2: + reward += 2 + if door_pos > 1.0: + reward += 8 + if door_pos > 1.35: + reward += 10 + + goal_achieved = True if door_pos >= 1.35 else False + + return ob, reward, False, dict(goal_achieved=goal_achieved) + + def get_obs(self): + # qpos for hand + # xpos for obj + # xpos for target + qp = self.data.qpos.ravel() + handle_pos = self.data.site_xpos[self.handle_sid].ravel() + palm_pos = self.data.site_xpos[self.grasp_sid].ravel() + door_pos = np.array([self.data.qpos[self.door_hinge_did]]) + if door_pos > 1.0: + door_open = 1.0 + else: + door_open = -1.0 + latch_pos = qp[-1] + return np.concatenate([qp[1:-2], [latch_pos], door_pos, palm_pos, handle_pos, palm_pos-handle_pos, [door_open]]) + + def reset_model(self): + qp = self.init_qpos.copy() + qv = self.init_qvel.copy() + self.set_state(qp, qv) + + self.model.body_pos[self.door_bid,0] = self.np_random.uniform(low=-0.3, high=-0.2) + self.model.body_pos[self.door_bid,1] = self.np_random.uniform(low=0.25, high=0.35) + self.model.body_pos[self.door_bid,2] = self.np_random.uniform(low=0.252, high=0.35) + self.sim.forward() + return self.get_obs() + + def get_env_state(self): + """ + Get state of hand as well as objects and targets in the scene + """ + qp = self.data.qpos.ravel().copy() + qv = self.data.qvel.ravel().copy() + door_body_pos = self.model.body_pos[self.door_bid].ravel().copy() + return dict(qpos=qp, qvel=qv, door_body_pos=door_body_pos) + + def set_env_state(self, state_dict): + """ + Set the state which includes hand as well as objects and targets in the scene + """ + qp = state_dict['qpos'] + qv = state_dict['qvel'] + self.set_state(qp, qv) + self.model.body_pos[self.door_bid] = state_dict['door_body_pos'] + self.sim.forward() + + def mj_viewer_setup(self): + self.viewer = MjViewer(self.sim) + self.viewer.cam.azimuth = 90 + self.sim.forward() + self.viewer.cam.distance = 1.5 + + def evaluate_success(self, paths): + num_success = 0 + num_paths = len(paths) + # success if door open for 25 steps + for path in paths: + if np.sum(path['env_infos']['goal_achieved']) > 25: + num_success += 1 + success_percentage = num_success*100.0/num_paths + return success_percentage diff --git a/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/hammer_v0.py b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/hammer_v0.py new file mode 100644 index 0000000..9c5278c --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/hammer_v0.py @@ -0,0 +1,128 @@ +import numpy as np +from gym import utils +from mjrl.envs import mujoco_env +from mujoco_py import MjViewer +from mj_envs.utils.quatmath import * +import os + +ADD_BONUS_REWARDS = True + +class HammerEnvV0(mujoco_env.MujocoEnv, utils.EzPickle): + def __init__(self): + self.target_obj_sid = -1 + self.S_grasp_sid = -1 + self.obj_bid = -1 + self.tool_sid = -1 + self.goal_sid = -1 + curr_dir = os.path.dirname(os.path.abspath(__file__)) + mujoco_env.MujocoEnv.__init__(self, curr_dir+'/assets/DAPG_hammer.xml', 5) + utils.EzPickle.__init__(self) + + # change actuator sensitivity + self.sim.model.actuator_gainprm[self.sim.model.actuator_name2id('A_WRJ1'):self.sim.model.actuator_name2id('A_WRJ0')+1,:3] = np.array([10, 0, 0]) + self.sim.model.actuator_gainprm[self.sim.model.actuator_name2id('A_FFJ3'):self.sim.model.actuator_name2id('A_THJ0')+1,:3] = np.array([1, 0, 0]) + self.sim.model.actuator_biasprm[self.sim.model.actuator_name2id('A_WRJ1'):self.sim.model.actuator_name2id('A_WRJ0')+1,:3] = np.array([0, -10, 0]) + self.sim.model.actuator_biasprm[self.sim.model.actuator_name2id('A_FFJ3'):self.sim.model.actuator_name2id('A_THJ0')+1,:3] = np.array([0, -1, 0]) + + self.target_obj_sid = self.sim.model.site_name2id('S_target') + self.S_grasp_sid = self.sim.model.site_name2id('S_grasp') + self.obj_bid = self.sim.model.body_name2id('Object') + self.tool_sid = self.sim.model.site_name2id('tool') + self.goal_sid = self.sim.model.site_name2id('nail_goal') + self.act_mid = np.mean(self.model.actuator_ctrlrange, axis=1) + self.act_rng = 0.5 * (self.model.actuator_ctrlrange[:, 1] - self.model.actuator_ctrlrange[:, 0]) + + def step(self, a): + a = np.clip(a, -1.0, 1.0) + try: + a = self.act_mid + a * self.act_rng # mean center and scale + except: + a = a # only for the initialization phase + self.do_simulation(a, self.frame_skip) + ob = self.get_obs() + obj_pos = self.data.body_xpos[self.obj_bid].ravel() + palm_pos = self.data.site_xpos[self.S_grasp_sid].ravel() + tool_pos = self.data.site_xpos[self.tool_sid].ravel() + target_pos = self.data.site_xpos[self.target_obj_sid].ravel() + goal_pos = self.data.site_xpos[self.goal_sid].ravel() + + # get to hammer + reward = - 0.1 * np.linalg.norm(palm_pos - obj_pos) + # take hammer head to nail + reward -= np.linalg.norm((tool_pos - target_pos)) + # make nail go inside + reward -= 10 * np.linalg.norm(target_pos - goal_pos) + # velocity penalty + reward -= 1e-2 * np.linalg.norm(self.data.qvel.ravel()) + + if ADD_BONUS_REWARDS: + # bonus for lifting up the hammer + if obj_pos[2] > 0.04 and tool_pos[2] > 0.04: + reward += 2 + + # bonus for hammering the nail + if (np.linalg.norm(target_pos - goal_pos) < 0.020): + reward += 25 + if (np.linalg.norm(target_pos - goal_pos) < 0.010): + reward += 75 + + goal_achieved = True if np.linalg.norm(target_pos - goal_pos) < 0.010 else False + + return ob, reward, False, dict(goal_achieved=goal_achieved) + + def get_obs(self): + # qpos for hand + # xpos for obj + # xpos for target + qp = self.data.qpos.ravel() + qv = np.clip(self.data.qvel.ravel(), -1.0, 1.0) + obj_pos = self.data.body_xpos[self.obj_bid].ravel() + obj_rot = quat2euler(self.data.body_xquat[self.obj_bid].ravel()).ravel() + palm_pos = self.data.site_xpos[self.S_grasp_sid].ravel() + target_pos = self.data.site_xpos[self.target_obj_sid].ravel() + nail_impact = np.clip(self.sim.data.sensordata[self.sim.model.sensor_name2id('S_nail')], -1.0, 1.0) + return np.concatenate([qp[:-6], qv[-6:], palm_pos, obj_pos, obj_rot, target_pos, np.array([nail_impact])]) + + def reset_model(self): + self.sim.reset() + target_bid = self.model.body_name2id('nail_board') + self.model.body_pos[target_bid,2] = self.np_random.uniform(low=0.1, high=0.25) + self.sim.forward() + return self.get_obs() + + def get_env_state(self): + """ + Get state of hand as well as objects and targets in the scene + """ + qpos = self.data.qpos.ravel().copy() + qvel = self.data.qvel.ravel().copy() + board_pos = self.model.body_pos[self.model.body_name2id('nail_board')].copy() + target_pos = self.data.site_xpos[self.target_obj_sid].ravel().copy() + return dict(qpos=qpos, qvel=qvel, board_pos=board_pos, target_pos=target_pos) + + def set_env_state(self, state_dict): + """ + Set the state which includes hand as well as objects and targets in the scene + """ + qp = state_dict['qpos'] + qv = state_dict['qvel'] + board_pos = state_dict['board_pos'] + self.set_state(qp, qv) + self.model.body_pos[self.model.body_name2id('nail_board')] = board_pos + self.sim.forward() + + def mj_viewer_setup(self): + self.viewer = MjViewer(self.sim) + self.viewer.cam.azimuth = 45 + self.viewer.cam.distance = 2.0 + self.sim.forward() + + def evaluate_success(self, paths): + num_success = 0 + num_paths = len(paths) + # success if nail insude board for 25 steps + for path in paths: + if np.sum(path['env_infos']['goal_achieved']) > 25: + num_success += 1 + success_percentage = num_success*100.0/num_paths + return success_percentage diff --git a/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/pen_v0.py b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/pen_v0.py new file mode 100644 index 0000000..8d45ec7 --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/pen_v0.py @@ -0,0 +1,142 @@ +import numpy as np +from gym import utils +from mjrl.envs import mujoco_env +from mj_envs.utils.quatmath import quat2euler, euler2quat +from mujoco_py import MjViewer +import os + +ADD_BONUS_REWARDS = True + +class PenEnvV0(mujoco_env.MujocoEnv, utils.EzPickle): + def __init__(self): + self.target_obj_bid = 0 + self.S_grasp_sid = 0 + self.eps_ball_sid = 0 + self.obj_bid = 0 + self.obj_t_sid = 0 + self.obj_b_sid = 0 + self.tar_t_sid = 0 + self.tar_b_sid = 0 + self.pen_length = 1.0 + self.tar_length = 1.0 + + curr_dir = os.path.dirname(os.path.abspath(__file__)) + mujoco_env.MujocoEnv.__init__(self, curr_dir+'/assets/DAPG_pen.xml', 5) + + # change actuator sensitivity + self.sim.model.actuator_gainprm[self.sim.model.actuator_name2id('A_WRJ1'):self.sim.model.actuator_name2id('A_WRJ0')+1,:3] = np.array([10, 0, 0]) + self.sim.model.actuator_gainprm[self.sim.model.actuator_name2id('A_FFJ3'):self.sim.model.actuator_name2id('A_THJ0')+1,:3] = np.array([1, 0, 0]) + self.sim.model.actuator_biasprm[self.sim.model.actuator_name2id('A_WRJ1'):self.sim.model.actuator_name2id('A_WRJ0')+1,:3] = np.array([0, -10, 0]) + self.sim.model.actuator_biasprm[self.sim.model.actuator_name2id('A_FFJ3'):self.sim.model.actuator_name2id('A_THJ0')+1,:3] = np.array([0, -1, 0]) + + utils.EzPickle.__init__(self) + self.target_obj_bid = self.sim.model.body_name2id("target") + self.S_grasp_sid = self.sim.model.site_name2id('S_grasp') + self.obj_bid = self.sim.model.body_name2id('Object') + self.eps_ball_sid = self.sim.model.site_name2id('eps_ball') + self.obj_t_sid = self.sim.model.site_name2id('object_top') + self.obj_b_sid = self.sim.model.site_name2id('object_bottom') + self.tar_t_sid = self.sim.model.site_name2id('target_top') + self.tar_b_sid = self.sim.model.site_name2id('target_bottom') + + self.pen_length = np.linalg.norm(self.data.site_xpos[self.obj_t_sid] - self.data.site_xpos[self.obj_b_sid]) + self.tar_length = np.linalg.norm(self.data.site_xpos[self.tar_t_sid] - self.data.site_xpos[self.tar_b_sid]) + + self.act_mid = np.mean(self.model.actuator_ctrlrange, axis=1) + self.act_rng = 0.5*(self.model.actuator_ctrlrange[:,1]-self.model.actuator_ctrlrange[:,0]) + + def step(self, a): + a = np.clip(a, -1.0, 1.0) + try: + starting_up = False + a = self.act_mid + a*self.act_rng # mean center and scale + except: + starting_up = True + a = a # only for the initialization phase + self.do_simulation(a, self.frame_skip) + + obj_pos = self.data.body_xpos[self.obj_bid].ravel() + desired_loc = self.data.site_xpos[self.eps_ball_sid].ravel() + obj_orien = (self.data.site_xpos[self.obj_t_sid] - self.data.site_xpos[self.obj_b_sid])/self.pen_length + desired_orien = (self.data.site_xpos[self.tar_t_sid] - self.data.site_xpos[self.tar_b_sid])/self.tar_length + + # pos cost + dist = np.linalg.norm(obj_pos-desired_loc) + reward = -dist + # orien cost + orien_similarity = np.dot(obj_orien, desired_orien) + reward += orien_similarity + + if ADD_BONUS_REWARDS: + # bonus for being close to desired orientation + if dist < 0.075 and orien_similarity > 0.9: + reward += 10 + if dist < 0.075 and orien_similarity > 0.95: + reward += 50 + + # penalty for dropping the pen + done = False + if obj_pos[2] < 0.075: + reward -= 5 + done = True if not starting_up else False + + goal_achieved = True if (dist < 0.075 and orien_similarity > 0.95) else False + + return self.get_obs(), reward, done, dict(goal_achieved=goal_achieved) + + def get_obs(self): + qp = self.data.qpos.ravel() + obj_vel = self.data.qvel[-6:].ravel() + obj_pos = self.data.body_xpos[self.obj_bid].ravel() + desired_pos = self.data.site_xpos[self.eps_ball_sid].ravel() + obj_orien = (self.data.site_xpos[self.obj_t_sid] - self.data.site_xpos[self.obj_b_sid])/self.pen_length + desired_orien = (self.data.site_xpos[self.tar_t_sid] - self.data.site_xpos[self.tar_b_sid])/self.tar_length + return np.concatenate([qp[:-6], obj_pos, obj_vel, obj_orien, desired_orien, + obj_pos-desired_pos, obj_orien-desired_orien]) + + def reset_model(self): + qp = self.init_qpos.copy() + qv = self.init_qvel.copy() + self.set_state(qp, qv) + desired_orien = np.zeros(3) + desired_orien[0] = self.np_random.uniform(low=-1, high=1) + desired_orien[1] = self.np_random.uniform(low=-1, high=1) + self.model.body_quat[self.target_obj_bid] = euler2quat(desired_orien) + self.sim.forward() + return self.get_obs() + + def get_env_state(self): + """ + Get state of hand as well as objects and targets in the scene + """ + qp = self.data.qpos.ravel().copy() + qv = self.data.qvel.ravel().copy() + desired_orien = self.model.body_quat[self.target_obj_bid].ravel().copy() + return dict(qpos=qp, qvel=qv, desired_orien=desired_orien) + + def set_env_state(self, state_dict): + """ + Set the state which includes hand as well as objects and targets in the scene + """ + qp = state_dict['qpos'] + qv = state_dict['qvel'] + desired_orien = state_dict['desired_orien'] + self.set_state(qp, qv) + self.model.body_quat[self.target_obj_bid] = desired_orien + self.sim.forward() + + def mj_viewer_setup(self): + self.viewer = MjViewer(self.sim) + self.viewer.cam.azimuth = -45 + self.sim.forward() + self.viewer.cam.distance = 1.0 + + def evaluate_success(self, paths): + num_success = 0 + num_paths = len(paths) + # success if pen within 15 degrees of target for 20 steps + for path in paths: + if np.sum(path['env_infos']['goal_achieved']) > 20: + num_success += 1 + success_percentage = num_success*100.0/num_paths + return success_percentage diff --git a/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/relocate_v0.py b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/relocate_v0.py new file mode 100644 index 0000000..f419492 --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/hand_manipulation_suite/relocate_v0.py @@ -0,0 +1,120 @@ +import numpy as np +from gym import utils +from mjrl.envs import mujoco_env +from mujoco_py import MjViewer +import os + +ADD_BONUS_REWARDS = True + +class RelocateEnvV0(mujoco_env.MujocoEnv, utils.EzPickle): + def __init__(self): + self.target_obj_sid = 0 + self.S_grasp_sid = 0 + self.obj_bid = 0 + curr_dir = os.path.dirname(os.path.abspath(__file__)) + mujoco_env.MujocoEnv.__init__(self, curr_dir+'/assets/DAPG_relocate.xml', 5) + + # change actuator sensitivity + self.sim.model.actuator_gainprm[self.sim.model.actuator_name2id('A_WRJ1'):self.sim.model.actuator_name2id('A_WRJ0')+1,:3] = np.array([10, 0, 0]) + self.sim.model.actuator_gainprm[self.sim.model.actuator_name2id('A_FFJ3'):self.sim.model.actuator_name2id('A_THJ0')+1,:3] = np.array([1, 0, 0]) + self.sim.model.actuator_biasprm[self.sim.model.actuator_name2id('A_WRJ1'):self.sim.model.actuator_name2id('A_WRJ0')+1,:3] = np.array([0, -10, 0]) + self.sim.model.actuator_biasprm[self.sim.model.actuator_name2id('A_FFJ3'):self.sim.model.actuator_name2id('A_THJ0')+1,:3] = np.array([0, -1, 0]) + + self.target_obj_sid = self.sim.model.site_name2id("target") + self.S_grasp_sid = self.sim.model.site_name2id('S_grasp') + self.obj_bid = self.sim.model.body_name2id('Object') + utils.EzPickle.__init__(self) + self.act_mid = np.mean(self.model.actuator_ctrlrange, axis=1) + self.act_rng = 0.5*(self.model.actuator_ctrlrange[:,1]-self.model.actuator_ctrlrange[:,0]) + + def step(self, a): + a = np.clip(a, -1.0, 1.0) + try: + a = self.act_mid + a*self.act_rng # mean center and scale + except: + a = a # only for the initialization phase + self.do_simulation(a, self.frame_skip) + ob = self.get_obs() + obj_pos = self.data.body_xpos[self.obj_bid].ravel() + palm_pos = self.data.site_xpos[self.S_grasp_sid].ravel() + target_pos = self.data.site_xpos[self.target_obj_sid].ravel() + + reward = -0.1*np.linalg.norm(palm_pos-obj_pos) # take hand to object + if obj_pos[2] > 0.04: # if object off the table + reward += 1.0 # bonus for lifting the object + reward += -0.5*np.linalg.norm(palm_pos-target_pos) # make hand go to target + reward += -0.5*np.linalg.norm(obj_pos-target_pos) # make object go to target + + if ADD_BONUS_REWARDS: + if np.linalg.norm(obj_pos-target_pos) < 0.1: + reward += 10.0 # bonus for object close to target + if np.linalg.norm(obj_pos-target_pos) < 0.05: + reward += 20.0 # bonus for object "very" close to target + + goal_achieved = True if np.linalg.norm(obj_pos-target_pos) < 0.1 else False + + return ob, reward, False, dict(goal_achieved=goal_achieved) + + def get_obs(self): + # qpos for hand + # xpos for obj + # xpos for target + qp = self.data.qpos.ravel() + obj_pos = self.data.body_xpos[self.obj_bid].ravel() + palm_pos = self.data.site_xpos[self.S_grasp_sid].ravel() + target_pos = self.data.site_xpos[self.target_obj_sid].ravel() + return np.concatenate([qp[:-6], palm_pos-obj_pos, palm_pos-target_pos, obj_pos-target_pos]) + + def reset_model(self): + qp = self.init_qpos.copy() + qv = self.init_qvel.copy() + self.set_state(qp, qv) + self.model.body_pos[self.obj_bid,0] = self.np_random.uniform(low=-0.15, high=0.15) + self.model.body_pos[self.obj_bid,1] = self.np_random.uniform(low=-0.15, high=0.3) + self.model.site_pos[self.target_obj_sid, 0] = self.np_random.uniform(low=-0.2, high=0.2) + self.model.site_pos[self.target_obj_sid,1] = self.np_random.uniform(low=-0.2, high=0.2) + self.model.site_pos[self.target_obj_sid,2] = self.np_random.uniform(low=0.15, high=0.35) + self.sim.forward() + return self.get_obs() + + def get_env_state(self): + """ + Get state of hand as well as objects and targets in the scene + """ + qp = self.data.qpos.ravel().copy() + qv = self.data.qvel.ravel().copy() + hand_qpos = qp[:30] + obj_pos = self.data.body_xpos[self.obj_bid].ravel() + palm_pos = self.data.site_xpos[self.S_grasp_sid].ravel() + target_pos = self.data.site_xpos[self.target_obj_sid].ravel() + return dict(hand_qpos=hand_qpos, obj_pos=obj_pos, target_pos=target_pos, palm_pos=palm_pos, + qpos=qp, qvel=qv) + + def set_env_state(self, state_dict): + """ + Set the state which includes hand as well as objects and targets in the scene + """ + qp = state_dict['qpos'] + qv = state_dict['qvel'] + obj_pos = state_dict['obj_pos'] + target_pos = state_dict['target_pos'] + self.set_state(qp, qv) + self.model.body_pos[self.obj_bid] = obj_pos + self.model.site_pos[self.target_obj_sid] = target_pos + self.sim.forward() + + def mj_viewer_setup(self): + self.viewer = MjViewer(self.sim) + self.viewer.cam.azimuth = 90 + self.sim.forward() + self.viewer.cam.distance = 1.5 + + def evaluate_success(self, paths): + num_success = 0 + num_paths = len(paths) + # success if object close to target for 25 steps + for path in paths: + if np.sum(path['env_infos']['goal_achieved']) > 25: + num_success += 1 + success_percentage = num_success*100.0/num_paths + return success_percentage diff --git a/src/suite/adroit/mj_envs/mj_envs/utils/quatmath.py b/src/suite/adroit/mj_envs/mj_envs/utils/quatmath.py new file mode 100644 index 0000000..7fef129 --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/utils/quatmath.py @@ -0,0 +1,164 @@ +import numpy as np +# For testing whether a number is close to zero +_FLOAT_EPS = np.finfo(np.float64).eps +_EPS4 = _FLOAT_EPS * 4.0 + + +def mulQuat(qa, qb): + res = np.zeros(4) + res[0] = qa[0]*qb[0] - qa[1]*qb[1] - qa[2]*qb[2] - qa[3]*qb[3] + res[1] = qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2] + res[2] = qa[0]*qb[2] - qa[1]*qb[3] + qa[2]*qb[0] + qa[3]*qb[1] + res[3] = qa[0]*qb[3] + qa[1]*qb[2] - qa[2]*qb[1] + qa[3]*qb[0] + return res + +def negQuat(quat): + return np.array([quat[0], -quat[1], -quat[2], -quat[3]]) + +def quat2Vel(quat, dt=1): + axis = quat[1:].copy() + sin_a_2 = np.sqrt(np.sum(axis**2)) + axis = axis/(sin_a_2+1e-8) + speed = 2*np.arctan2(sin_a_2, quat[0])/dt + return speed, axis + +def quatDiff2Vel(quat1, quat2, dt): + neg = negQuat(quat1) + diff = mulQuat(quat2, neg) + return quat2Vel(diff, dt) + + +def axis_angle2quat(axis, angle): + c = np.cos(angle/2) + s = np.sin(angle/2) + return np.array([c, s*axis[0], s*axis[1], s*axis[2]]) + +def euler2mat(euler): + """ Convert Euler Angles to Rotation Matrix. See rotation.py for notes """ + euler = np.asarray(euler, dtype=np.float64) + assert euler.shape[-1] == 3, "Invalid shaped euler {}".format(euler) + + ai, aj, ak = -euler[..., 2], -euler[..., 1], -euler[..., 0] + si, sj, sk = np.sin(ai), np.sin(aj), np.sin(ak) + ci, cj, ck = np.cos(ai), np.cos(aj), np.cos(ak) + cc, cs = ci * ck, ci * sk + sc, ss = si * ck, si * sk + + mat = np.empty(euler.shape[:-1] + (3, 3), dtype=np.float64) + mat[..., 2, 2] = cj * ck + mat[..., 2, 1] = sj * sc - cs + mat[..., 2, 0] = sj * cc + ss + mat[..., 1, 2] = cj * sk + mat[..., 1, 1] = sj * ss + cc + mat[..., 1, 0] = sj * cs - sc + mat[..., 0, 2] = -sj + mat[..., 0, 1] = cj * si + mat[..., 0, 0] = cj * ci + return mat + + +def euler2quat(euler): + """ Convert Euler Angles to Quaternions. See rotation.py for notes """ + euler = np.asarray(euler, dtype=np.float64) + assert euler.shape[-1] == 3, "Invalid shape euler {}".format(euler) + + ai, aj, ak = euler[..., 2] / 2, -euler[..., 1] / 2, euler[..., 0] / 2 + si, sj, sk = np.sin(ai), np.sin(aj), np.sin(ak) + ci, cj, ck = np.cos(ai), np.cos(aj), np.cos(ak) + cc, cs = ci * ck, ci * sk + sc, ss = si * ck, si * sk + + quat = np.empty(euler.shape[:-1] + (4,), dtype=np.float64) + quat[..., 0] = cj * cc + sj * ss + quat[..., 3] = cj * sc - sj * cs + quat[..., 2] = -(cj * ss + sj * cc) + quat[..., 1] = cj * cs - sj * sc + return quat + + +def mat2euler(mat): + """ Convert Rotation Matrix to Euler Angles. See rotation.py for notes """ + mat = np.asarray(mat, dtype=np.float64) + assert mat.shape[-2:] == (3, 3), "Invalid shape matrix {}".format(mat) + + cy = np.sqrt(mat[..., 2, 2] * mat[..., 2, 2] + mat[..., 1, 2] * mat[..., 1, 2]) + condition = cy > _EPS4 + euler = np.empty(mat.shape[:-1], dtype=np.float64) + euler[..., 2] = np.where(condition, + -np.arctan2(mat[..., 0, 1], mat[..., 0, 0]), + -np.arctan2(-mat[..., 1, 0], mat[..., 1, 1])) + euler[..., 1] = np.where(condition, + -np.arctan2(-mat[..., 0, 2], cy), + -np.arctan2(-mat[..., 0, 2], cy)) + euler[..., 0] = np.where(condition, + -np.arctan2(mat[..., 1, 2], mat[..., 2, 2]), + 0.0) + return euler + + +def mat2quat(mat): + """ Convert Rotation Matrix to Quaternion. See rotation.py for notes """ + mat = np.asarray(mat, dtype=np.float64) + assert mat.shape[-2:] == (3, 3), "Invalid shape matrix {}".format(mat) + + Qxx, Qyx, Qzx = mat[..., 0, 0], mat[..., 0, 1], mat[..., 0, 2] + Qxy, Qyy, Qzy = mat[..., 1, 0], mat[..., 1, 1], mat[..., 1, 2] + Qxz, Qyz, Qzz = mat[..., 2, 0], mat[..., 2, 1], mat[..., 2, 2] + # Fill only lower half of symmetric matrix + K = np.zeros(mat.shape[:-2] + (4, 4), dtype=np.float64) + K[..., 0, 0] = Qxx - Qyy - Qzz + K[..., 1, 0] = Qyx + Qxy + K[..., 1, 1] = Qyy - Qxx - Qzz + K[..., 2, 0] = Qzx + Qxz + K[..., 2, 1] = Qzy + Qyz + K[..., 2, 2] = Qzz - Qxx - Qyy + K[..., 3, 0] = Qyz - Qzy + K[..., 3, 1] = Qzx - Qxz + K[..., 3, 2] = Qxy - Qyx + K[..., 3, 3] = Qxx + Qyy + Qzz + K /= 3.0 + # TODO: vectorize this -- probably could be made faster + q = np.empty(K.shape[:-2] + (4,)) + it = np.nditer(q[..., 0], flags=['multi_index']) + while not it.finished: + # Use Hermitian eigenvectors, values for speed + vals, vecs = np.linalg.eigh(K[it.multi_index]) + # Select largest eigenvector, reorder to w,x,y,z quaternion + q[it.multi_index] = vecs[[3, 0, 1, 2], np.argmax(vals)] + # Prefer quaternion with positive w + # (q * -1 corresponds to same rotation as q) + if q[it.multi_index][0] < 0: + q[it.multi_index] *= -1 + it.iternext() + return q + + +def quat2euler(quat): + """ Convert Quaternion to Euler Angles. See rotation.py for notes """ + return mat2euler(quat2mat(quat)) + + +def quat2mat(quat): + """ Convert Quaternion to Euler Angles. See rotation.py for notes """ + quat = np.asarray(quat, dtype=np.float64) + assert quat.shape[-1] == 4, "Invalid shape quat {}".format(quat) + + w, x, y, z = quat[..., 0], quat[..., 1], quat[..., 2], quat[..., 3] + Nq = np.sum(quat * quat, axis=-1) + s = 2.0 / Nq + X, Y, Z = x * s, y * s, z * s + wX, wY, wZ = w * X, w * Y, w * Z + xX, xY, xZ = x * X, x * Y, x * Z + yY, yZ, zZ = y * Y, y * Z, z * Z + + mat = np.empty(quat.shape[:-1] + (3, 3), dtype=np.float64) + mat[..., 0, 0] = 1.0 - (yY + zZ) + mat[..., 0, 1] = xY - wZ + mat[..., 0, 2] = xZ + wY + mat[..., 1, 0] = xY + wZ + mat[..., 1, 1] = 1.0 - (xX + zZ) + mat[..., 1, 2] = yZ - wX + mat[..., 2, 0] = xZ - wY + mat[..., 2, 1] = yZ + wX + mat[..., 2, 2] = 1.0 - (xX + yY) + return np.where((Nq > _FLOAT_EPS)[..., np.newaxis, np.newaxis], mat, np.eye(3)) \ No newline at end of file diff --git a/src/suite/adroit/mj_envs/mj_envs/utils/visualize_env.py b/src/suite/adroit/mj_envs/mj_envs/utils/visualize_env.py new file mode 100644 index 0000000..c3ac4f2 --- /dev/null +++ b/src/suite/adroit/mj_envs/mj_envs/utils/visualize_env.py @@ -0,0 +1,38 @@ +import gym +import mj_envs +import click +import os +import gym +import numpy as np +import pickle +from mjrl.utils.gym_env import GymEnv +from mjrl.policies.gaussian_mlp import MLP + +DESC = ''' +Helper script to visualize policy (in mjrl format).\n +USAGE:\n + Visualizes policy on the env\n + $ python visualize_policy.py --env_name door-v0 \n + $ python visualize_policy.py --env_name door-v0 --policy my_policy.pickle --mode evaluation --episodes 10 \n +''' + +# MAIN ========================================================= +@click.command(help=DESC) +@click.option('--env_name', type=str, help='environment to load', required= True) +@click.option('--policy', type=str, help='absolute path of the policy file', default=None) +@click.option('--mode', type=str, help='exploration or evaluation mode for policy', default='evaluation') +@click.option('--seed', type=int, help='seed for generating environment instances', default=123) +@click.option('--episodes', type=int, help='number of episodes to visualize', default=10) + +def main(env_name, policy, mode, seed, episodes): + e = GymEnv(env_name) + e.set_seed(seed) + if policy is not None: + pi = pickle.load(open(policy, 'rb')) + else: + pi = MLP(e.spec, hidden_sizes=(32,32), seed=seed, init_log_std=-1.0) + # render policy + e.visualize_policy(pi, num_episodes=episodes, horizon=e.horizon, mode=mode) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/suite/adroit/mj_envs/setup.py b/src/suite/adroit/mj_envs/setup.py new file mode 100644 index 0000000..22a09bf --- /dev/null +++ b/src/suite/adroit/mj_envs/setup.py @@ -0,0 +1,23 @@ +import os +import sys +from setuptools import setup, find_packages + +if sys.version_info.major != 3: + print("This Python is only compatible with Python 3, but you are running " + "Python {}. The installation will likely fail.".format(sys.version_info.major)) + +def read(fname): + return open(os.path.join(os.path.dirname(__file__), fname)).read() + +setup( + name='mj_envs', + version='1.0.0', + packages=find_packages(), + description='environments simulated in MuJoCo', + long_description=read('README.md'), + url='https://github.com/vikashplus/mj_envs.git', + author='Movement Control Lab, UW', + install_requires=[ + 'click', 'gym==0.13', 'mujoco-py<2.1,>=2.0', 'termcolor', + ], +) \ No newline at end of file diff --git a/src/suite/adroit/penhand.jl b/src/suite/adroit/penhand.jl new file mode 100644 index 0000000..d9a062b --- /dev/null +++ b/src/suite/adroit/penhand.jl @@ -0,0 +1,249 @@ +struct PenHand{SIM<:MJSim,O,S} <: AbstractMuJoCoEnvironment + sim::SIM + osp::O + ssp::S + penlength::Float64 + tarlength::Float64 + act_mid::Vector{Float64} + act_rng::Vector{Float64} + + obj_id::Int + obj_top::Int + obj_bot::Int + trg_top::Int + trg_bot::Int + trg_id::Int + eps_ball::SVector{3, Float64} + + function PenHand(sim::MJSim) + m = sim.m + mn, dn = sim.mn, sim.dn + + osp = MultiShape(qpos = VectorShape(Float64, m.nq-6), + objvel = VectorShape(Float64, 6), + objpos = VectorShape(Float64, 3), + despos = VectorShape(Float64, 3), + objorien = VectorShape(Float64, 3), + desorien = VectorShape(Float64, 3)) + + ssp = MultiShape(statespace(sim), + objquat = VectorShape(Float64, 4)) + + penlength = norm(dn.site_xpos[:, :object_top] - dn.site_xpos[:, :object_bottom]) + tarlength = norm(dn.site_xpos[:, :target_top] - dn.site_xpos[:, :target_bottom]) + + act_mid = vec(mean(m.actuator_ctrlrange, dims=1)) + act_rng = 0.5*(m.actuator_ctrlrange[2,:]-m.actuator_ctrlrange[1,:]) + + obj_id = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_BODY, "Object") + obj_top = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_SITE, "object_top") + obj_bot = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_SITE, "object_bottom") + trg_top = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_SITE, "target_top") + trg_bot = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_SITE, "target_bottom") + trg_id = jl_name2id(sim.m, MuJoCo.MJCore.mjOBJ_BODY, "target") + + eps_ball = SVector{3, Float64}(dn.site_xpos[:, :eps_ball]) + + new{typeof(sim), typeof(osp), typeof(ssp)}(sim, osp, ssp, + penlength, tarlength, + act_mid, act_rng, + obj_id, + obj_top, obj_bot, + trg_top, trg_bot, + trg_id, eps_ball) + end +end + +PenHand() = first(tconstruct(PenHand, 1)) + +function tconstruct(::Type{PenHand}, n::Integer) + modelpath = joinpath(@__DIR__, "mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_pen.xml") + return Tuple(PenHand(s) for s in tconstruct(MJSim, n, modelpath, skip = 5)) +end + +function ensembleconstruct(::Type{PenHand}, n::Integer) + modelpath = joinpath(@__DIR__, "mj_envs/mj_envs/hand_manipulation_suite/assets/DAPG_pen.xml") + return Tuple(PenHand(MJSim(modelpath, skip = 5)) for m=1:n ) +end + + +@inline getsim(env::PenHand) = env.sim + + +@inline statespace(env::PenHand) = env.ssp + +@propagate_inbounds function setstate!(env::PenHand, s) + @boundscheck checkaxes(statespace(env), s) + + @uviews s begin + simstate = view(s, 1:length(statespace(env.sim))) + setstate!(env.sim, simstate) + end + objquat = env.ssp(s).objquat + env.sim.m.body_quat[1, env.trg_id] = objquat[1] + env.sim.m.body_quat[2, env.trg_id] = objquat[2] + env.sim.m.body_quat[3, env.trg_id] = objquat[3] + env.sim.m.body_quat[4, env.trg_id] = objquat[4] + env +end + +@propagate_inbounds function getstate!(s, env::PenHand) + @boundscheck checkaxes(statespace(env), s) + + @uviews s begin + simstate = view(s, 1:length(statespace(env.sim))) + getstate!(simstate, env.sim) + end + #env.ssp(s).objquat .= env.sim.m.body_quat[:, env.trg_id] # TODO this allocates + objquat = env.ssp(s).objquat + objquat[1] = env.sim.m.body_quat[1, env.trg_id] + objquat[2] = env.sim.m.body_quat[2, env.trg_id] + objquat[3] = env.sim.m.body_quat[3, env.trg_id] + objquat[4] = env.sim.m.body_quat[4, env.trg_id] + s +end + + +@inline obsspace(env::PenHand) = env.osp + +@propagate_inbounds function getobs!(o, env::PenHand) + @boundscheck checkaxes(obsspace(env), o) + + m, d = env.sim.m, env.sim.d + osp = obsspace(env) + qpos = d.qpos + qvel = d.qvel + sx = d.site_xpos + xpos = d.xpos + + @uviews o qpos qvel @inbounds begin + shaped = osp(o) + shaped.qpos .= view(qpos, 1:m.nq-6) + shaped.objvel .= view(qvel, (m.nv-5):m.nv) + shaped.objpos .= SPoint3D(xpos, env.obj_id) + shaped.despos .= env.eps_ball + shaped.objorien .= (SPoint3D(sx, env.obj_top) - SPoint3D(sx, env.obj_bot)) / env.penlength + shaped.desorien .= (SPoint3D(sx, env.trg_top) - SPoint3D(sx, env.trg_bot)) / env.tarlength + end + o +end + + +@propagate_inbounds function getaction!(a, env::PenHand) + @boundscheck checkaxes(actionspace(env), a) + + @. a = ( env.sim.d.ctrl - env.act_mid ) / env.act_rng + a +end + +@propagate_inbounds function setaction!(env::PenHand, a) + @boundscheck checkaxes(actionspace(env), a) + + env.sim.d.ctrl .= clamp.(a, -1.0, 1.0) + @. env.sim.d.ctrl = env.act_mid + env.sim.d.ctrl * env.act_rng + #forwardskip!(env.sim, MuJoCo.MJCore.mjSTAGE_ACC, false) + env +end + + +@propagate_inbounds function reset!(env::PenHand) + fastreset_nofwd!(env.sim) + env.sim.m.body_quat[1, env.trg_id] = 1.0 + env.sim.m.body_quat[2, env.trg_id] = 0.0 + env.sim.m.body_quat[3, env.trg_id] = 0.0 + env.sim.m.body_quat[4, env.trg_id] = 0.0 + forward!(env.sim) + env +end + +quatvec(q::Quat) = SVector(q.w, q.x, q.y, q.z) + +@propagate_inbounds function randreset!(rng::Random.AbstractRNG, env::PenHand) + # --- + #reset!(env) + # --- + + # --- + #fastreset_nofwd!(env.sim) + #desorien = SA_F64[rand(rng, Uniform(-1, 1)), rand(rng, Uniform(-1, 1)), 0.0] + #env.sim.m.body_quat[:, env.trg_id] .= _euler2quat(desorien) # TODO change the model + #forward!(env.sim) + #env + # --- + + + # --- + reset_nofwd!(env.sim) + desorien0 = [rand(rng, Uniform(-1, 1)), rand(rng, Uniform(-1, 1)), 0.0] + + r = RotXYZ(desorien0...) + #r = RotZYX(desorien0...) + q = quatvec(Quat(r)) + + #q2 = _euler2quat(desorien0) + #@info ([q...], [q2...]) + #@assert isapprox([q...], [q2...]) + + env.sim.m.body_quat[:, env.trg_id] .= q + forward!(env.sim) + env +end + + +@propagate_inbounds function getreward(::Any, ::Any, obs, env::PenHand) + @boundscheck checkaxes(obsspace(env), obs) + + os = obsspace(env)(obs) + objpos = SPoint3D(os.objpos) + despos = SPoint3D(os.despos) + objorien = SPoint3D(os.objorien) + desorien = SPoint3D(os.desorien) + + dist = norm(objpos - despos) + reward = -dist + similarity = dot(objorien, desorien) + reward += similarity + + # bonus for being close to desired orientation + if dist < 0.075 && similarity > 0.90 + reward += 10 + end + if dist < 0.075 && similarity > 0.95 + reward += 50 + end + + if objpos[3] < 0.075 # this is supposed to happen with the is-done calculation + reward -= 5 + end + + return reward +end + +@propagate_inbounds function geteval(::Any, ::Any, obs, env::PenHand) + @boundscheck checkaxes(obsspace(env), obs) + + os = obsspace(env)(obs) + objorien = SPoint3D(os.objorien) + desorien = SPoint3D(os.desorien) + similarity = dot(objorien, desorien) + + #@info (env.sim.d.xpos[:, env.obj_id], obsspace(env)(obs).objpos) + return similarity +end + + +function isdone(::Any, ::Any, obs, env::PenHand) + shaped = obsspace(env)(obs) + objz = shaped.objpos[3] + return objz < 0.075 +end + + +@propagate_inbounds function _euler2quat(euler) + ai, aj, ak = euler + si, sj, sk = sin(ai), sin(aj), sin(ak) + ci, cj, ck = cos(ai), cos(aj), cos(ak) + cc, cs = ci * ck, ci * sk + sc, ss = si * ck, si * sk + return (cj*cc+sj*ss, cj*sc-sj*cs, -(cj*ss+sj*cc), cj*cs-sj*sc) +end diff --git a/test/runtests.jl b/test/runtests.jl index 44fc69c..c5ea5b3 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -4,6 +4,7 @@ using MuJoCo: TESTMODELXML const LYCEUM_SUITE = [ (LyceumMuJoCo.PointMass, (), ()), + (LyceumMuJoCo.PenHand, (), ()), (LyceumMuJoCo.ArmHandPickup, (), ()), ]