├── .gitignore ├── CLA ├── LICENSE ├── README.md ├── content ├── assets │ ├── collision_maps │ │ ├── collision_map.png │ │ ├── collision_map.svg │ │ ├── collision_map_cem.png │ │ ├── collision_map_corridor.png │ │ ├── collision_map_test.png │ │ └── collision_map_xy.png │ └── urdf │ │ ├── franka_description │ │ ├── franka_panda_no_gripper.urdf │ │ └── meshes │ │ │ ├── collision │ │ │ ├── finger.obj │ │ │ ├── finger.stl │ │ │ ├── hand.obj │ │ │ ├── hand.stl │ │ │ ├── hand_gripper.obj │ │ │ ├── hand_gripper.stl │ │ │ ├── link0.obj │ │ │ ├── link0.stl │ │ │ ├── link1.obj │ │ │ ├── link1.stl │ │ │ ├── link2.obj │ │ │ ├── link2.stl │ │ │ ├── link3.obj │ │ │ ├── link3.stl │ │ │ ├── link4.obj │ │ │ ├── link4.stl │ │ │ ├── link5.obj │ │ │ ├── link5.stl │ │ │ ├── link6.obj │ │ │ ├── link6.stl │ │ │ ├── link7.obj │ │ │ └── link7.stl │ │ │ └── visual │ │ │ ├── finger.dae │ │ │ ├── finger.mtl │ │ │ ├── finger.obj │ │ │ ├── hand.dae │ │ │ ├── hand.mtl │ │ │ ├── hand.obj │ │ │ ├── hand_gripper.obj │ │ │ ├── hand_gripper.stl │ │ │ ├── link0.dae │ │ │ ├── link0.mtl │ │ │ ├── link0.obj │ │ │ ├── link1.dae │ │ │ ├── link1.mtl │ │ │ ├── link1.obj │ │ │ ├── link2.dae │ │ │ ├── link2.mtl │ │ │ ├── link2.obj │ │ │ ├── link3.dae │ │ │ ├── link3.mtl │ │ │ ├── link3.obj │ │ │ ├── link4.dae │ │ │ ├── link4.mtl │ │ │ ├── link4.obj │ │ │ ├── link5.dae │ │ │ ├── link5.mtl │ │ │ ├── link5.obj │ │ │ ├── link6.dae │ │ │ ├── link6.mtl │ │ │ ├── link6.obj │ │ │ ├── link7.dae │ │ │ ├── link7.mtl │ │ │ ├── link7.obj │ │ │ └── train_gripper.obj │ │ └── mug │ │ ├── movable_mug.urdf │ │ ├── mug.obj │ │ └── mug.urdf └── configs │ ├── gym │ ├── collision_demo.yml │ ├── collision_primitives_3d.yml │ ├── collision_table.yml │ ├── franka.yml │ └── physx.yml │ ├── mpc │ ├── franka_reacher.yml │ └── simple_reacher.yml │ └── robot │ ├── franka.yml │ ├── franka_real_robot.yml │ └── kinova_gen3.yml ├── docs ├── Makefile ├── conf.py ├── generate_docs.sh ├── images │ ├── coll_demo.gif │ ├── constrained_storm.gif │ ├── mpc_approach.png │ └── mpc_framework.png └── index.rst ├── environment.yml ├── examples ├── franka_reacher.py └── simple_reacher.py ├── install_instructions.md ├── scripts └── train_self_collision.py ├── setup.py ├── storm_kit ├── __init__.py ├── differentiable_robot_model │ ├── LICENSE │ ├── __init__.py │ ├── coordinate_transform.py │ ├── differentiable_rigid_body.py │ ├── differentiable_robot_model.py │ ├── urdf_utils.py │ └── utils.py ├── geom │ ├── __init__.py │ ├── geom_types.py │ ├── nn_model │ │ ├── __init__.py │ │ ├── network_macros.py │ │ └── robot_self_collision.py │ ├── sdf │ │ ├── __init__.py │ │ ├── primitives.py │ │ ├── robot.py │ │ ├── robot_world.py │ │ └── world.py │ └── utils.py ├── gym │ ├── __init__.py │ ├── core.py │ ├── helpers.py │ ├── kdl_parser.py │ └── sim_robot.py ├── mpc │ ├── __init__.py │ ├── control │ │ ├── __init__.py │ │ ├── control_base.py │ │ ├── control_utils.py │ │ ├── mppi.py │ │ ├── olgaussian_mpc.py │ │ └── sample_libs.py │ ├── cost │ │ ├── __init__.py │ │ ├── bound_cost.py │ │ ├── capsule_collision_cost.py │ │ ├── circle_collision_cost.py │ │ ├── collision_cost.py │ │ ├── cost_base.py │ │ ├── dist_cost.py │ │ ├── ee_vel_cost.py │ │ ├── finite_difference_cost.py │ │ ├── gaussian_projection.py │ │ ├── image_collision_cost.py │ │ ├── jacobian_cost.py │ │ ├── manipulability_cost.py │ │ ├── null_costs.py │ │ ├── pose_cost.py │ │ ├── primitive_collision_cost.py │ │ ├── projected_dist_cost.py │ │ ├── robot_self_collision_cost.py │ │ ├── stop_cost.py │ │ ├── voxel_collision_cost.py │ │ └── zero_cost.py │ ├── model │ │ ├── __init__.py │ │ ├── integration_utils.py │ │ ├── model_base.py │ │ ├── simple_model.py │ │ ├── urdf_kinematic_model.py │ │ └── urdf_kinematic_model_baseline.py │ ├── rollout │ │ ├── __init__.py │ │ ├── arm_base.py │ │ ├── arm_reacher.py │ │ ├── rollout_base.py │ │ └── simple_reacher.py │ ├── task │ │ ├── __init__.py │ │ ├── arm_task.py │ │ ├── reacher_task.py │ │ ├── simple_task.py │ │ └── task_base.py │ └── utils │ │ ├── __init__.py │ │ ├── helpers.py │ │ ├── mpc_process_wrapper.py │ │ ├── state_filter.py │ │ ├── torch_utils.py │ │ └── zmq_robot_interface.py └── util_file.py └── weights └── robot_self └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode* 2 | *.hdf5 3 | *data/ 4 | 5 | *.p 6 | *.mlx 7 | 8 | *# 9 | *logs 10 | # Byte-compiled / optimized / DLL files 11 | __pycache__/ 12 | *.py[cod] 13 | *$py.class 14 | 15 | *.json 16 | 17 | # C extensions 18 | *.so 19 | 20 | # Distribution / packaging 21 | .Python 22 | build/ 23 | develop-eggs/ 24 | dist/ 25 | downloads/ 26 | eggs/ 27 | .eggs/ 28 | lib/ 29 | lib64/ 30 | parts/ 31 | sdist/ 32 | var/ 33 | wheels/ 34 | share/python-wheels/ 35 | *.egg-info/ 36 | .installed.cfg 37 | *.egg 38 | MANIFEST 39 | 40 | # PyInstaller 41 | # Usually these files are written by a python script from a template 42 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 43 | *.manifest 44 | *.spec 45 | 46 | # Installer logs 47 | pip-log.txt 48 | pip-delete-this-directory.txt 49 | 50 | # Unit test / coverage reports 51 | htmlcov/ 52 | .tox/ 53 | .nox/ 54 | .coverage 55 | .coverage.* 56 | .cache 57 | nosetests.xml 58 | coverage.xml 59 | *.cover 60 | *.py,cover 61 | .hypothesis/ 62 | .pytest_cache/ 63 | cover/ 64 | 65 | # Translations 66 | *.mo 67 | *.pot 68 | 69 | # Django stuff: 70 | *.log 71 | local_settings.py 72 | db.sqlite3 73 | db.sqlite3-journal 74 | 75 | # Flask stuff: 76 | instance/ 77 | .webassets-cache 78 | 79 | # Scrapy stuff: 80 | .scrapy 81 | 82 | # Sphinx documentation 83 | docs/_build/ 84 | docs/_source/ 85 | 86 | # PyBuilder 87 | .pybuilder/ 88 | target/ 89 | 90 | # Jupyter Notebook 91 | .ipynb_checkpoints 92 | 93 | # IPython 94 | profile_default/ 95 | ipython_config.py 96 | 97 | # pyenv 98 | # For a library or package, you might want to ignore these files since the code is 99 | # intended to run in multiple environments; otherwise, check them in: 100 | # .python-version 101 | 102 | # pipenv 103 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 104 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 105 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 106 | # install all needed dependencies. 107 | #Pipfile.lock 108 | 109 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 110 | __pypackages__/ 111 | 112 | # Celery stuff 113 | celerybeat-schedule 114 | celerybeat.pid 115 | 116 | # SageMath parsed files 117 | *.sage.py 118 | 119 | # Environments 120 | .env 121 | .venv 122 | env/ 123 | venv/ 124 | ENV/ 125 | env.bak/ 126 | venv.bak/ 127 | .#* 128 | # Spyder project settings 129 | .spyderproject 130 | .spyproject 131 | 132 | # Rope project settings 133 | .ropeproject 134 | 135 | # mkdocs documentation 136 | /site 137 | 138 | # mypy 139 | .mypy_cache/ 140 | .dmypy.json 141 | dmypy.json 142 | 143 | # Pyre type checker 144 | .pyre/ 145 | 146 | # pytype static type analyzer 147 | .pytype/ 148 | 149 | # Cython debug symbols 150 | cython_debug/ 151 | 152 | experiments/ 153 | 154 | # torch pickles: 155 | *.t 156 | *.npy 157 | *.zip 158 | *.qdrep 159 | *.sql* 160 | *.npz 161 | /examples/*.stl 162 | 163 | *Flymake log* 164 | 165 | *.pth* 166 | *.tar 167 | *.pt 168 | 169 | storm_kit.* 170 | docs/modules.rst 171 | docs/setup.rst 172 | 173 | 174 | # VIM 175 | *swp 176 | *swo 177 | -------------------------------------------------------------------------------- /CLA: -------------------------------------------------------------------------------- 1 | Individual Contributor License Agreement (CLA) 2 | Thank you for submitting your contributions to this project. 3 | 4 | By signing this CLA, you agree that the following terms apply to all of your past, present and future contributions to the project. 5 | 6 | License. 7 | You hereby represent that all present, past and future contributions are governed by the MIT License copyright statement. 8 | 9 | This entails that to the extent possible under law, you transfer all copyright and related or neighboring rights of the code or documents you contribute to the project itself or its maintainers. Furthermore you also represent that you have the authority to perform the above waiver with respect to the entirety of you contributions. 10 | 11 | Moral Rights. 12 | To the fullest extent permitted under applicable law, you hereby waive, and agree not to assert, all of your “moral rights” in or relating to your contributions for the benefit of the project. 13 | 14 | Third Party Content. 15 | If your Contribution includes or is based on any source code, object code, bug fixes, configuration changes, tools, specifications, documentation, data, materials, feedback, information or other works of authorship that were not authored by you (“Third Party Content”) or if you are aware of any third party intellectual property or proprietary rights associated with your Contribution (“Third Party Rights”), then you agree to include with the submission of your Contribution full details respecting such Third Party Content and Third Party Rights, including, without limitation, identification of which aspects of your Contribution contain Third Party Content or are associated with Third Party Rights, the owner/author of the Third Party Content and Third Party Rights, where you obtained the Third Party Content, and any applicable third party license terms or restrictions respecting the Third Party Content and Third Party Rights. For greater certainty, the foregoing obligations respecting the identification of Third Party Content and Third Party Rights do not apply to any portion of a Project that is incorporated into your Contribution to that same Project. 16 | 17 | Representations. 18 | You represent that, other than the Third Party Content and Third Party Rights identified by you in accordance with this Agreement, you are the sole author of your Contributions and are legally entitled to grant the foregoing licenses and waivers in respect of your Contributions. If your Contributions were created in the course of your employment with your past or present employer(s), you represent that such employer(s) has authorized you to make your Contributions on behalf of such employer(s) or such employer (s) has waived all of their right, title or interest in or to your Contributions. 19 | 20 | Disclaimer. 21 | To the fullest extent permitted under applicable law, your Contributions are provided on an "as is" basis, without any warranties or conditions, express or implied, including, without limitation, any implied warranties or conditions of non-infringement, merchantability or fitness for a particular purpose. You are not required to provide support for your Contributions, except to the extent you desire to provide support. 22 | 23 | No Obligation. 24 | You acknowledge that the maintainers of this project are under no obligation to use or incorporate your contributions into the project. The decision to use or incorporate your contributions into the project will be made at the sole discretion of the maintainers or their authorized delegates. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # STORM 2 | **Stochastic Tensor Optimization for Robot Motion** - *A GPU Robot Motion Toolkit* 3 | 4 | [[Install Instructions](install_instructions.md)] [[Paper](https://arxiv.org/abs/2104.13542)] [[Website](https://sites.google.com/view/manipulation-mpc/home)] 5 | 6 | This package contains code for reactive robot motion leveraging parallel compute on the GPU. The implemented control framework leverages MPPI to optimize over sampled actions and their costs. The costs are computed by rolling out the forward model from the current state with the sampled actions. Most files are documented with sphinx. Once you clone this repo, go into docs folder and run `sh generate_docs.sh` to generate documentation. 7 | 8 | **To run on a real Franka Panda, you can use this low-level control wrapper:** [franka_motion_control](https://github.com/mohakbhardwaj/franka_motion_control) from [Mohak Bhardwaj](https://github.com/mohakbhardwaj). 9 |

10 | 11 | 12 |

13 | 14 | 15 | ## Updates 16 | Jan. 2022 - Add CoRL citation, merge torch.size() bug (thanks [@maxpahn](https://github.com/maxspahn)). 17 | 18 | ## Contributors 19 | - Mohak Bhardwaj 20 | - Balakumar Sundaralingam 21 | 22 | ## Citation 23 | If you use this source code, please cite the below article, 24 | 25 | ``` 26 | @article{storm2021, 27 | title={{STORM}: An Integrated Framework for Fast Joint-Space Model-Predictive Control for Reactive Manipulation}, 28 | author={Mohak Bhardwaj and Balakumar Sundaralingam and Arsalan Mousavian and Nathan D. Ratliff and Dieter Fox and Fabio Ramos and Byron Boots}, 29 | booktitle={5th Annual Conference on Robot Learning }, 30 | year={2021},} 31 | ``` 32 | 33 | ## Contributing to this code 34 | Refer to CLA before making contributions. 35 | -------------------------------------------------------------------------------- /content/assets/collision_maps/collision_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/collision_maps/collision_map.png -------------------------------------------------------------------------------- /content/assets/collision_maps/collision_map.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 19 | 21 | 39 | 41 | 42 | 44 | image/svg+xml 45 | 47 | 48 | 49 | 50 | 51 | 56 | 63 | 80 | 97 | 115 | 133 | 134 | 135 | -------------------------------------------------------------------------------- /content/assets/collision_maps/collision_map_cem.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/collision_maps/collision_map_cem.png -------------------------------------------------------------------------------- /content/assets/collision_maps/collision_map_corridor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/collision_maps/collision_map_corridor.png -------------------------------------------------------------------------------- /content/assets/collision_maps/collision_map_test.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/collision_maps/collision_map_test.png -------------------------------------------------------------------------------- /content/assets/collision_maps/collision_map_xy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/collision_maps/collision_map_xy.png -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/collision/finger.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object finger.obj 7 | # 8 | # Vertices: 52 9 | # Faces: 32 10 | # 11 | #### 12 | vn 0.999991 0.003723 -0.001919 13 | v 0.010360 0.026403 0.000155 14 | vn 0.019341 -0.997893 -0.061925 15 | v 0.010449 0.002583 0.000147 16 | vn -0.999568 -0.025962 0.013789 17 | v -0.010387 0.002534 0.000132 18 | vn -0.999606 -0.009503 0.026403 19 | v -0.010479 0.016102 0.018988 20 | vn -0.000579 0.001464 -0.999999 21 | v -0.010401 0.026309 0.000167 22 | vn -0.044737 0.976483 0.210900 23 | v -0.010389 0.025220 0.019188 24 | vn -0.871286 -0.490748 0.005227 25 | v -0.008730 -0.000024 0.036165 26 | vn 0.999861 0.006488 0.015354 27 | v 0.010400 0.025253 0.019037 28 | vn 0.377718 0.867563 0.323518 29 | v 0.005840 0.014274 0.053803 30 | vn 0.736099 -0.021564 0.676530 31 | v 0.008616 0.013989 0.051328 32 | vn 0.999373 -0.008600 0.034345 33 | v 0.010495 0.015103 0.018436 34 | vn 0.013041 -0.999896 -0.006124 35 | v 0.008693 -0.000133 0.050166 36 | vn -0.998603 -0.032800 0.041418 37 | v -0.008623 -0.000057 0.050953 38 | vn -0.588468 -0.017705 0.808327 39 | v -0.005481 -0.000091 0.053725 40 | vn 0.004085 -0.008700 0.999954 41 | v -0.005278 0.014293 0.053849 42 | vn -0.691057 -0.012018 0.722700 43 | v -0.007778 0.014218 0.052366 44 | vn -0.665951 0.690851 0.281486 45 | v -0.008841 0.013918 0.050589 46 | vn 0.736099 -0.021564 0.676530 47 | v 0.006138 -0.000021 0.053578 48 | vn -0.002818 0.998255 0.058981 49 | v 0.010360 0.026403 0.000155 50 | vn 0.000073 0.000898 -1.000000 51 | v 0.010360 0.026403 0.000155 52 | vn 0.999898 -0.012431 0.007036 53 | v 0.010449 0.002583 0.000147 54 | vn 0.000724 0.000331 -1.000000 55 | v 0.010449 0.002583 0.000147 56 | vn -0.871286 -0.490748 0.005227 57 | v -0.010387 0.002534 0.000132 58 | vn 0.002403 -0.997480 -0.070914 59 | v -0.010387 0.002534 0.000132 60 | vn 0.000073 0.000898 -1.000000 61 | v -0.010387 0.002534 0.000132 62 | vn -0.004486 0.998354 0.057168 63 | v -0.010401 0.026309 0.000167 64 | vn -0.999988 0.004662 -0.001626 65 | v -0.010401 0.026309 0.000167 66 | vn -0.665951 0.690851 0.281486 67 | v -0.010389 0.025220 0.019188 68 | vn -0.999597 0.009346 0.026807 69 | v -0.010389 0.025220 0.019188 70 | vn 0.006493 -0.999457 -0.032313 71 | v -0.008730 -0.000024 0.036165 72 | vn 0.377718 0.867563 0.323518 73 | v 0.010400 0.025253 0.019037 74 | vn -0.000242 0.983230 0.182372 75 | v 0.010400 0.025253 0.019037 76 | vn 0.665647 0.002096 0.746264 77 | v 0.005840 0.014274 0.053803 78 | vn 0.008418 -0.012115 0.999891 79 | v 0.005840 0.014274 0.053803 80 | vn 0.001757 0.953702 0.300749 81 | v 0.005840 0.014274 0.053803 82 | vn 0.377718 0.867563 0.323518 83 | v 0.008616 0.013989 0.051328 84 | vn 0.998361 0.003310 0.057136 85 | v 0.008616 0.013989 0.051328 86 | vn 0.798906 -0.045001 0.599770 87 | v 0.008693 -0.000133 0.050166 88 | vn 0.998687 -0.025065 0.044683 89 | v 0.008693 -0.000133 0.050166 90 | vn -0.769031 -0.017753 0.638965 91 | v -0.008623 -0.000057 0.050953 92 | vn -0.008996 -0.999957 -0.002185 93 | v -0.008623 -0.000057 0.050953 94 | vn -0.871286 -0.490748 0.005227 95 | v -0.008623 -0.000057 0.050953 96 | vn 0.008418 -0.012115 0.999891 97 | v -0.005481 -0.000091 0.053725 98 | vn -0.002059 -0.999940 0.010793 99 | v -0.005481 -0.000091 0.053725 100 | vn -0.510143 -0.000217 0.860089 101 | v -0.005278 0.014293 0.053849 102 | vn -0.108731 0.943365 0.313433 103 | v -0.005278 0.014293 0.053849 104 | vn -0.665951 0.690851 0.281486 105 | v -0.007778 0.014218 0.052366 106 | vn -0.218924 0.920873 0.322590 107 | v -0.007778 0.014218 0.052366 108 | vn -0.858159 -0.000049 0.513385 109 | v -0.008841 0.013918 0.050589 110 | vn -0.998665 -0.002749 0.051583 111 | v -0.008841 0.013918 0.050589 112 | vn 0.006542 -0.999267 0.037718 113 | v 0.006138 -0.000021 0.053578 114 | vn 0.012751 -0.015529 0.999798 115 | v 0.006138 -0.000021 0.053578 116 | # 52 vertices, 0 vertices normals 117 | 118 | f 20//20 22//22 25//25 119 | f 3//3 4//4 27//27 120 | f 27//27 4//4 29//29 121 | f 2//2 30//30 24//24 122 | f 32//32 6//6 35//35 123 | f 25//25 5//5 20//20 124 | f 37//37 11//11 8//8 125 | f 11//11 39//39 21//21 126 | f 37//37 39//39 11//11 127 | f 42//42 23//23 7//7 128 | f 2//2 12//12 30//30 129 | f 12//12 44//44 30//30 130 | f 8//8 11//11 21//21 131 | f 8//8 21//21 1//1 132 | f 32//32 19//19 6//6 133 | f 6//6 46//46 35//35 134 | f 48//48 46//46 6//6 135 | f 40//40 14//14 16//16 136 | f 3//3 13//13 4//4 137 | f 31//31 9//9 36//36 138 | f 19//19 26//26 6//6 139 | f 4//4 50//50 29//29 140 | f 17//17 47//47 28//28 141 | f 34//34 43//43 52//52 142 | f 15//15 43//43 34//34 143 | f 12//12 51//51 44//44 144 | f 18//18 38//38 10//10 145 | f 44//44 41//41 30//30 146 | f 16//16 14//14 45//45 147 | f 13//13 50//50 4//4 148 | f 18//18 10//10 33//33 149 | f 16//16 49//49 40//40 150 | # 32 faces, 0 coords texture 151 | 152 | # End of File 153 | -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/collision/finger.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/urdf/franka_description/meshes/collision/finger.stl -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/collision/hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/urdf/franka_description/meshes/collision/hand.stl -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/collision/hand_gripper.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/urdf/franka_description/meshes/collision/hand_gripper.stl -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/urdf/franka_description/meshes/collision/link0.stl -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/urdf/franka_description/meshes/collision/link1.stl -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/urdf/franka_description/meshes/collision/link2.stl -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/urdf/franka_description/meshes/collision/link3.stl -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/urdf/franka_description/meshes/collision/link4.stl -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/urdf/franka_description/meshes/collision/link5.stl -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/urdf/franka_description/meshes/collision/link6.stl -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/urdf/franka_description/meshes/collision/link7.stl -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/visual/finger.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 2 3 | 4 | newmtl Part__Feature001_006 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.901961 0.921569 0.929412 8 | Ks 0.250000 0.250000 0.250000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Part__Feature_007 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.250980 0.250980 0.250980 18 | Ks 0.250000 0.250000 0.250000 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/visual/hand.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 5 3 | 4 | newmtl Part__Feature001_008_005 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.250980 0.250980 0.250980 8 | Ks 0.007812 0.007812 0.007812 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Part__Feature002_005_005 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.901961 0.921569 0.929412 18 | Ks 0.015625 0.015625 0.015625 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Part__Feature005_001_005 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 1.000000 1.000000 1.000000 28 | Ks 0.015625 0.015625 0.015625 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Part__Feature005_001_005_001 35 | Ns -1.960784 36 | Ka 1.000000 1.000000 1.000000 37 | Kd 0.901961 0.921569 0.929412 38 | Ks 0.015625 0.015625 0.015625 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | 44 | newmtl Part__Feature_009_005 45 | Ns -1.960784 46 | Ka 1.000000 1.000000 1.000000 47 | Kd 0.250980 0.250980 0.250980 48 | Ks 0.015625 0.015625 0.015625 49 | Ke 0.000000 0.000000 0.000000 50 | Ni 1.000000 51 | d 1.000000 52 | illum 2 53 | -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/visual/hand_gripper.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/content/assets/urdf/franka_description/meshes/visual/hand_gripper.stl -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/visual/link0.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 12 3 | 4 | newmtl Face636_001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.901961 0.921569 0.929412 8 | Ks 0.125000 0.125000 0.125000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Part__Feature017_001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 1.000000 1.000000 1.000000 18 | Ks 0.500000 0.500000 0.500000 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Part__Feature018_001 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 1.000000 1.000000 1.000000 28 | Ks 0.500000 0.500000 0.500000 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Part__Feature019_001 35 | Ns -1.960784 36 | Ka 1.000000 1.000000 1.000000 37 | Kd 1.000000 1.000000 1.000000 38 | Ks 0.125000 0.125000 0.125000 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | 44 | newmtl Part__Feature022_001 45 | Ns -1.960784 46 | Ka 1.000000 1.000000 1.000000 47 | Kd 0.901961 0.921569 0.929412 48 | Ks 0.125000 0.125000 0.125000 49 | Ke 0.000000 0.000000 0.000000 50 | Ni 1.000000 51 | d 1.000000 52 | illum 2 53 | 54 | newmtl Part__Feature023_001 55 | Ns -1.960784 56 | Ka 1.000000 1.000000 1.000000 57 | Kd 0.250980 0.250980 0.250980 58 | Ks 0.125000 0.125000 0.125000 59 | Ke 0.000000 0.000000 0.000000 60 | Ni 1.000000 61 | d 1.000000 62 | illum 2 63 | 64 | newmtl Shell001_001 65 | Ns -1.960784 66 | Ka 1.000000 1.000000 1.000000 67 | Kd 0.250980 0.250980 0.250980 68 | Ks 0.125000 0.125000 0.125000 69 | Ke 0.000000 0.000000 0.000000 70 | Ni 1.000000 71 | d 1.000000 72 | illum 2 73 | 74 | newmtl Shell002_001 75 | Ns -1.960784 76 | Ka 1.000000 1.000000 1.000000 77 | Kd 0.901961 0.921569 0.929412 78 | Ks 0.125000 0.125000 0.125000 79 | Ke 0.000000 0.000000 0.000000 80 | Ni 1.000000 81 | d 1.000000 82 | illum 2 83 | 84 | newmtl Shell003_001 85 | Ns -1.960784 86 | Ka 1.000000 1.000000 1.000000 87 | Kd 0.901961 0.921569 0.929412 88 | Ks 0.125000 0.125000 0.125000 89 | Ke 0.000000 0.000000 0.000000 90 | Ni 1.000000 91 | d 1.000000 92 | illum 2 93 | 94 | newmtl Shell009_001 95 | Ns -1.960784 96 | Ka 1.000000 1.000000 1.000000 97 | Kd 0.250980 0.250980 0.250980 98 | Ks 0.125000 0.125000 0.125000 99 | Ke 0.000000 0.000000 0.000000 100 | Ni 1.000000 101 | d 1.000000 102 | illum 2 103 | 104 | newmtl Shell010_001 105 | Ns -1.960784 106 | Ka 1.000000 1.000000 1.000000 107 | Kd 0.901961 0.921569 0.929412 108 | Ks 0.125000 0.125000 0.125000 109 | Ke 0.000000 0.000000 0.000000 110 | Ni 1.000000 111 | d 1.000000 112 | illum 2 113 | 114 | newmtl Shell_001 115 | Ns -1.960784 116 | Ka 1.000000 1.000000 1.000000 117 | Kd 0.250980 0.250980 0.250980 118 | Ks 0.125000 0.125000 0.125000 119 | Ke 0.000000 0.000000 0.000000 120 | Ni 1.000000 121 | d 1.000000 122 | illum 2 123 | -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/visual/link1.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl Part__Feature_001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.062500 0.062500 0.062500 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/visual/link2.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl Part__Feature024 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.125000 0.125000 0.125000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/visual/link3.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 4 3 | 4 | newmtl Part__Feature001_010_001_002.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.007812 0.007812 0.007812 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Part__Feature002_007_001_002.001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 1.000000 1.000000 1.000000 18 | Ks 0.007812 0.007812 0.007812 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Part__Feature003_004_001_002.001 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 1.000000 1.000000 1.000000 28 | Ks 0.007812 0.007812 0.007812 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Part__Feature_001_001_001_002.001 35 | Ns -1.960784 36 | Ka 1.000000 1.000000 1.000000 37 | Kd 0.250980 0.250980 0.250980 38 | Ks 0.007812 0.007812 0.007812 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/visual/link4.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 4 3 | 4 | newmtl Part__Feature001_001_003_001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.007812 0.007812 0.007812 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Part__Feature002_001_003_001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.250980 0.250980 0.250980 18 | Ks 0.007812 0.007812 0.007812 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Part__Feature003_001_003_001 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 1.000000 1.000000 1.000000 28 | Ks 0.007812 0.007812 0.007812 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Part__Feature_002_003_001 35 | Ns -1.960784 36 | Ka 1.000000 1.000000 1.000000 37 | Kd 1.000000 1.000000 1.000000 38 | Ks 0.007812 0.007812 0.007812 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/visual/link5.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 3 3 | 4 | newmtl Part__Feature_002_004_003 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.015625 0.015625 0.015625 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Shell001_001_001_003 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.250000 0.250000 0.250000 18 | Ks 0.015625 0.015625 0.015625 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Shell_001_001_003 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 1.000000 1.000000 1.000000 28 | Ks 0.015625 0.015625 0.015625 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/visual/link6.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 17 3 | 4 | newmtl Face064_002_001_002_001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 0.000000 0.000000 8 | Ks 0.003906 0.003906 0.003906 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Face065_002_001_002_001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.000000 1.000000 0.000000 18 | Ks 0.003906 0.003906 0.003906 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Face374_002_001_002_001 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 1.000000 1.000000 1.000000 28 | Ks 0.003906 0.003906 0.003906 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Face539_002_001_002_001 35 | Ns -1.960784 36 | Ka 1.000000 1.000000 1.000000 37 | Kd 0.250980 0.250980 0.250980 38 | Ks 0.003906 0.003906 0.003906 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | 44 | newmtl Part__Feature001_009_001_002_001 45 | Ns -1.960784 46 | Ka 1.000000 1.000000 1.000000 47 | Kd 0.250980 0.250980 0.250980 48 | Ks 0.003906 0.003906 0.003906 49 | Ke 0.000000 0.000000 0.000000 50 | Ni 1.000000 51 | d 1.000000 52 | illum 2 53 | 54 | newmtl Part__Feature002_006_001_002_001 55 | Ns -1.960784 56 | Ka 1.000000 1.000000 1.000000 57 | Kd 0.250980 0.250980 0.250980 58 | Ks 0.003906 0.003906 0.003906 59 | Ke 0.000000 0.000000 0.000000 60 | Ni 1.000000 61 | d 1.000000 62 | illum 2 63 | 64 | newmtl Shell002_002_001_002_001 65 | Ns -1.960784 66 | Ka 1.000000 1.000000 1.000000 67 | Kd 1.000000 1.000000 1.000000 68 | Ks 0.003906 0.003906 0.003906 69 | Ke 0.000000 0.000000 0.000000 70 | Ni 1.000000 71 | d 1.000000 72 | illum 2 73 | 74 | newmtl Shell003_002_001_002_001 75 | Ns -1.960784 76 | Ka 1.000000 1.000000 1.000000 77 | Kd 1.000000 1.000000 1.000000 78 | Ks 0.003906 0.003906 0.003906 79 | Ke 0.000000 0.000000 0.000000 80 | Ni 1.000000 81 | d 1.000000 82 | illum 2 83 | 84 | newmtl Shell004_001_001_002_001 85 | Ns -1.960784 86 | Ka 1.000000 1.000000 1.000000 87 | Kd 1.000000 1.000000 1.000000 88 | Ks 0.003906 0.003906 0.003906 89 | Ke 0.000000 0.000000 0.000000 90 | Ni 1.000000 91 | d 1.000000 92 | illum 2 93 | 94 | newmtl Shell005_001_001_002_001 95 | Ns -1.960784 96 | Ka 1.000000 1.000000 1.000000 97 | Kd 1.000000 1.000000 1.000000 98 | Ks 0.003906 0.003906 0.003906 99 | Ke 0.000000 0.000000 0.000000 100 | Ni 1.000000 101 | d 1.000000 102 | illum 2 103 | 104 | newmtl Shell006_003_002_001 105 | Ns -1.960784 106 | Ka 1.000000 1.000000 1.000000 107 | Kd 0.901961 0.921569 0.929412 108 | Ks 0.015625 0.015625 0.015625 109 | Ke 0.000000 0.000000 0.000000 110 | Ni 1.000000 111 | d 1.000000 112 | illum 2 113 | 114 | newmtl Shell007_002_002_001 115 | Ns -1.960784 116 | Ka 1.000000 1.000000 1.000000 117 | Kd 0.250000 0.250000 0.250000 118 | Ks 0.003906 0.003906 0.003906 119 | Ke 0.000000 0.000000 0.000000 120 | Ni 1.000000 121 | d 1.000000 122 | illum 2 123 | 124 | newmtl Shell011_002_002_001 125 | Ns -1.960784 126 | Ka 1.000000 1.000000 1.000000 127 | Kd 1.000000 1.000000 1.000000 128 | Ks 0.003906 0.003906 0.003906 129 | Ke 0.000000 0.000000 0.000000 130 | Ni 1.000000 131 | d 1.000000 132 | illum 2 133 | 134 | newmtl Shell012_002_002_001 135 | Ns -1.960784 136 | Ka 1.000000 1.000000 1.000000 137 | Kd 1.000000 1.000000 1.000000 138 | Ks 0.003906 0.003906 0.003906 139 | Ke 0.000000 0.000000 0.000000 140 | Ni 1.000000 141 | d 1.000000 142 | illum 2 143 | 144 | newmtl Shell_003_001_002_001 145 | Ns -1.960784 146 | Ka 1.000000 1.000000 1.000000 147 | Kd 0.250980 0.250980 0.250980 148 | Ks 0.003906 0.003906 0.003906 149 | Ke 0.000000 0.000000 0.000000 150 | Ni 1.000000 151 | d 1.000000 152 | illum 2 153 | 154 | newmtl Union001_001_001_002_001 155 | Ns -1.960784 156 | Ka 1.000000 1.000000 1.000000 157 | Kd 0.039216 0.541176 0.780392 158 | Ks 0.003906 0.003906 0.003906 159 | Ke 0.000000 0.000000 0.000000 160 | Ni 1.000000 161 | d 1.000000 162 | illum 2 163 | 164 | newmtl Union_001_001_002_001 165 | Ns -1.960784 166 | Ka 1.000000 1.000000 1.000000 167 | Kd 0.039216 0.541176 0.780392 168 | Ks 0.003906 0.003906 0.003906 169 | Ke 0.000000 0.000000 0.000000 170 | Ni 1.000000 171 | d 1.000000 172 | illum 2 173 | -------------------------------------------------------------------------------- /content/assets/urdf/franka_description/meshes/visual/link7.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 8 3 | 4 | newmtl Part__Mirroring001_004_002 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.250980 0.250980 0.250980 8 | Ks 0.015625 0.015625 0.015625 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Part__Mirroring002_004_001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.250980 0.250980 0.250980 18 | Ks 0.031250 0.031250 0.031250 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Part__Mirroring003_004_001 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 0.250980 0.250980 0.250980 28 | Ks 0.031250 0.031250 0.031250 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Part__Mirroring004_004_002 35 | Ns -1.960784 36 | Ka 1.000000 1.000000 1.000000 37 | Kd 1.000000 1.000000 1.000000 38 | Ks 0.031250 0.031250 0.031250 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | 44 | newmtl Part__Mirroring005_004_001 45 | Ns -1.960784 46 | Ka 1.000000 1.000000 1.000000 47 | Kd 0.250980 0.250980 0.250980 48 | Ks 0.031250 0.031250 0.031250 49 | Ke 0.000000 0.000000 0.000000 50 | Ni 1.000000 51 | d 1.000000 52 | illum 2 53 | 54 | newmtl Part__Mirroring006_004_001 55 | Ns -1.960784 56 | Ka 1.000000 1.000000 1.000000 57 | Kd 0.250980 0.250980 0.250980 58 | Ks 0.031250 0.031250 0.031250 59 | Ke 0.000000 0.000000 0.000000 60 | Ni 1.000000 61 | d 1.000000 62 | illum 2 63 | 64 | newmtl Part__Mirroring007_004_001 65 | Ns -1.960784 66 | Ka 1.000000 1.000000 1.000000 67 | Kd 0.250980 0.250980 0.250980 68 | Ks 0.031250 0.031250 0.031250 69 | Ke 0.000000 0.000000 0.000000 70 | Ni 1.000000 71 | d 1.000000 72 | illum 2 73 | 74 | newmtl Part__Mirroring_004_001 75 | Ns -1.960784 76 | Ka 1.000000 1.000000 1.000000 77 | Kd 0.898039 0.917647 0.929412 78 | Ks 0.031250 0.031250 0.031250 79 | Ke 0.000000 0.000000 0.000000 80 | Ni 1.000000 81 | d 1.000000 82 | illum 2 83 | -------------------------------------------------------------------------------- /content/assets/urdf/mug/movable_mug.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /content/assets/urdf/mug/mug.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /content/configs/gym/collision_demo.yml: -------------------------------------------------------------------------------- 1 | ## 2 | ## MIT License 3 | ## 4 | ## Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | ## 6 | ## Permission is hereby granted, free of charge, to any person obtaining a 7 | ## copy of this software and associated documentation files (the "Software"), 8 | ## to deal in the Software without restriction, including without limitation 9 | ## the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | ## and/or sell copies of the Software, and to permit persons to whom the 11 | ## Software is furnished to do so, subject to the following conditions: 12 | ## 13 | ## The above copyright notice and this permission notice shall be included in 14 | ## all copies or substantial portions of the Software. 15 | ## 16 | ## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | ## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | ## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | ## THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | ## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | ## FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | ## DEALINGS IN THE SOFTWARE.## 23 | world_model: 24 | coll_objs: 25 | sphere: 26 | sphere1: 27 | radius: 0.001 #5 # meters 28 | position: [10.2, -0.0, 0.4] 29 | cube: 30 | table: 31 | dims: [2.0, 2.0, 0.2] # x, y, z 32 | pose: [0.0, 0.0, -0.1, 0, 0, 0, 1.0] # x, y, z, qx, qy, qz, qw 33 | -------------------------------------------------------------------------------- /content/configs/gym/collision_primitives_3d.yml: -------------------------------------------------------------------------------- 1 | ## 2 | ## MIT License 3 | ## 4 | ## Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | ## 6 | ## Permission is hereby granted, free of charge, to any person obtaining a 7 | ## copy of this software and associated documentation files (the "Software"), 8 | ## to deal in the Software without restriction, including without limitation 9 | ## the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | ## and/or sell copies of the Software, and to permit persons to whom the 11 | ## Software is furnished to do so, subject to the following conditions: 12 | ## 13 | ## The above copyright notice and this permission notice shall be included in 14 | ## all copies or substantial portions of the Software. 15 | ## 16 | ## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | ## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | ## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | ## THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | ## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | ## FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | ## DEALINGS IN THE SOFTWARE.## 23 | world_model: 24 | coll_objs: 25 | sphere: 26 | sphere1: 27 | radius: 0.1 #5 # meters 28 | position: [0.4, 0.4, 0.1] 29 | cube: 30 | cube1: 31 | dims: [0.3, 0.1, 0.4] # x, y, z 32 | pose: [0.4, 0.2, 0.2, 0, 0, 0, 1.0] # x, y, z, qx, qy, qz, qw 33 | 34 | cube2: 35 | dims: [0.3, 0.1, 0.5] # x, y, z 36 | pose: [0.4, -0.3, 0.2, 0, 0, 0, 1.0] # x, y, z, qx, qy, qz, qw 37 | 38 | cube3: 39 | dims: [2.0, 2.0, 0.2] # x, y, z 40 | pose: [0.0, 0.0, -0.1, 0, 0, 0, 1.0] # x, y, z, qx, qy, qz, qw 41 | -------------------------------------------------------------------------------- /content/configs/gym/collision_table.yml: -------------------------------------------------------------------------------- 1 | ## 2 | ## MIT License 3 | ## 4 | ## Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | ## 6 | ## Permission is hereby granted, free of charge, to any person obtaining a 7 | ## copy of this software and associated documentation files (the "Software"), 8 | ## to deal in the Software without restriction, including without limitation 9 | ## the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | ## and/or sell copies of the Software, and to permit persons to whom the 11 | ## Software is furnished to do so, subject to the following conditions: 12 | ## 13 | ## The above copyright notice and this permission notice shall be included in 14 | ## all copies or substantial portions of the Software. 15 | ## 16 | ## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | ## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | ## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | ## THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | ## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | ## FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | ## DEALINGS IN THE SOFTWARE.## 23 | world_model: 24 | coll_objs: 25 | sphere: 26 | sphere1: 27 | radius: 0.001 #5 # meters 28 | position: [10.2, -0.0, 0.4] 29 | cube: 30 | table: 31 | dims: [2.0, 2.0, 0.2] # x, y, z 32 | pose: [0.0, 0.0, -0.1, 0, 0, 0, 1.0] # x, y, z, qx, qy, qz, qw 33 | -------------------------------------------------------------------------------- /content/configs/gym/franka.yml: -------------------------------------------------------------------------------- 1 | ## 2 | ## MIT License 3 | ## 4 | ## Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | ## 6 | ## Permission is hereby granted, free of charge, to any person obtaining a 7 | ## copy of this software and associated documentation files (the "Software"), 8 | ## to deal in the Software without restriction, including without limitation 9 | ## the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | ## and/or sell copies of the Software, and to permit persons to whom the 11 | ## Software is furnished to do so, subject to the following conditions: 12 | ## 13 | ## The above copyright notice and this permission notice shall be included in 14 | ## all copies or substantial portions of the Software. 15 | ## 16 | ## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | ## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | ## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | ## THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | ## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | ## FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | ## DEALINGS IN THE SOFTWARE.## 23 | 24 | # @package _global_ 25 | use_cuda: True 26 | cuda_device_num: 0 27 | 28 | 29 | sim_params: 30 | sim_urdf: "urdf/franka_description/franka_panda_no_gripper.urdf" 31 | robot_pose: [0, 1.0, 0, 0.0, -0.707107, -0.707107, 0.0] 32 | init_state: [1.00, -1.0, 0.00, -2.0, 0.00, 1.57, 0.78] 33 | control_mode: 'torque' 34 | 35 | asset_options: 36 | fix_base_link: True 37 | flip_visual_attachments: True 38 | armature: 0.001 39 | disable_gravity: True 40 | collapse_fixed_joints: True 41 | camera: 42 | label_map: {'robot':2, 'ground':0} 43 | 44 | -------------------------------------------------------------------------------- /content/configs/gym/physx.yml: -------------------------------------------------------------------------------- 1 | ## 2 | ## MIT License 3 | ## 4 | ## Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | ## 6 | ## Permission is hereby granted, free of charge, to any person obtaining a 7 | ## copy of this software and associated documentation files (the "Software"), 8 | ## to deal in the Software without restriction, including without limitation 9 | ## the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | ## and/or sell copies of the Software, and to permit persons to whom the 11 | ## Software is furnished to do so, subject to the following conditions: 12 | ## 13 | ## The above copyright notice and this permission notice shall be included in 14 | ## all copies or substantial portions of the Software. 15 | ## 16 | ## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | ## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | ## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | ## THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | ## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | ## FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | ## DEALINGS IN THE SOFTWARE.## 23 | physics_engine: 'physx' 24 | compute_device_id: 0 25 | graphics_device_id: 0 26 | headless: False 27 | sim_params: 28 | dt: 0.01 29 | substeps: 1 30 | physx: 31 | bounce_threshold_velocity: 0.1 32 | use_gpu: False 33 | solver_type: 0 34 | num_threads: 4 35 | contact_offset: 0.01 36 | num_position_iterations: 8 37 | num_velocity_iterations: 4 38 | rest_offset: 0.001 39 | -------------------------------------------------------------------------------- /content/configs/mpc/simple_reacher.yml: -------------------------------------------------------------------------------- 1 | ## 2 | ## MIT License 3 | ## 4 | ## Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | ## 6 | ## Permission is hereby granted, free of charge, to any person obtaining a 7 | ## copy of this software and associated documentation files (the "Software"), 8 | ## to deal in the Software without restriction, including without limitation 9 | ## the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | ## and/or sell copies of the Software, and to permit persons to whom the 11 | ## Software is furnished to do so, subject to the following conditions: 12 | ## 13 | ## The above copyright notice and this permission notice shall be included in 14 | ## all copies or substantial portions of the Software. 15 | ## 16 | ## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | ## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | ## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | ## THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | ## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | ## FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | ## DEALINGS IN THE SOFTWARE.## 23 | 24 | # @package _global_ 25 | use_cuda: True 26 | cuda_device_num: 0 27 | 28 | control_dt: 0.1 29 | control_space: 'acc' 30 | float_dtype: 'float32' 31 | state_filter_coeff: 1.0 32 | cmd_filter_coeff: 1.0 33 | 34 | model: 35 | # any link that is not specified as learnable will be initialized from urdf 36 | #urdf_path: "urdf/franka_description/franka_panda_no_gripper.urdf" 37 | #learnable_rigid_body_config: 38 | # learnable_links: [] 39 | #name: "franka_panda" 40 | dt: 0.0 41 | max_action: 0.1 #10.0 42 | 43 | dt_traj_params: 44 | base_dt: 0.1 45 | base_ratio: 1.0 46 | max_dt: 0.3 47 | init_state: [0.0,0.0] 48 | position_bounds: [[0.0, 0.5], [0.0,0.5]] 49 | 50 | cost: 51 | goal_state: 52 | vec_weight: [1.0, 1.0] 53 | weight: 100.0 54 | gaussian_params: {'n':0, 'c':0.2, 's':0.0, 'r':10.0} 55 | 56 | 57 | zero_vel: 58 | weight: 0.0 59 | hinge_val: 0.2 #0.2 60 | gaussian_params: {'n':0, 'c':0.2, 's':0, 'r':1.0} 61 | 62 | stop_cost: 63 | weight: 100.0 64 | max_nlimit: 0.05 #0.2 65 | gaussian_params: {'n':0, 'c':0.2, 's':0, 'r':10.0} 66 | stop_cost_acc: 67 | weight: 100.0 68 | max_limit: 0.01 #0.2 69 | gaussian_params: {'n':0, 'c':0.2, 's':0, 'r':10.0} 70 | 71 | 72 | smooth: # on robot acceleration 73 | weight: 0.0 74 | gaussian_params: {'n':0, 'c':0.2, 's':0, 'r':1.0} 75 | order: 3 # on velocity 76 | 77 | 78 | image_collision: # on robot acceleration 79 | weight: 1000.0 80 | gaussian_params: {'n':0, 'c':1.0, 's':0, 'r':10.0} 81 | collision_file: 'collision_maps/collision_map_cem.png' 82 | dist_thresh: 0.01 83 | state_bound: 84 | weight: 100.0 85 | gaussian_params: {'n':0, 'c':1.0, 's':0, 'r':10.0} 86 | terminal: 87 | weight: 0.0 88 | gaussian_params: {'n':0, 'c':1.0, 's':0, 'r':10.0} 89 | mppi: 90 | horizon : 30 # 100 91 | init_cov : 0.01 #.5 92 | gamma : 0.98 # 93 | n_iters : 1 94 | step_size_mean : 0.9 95 | step_size_cov : 0.6 96 | beta : 1.0 97 | alpha : 1 98 | num_particles : 500 #10000 99 | update_cov : True 100 | cov_type : 'diag_AxA' # 101 | kappa : 0.0001 102 | null_act_frac : 0.01 103 | sample_mode : 'mean' 104 | base_action : 'repeat' 105 | squash_fn : 'clamp' # [clamp,] 106 | hotstart : True 107 | visual_traj : 'state_seq' 108 | sample_params: 109 | type: 'multiple' 110 | fixed_samples: True 111 | sample_ratio: {'halton':0.0, 'halton-knot':1.0, 'random':0.0, 'random-knot':0.0} 112 | seed: 0 113 | filter_coeffs: None #[0.5, 0.3, 0.2] 114 | knot_scale: 5 115 | #filter_coeffs: [1.0, 0.0, 0.0] 116 | -------------------------------------------------------------------------------- /content/configs/robot/franka.yml: -------------------------------------------------------------------------------- 1 | ## 2 | ## MIT License 3 | ## 4 | ## Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | ## 6 | ## Permission is hereby granted, free of charge, to any person obtaining a 7 | ## copy of this software and associated documentation files (the "Software"), 8 | ## to deal in the Software without restriction, including without limitation 9 | ## the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | ## and/or sell copies of the Software, and to permit persons to whom the 11 | ## Software is furnished to do so, subject to the following conditions: 12 | ## 13 | ## The above copyright notice and this permission notice shall be included in 14 | ## all copies or substantial portions of the Software. 15 | ## 16 | ## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | ## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | ## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | ## THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | ## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | ## FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | ## DEALINGS IN THE SOFTWARE.## 23 | robot: 'Franka Panda' 24 | 25 | collision_spheres: 26 | panda_link0: 27 | - "center": [0.0, 0.0, 0.05] 28 | "radius": 0.08 29 | panda_link1: 30 | - "center": [0.0, -0.08, 0.0] 31 | "radius": 0.06 32 | - "center": [0.0, -0.03, 0.0] 33 | "radius": 0.06 34 | - "center": [0.0, 0.0, -0.12] 35 | "radius": 0.06 36 | - "center": [0.0, 0.0, -0.17] 37 | "radius": 0.06 38 | panda_link2: 39 | - "center": [0.0, 0.0, 0.03] 40 | "radius": 0.06 41 | - "center": [0.0, 0.0, 0.08] 42 | "radius": 0.06 43 | - "center": [0.0, -0.12, 0.0] 44 | "radius": 0.06 45 | - "center": [0.0, -0.17, 0.0] 46 | "radius": 0.06 47 | panda_link3: 48 | - "center": [0.0, 0.0, -0.06] 49 | "radius": 0.05 50 | - "center": [0.0, 0.0, -0.1] 51 | "radius": 0.06 52 | - "center": [0.08, 0.06, 0.0] 53 | "radius": 0.055 54 | - "center": [0.08, 0.02, 0.0] 55 | "radius": 0.055 56 | panda_link4: 57 | - "center": [0.0, 0.0, 0.02] 58 | "radius": 0.055 59 | - "center": [0.0, 0.0, 0.06] 60 | "radius": 0.055 61 | - "center": [-0.08, 0.095, 0.0] 62 | "radius": 0.06 63 | - "center": [-0.08, 0.06, 0.0] 64 | "radius": 0.055 65 | panda_link5: 66 | - "center": [0.0, 0.055, 0.0] 67 | "radius": 0.06 68 | - "center": [0.0, 0.075, 0.0] 69 | "radius": 0.06 70 | - "center": [0.0, 0.000, -0.22] 71 | "radius": 0.06 72 | - "center": [0.0, 0.05, -0.18] 73 | "radius": 0.05 74 | - "center": [0.01, 0.08, -0.14] 75 | "radius": 0.025 76 | - "center": [0.01, 0.085, -0.11] 77 | "radius": 0.025 78 | - "center": [0.01, 0.09, -0.08] 79 | "radius": 0.025 80 | - "center": [0.01, 0.095, -0.05] 81 | "radius": 0.025 82 | - "center": [-0.01, 0.08, -0.14] 83 | "radius": 0.025 84 | - "center": [-0.01, 0.085, -0.11] 85 | "radius": 0.025 86 | - "center": [-0.01, 0.09, -0.08] 87 | "radius": 0.025 88 | - "center": [-0.01, 0.095, -0.05] 89 | "radius": 0.025 90 | panda_link6: 91 | - "center": [0.0, 0.0, 0.0] 92 | "radius": 0.05 93 | - "center": [0.08, 0.035, 0.0] 94 | "radius": 0.052 95 | - "center": [0.08, -0.01, 0.0] 96 | "radius": 0.05 97 | panda_link7: 98 | - "center": [0.0, 0.0, 0.07] 99 | "radius": 0.05 100 | - "center": [0.02, 0.04, 0.08] 101 | "radius": 0.025 102 | - "center": [0.04, 0.02, 0.08] 103 | "radius": 0.025 104 | - "center": [0.04, 0.06, 0.085] 105 | "radius": 0.02 106 | - "center": [0.06, 0.04, 0.085] 107 | "radius": 0.02 108 | panda_hand: 109 | - "center": [0.0, -0.075, 0.01] 110 | "radius": 0.028 111 | - "center": [0.0, -0.045, 0.01] 112 | "radius": 0.028 113 | - "center": [0.0, -0.015, 0.01] 114 | "radius": 0.028 115 | - "center": [0.0, 0.015, 0.01] 116 | "radius": 0.028 117 | - "center": [0.0, 0.045, 0.01] 118 | "radius": 0.028 119 | - "center": [0.0, 0.075, 0.01] 120 | "radius": 0.028 121 | - "center": [0.0, -0.075, 0.03] 122 | "radius": 0.026 123 | - "center": [0.0, -0.045, 0.03] 124 | "radius": 0.026 125 | - "center": [0.0, -0.015, 0.03] 126 | "radius": 0.026 127 | - "center": [0.0, 0.015, 0.03] 128 | "radius": 0.026 129 | - "center": [0.0, 0.045, 0.03] 130 | "radius": 0.026 131 | - "center": [0.0, 0.075, 0.03] 132 | "radius": 0.026 133 | - "center": [0.0, -0.075, 0.05] 134 | "radius": 0.024 135 | - "center": [0.0, -0.045, 0.05] 136 | "radius": 0.024 137 | - "center": [0.0, -0.015, 0.05] 138 | "radius": 0.024 139 | - "center": [0.0, 0.015, 0.05] 140 | "radius": 0.024 141 | - "center": [0.0, 0.045, 0.05] 142 | "radius": 0.024 143 | - "center": [0.0, 0.075, 0.05] 144 | "radius": 0.024 145 | -------------------------------------------------------------------------------- /content/configs/robot/franka_real_robot.yml: -------------------------------------------------------------------------------- 1 | ## 2 | ## MIT License 3 | ## 4 | ## Copyright (c) 2020-2022 NVIDIA CORPORATION. 5 | ## 6 | ## Permission is hereby granted, free of charge, to any person obtaining a 7 | ## copy of this software and associated documentation files (the "Software"), 8 | ## to deal in the Software without restriction, including without limitation 9 | ## the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | ## and/or sell copies of the Software, and to permit persons to whom the 11 | ## Software is furnished to do so, subject to the following conditions: 12 | ## 13 | ## The above copyright notice and this permission notice shall be included in 14 | ## all copies or substantial portions of the Software. 15 | ## 16 | ## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | ## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | ## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | ## THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | ## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | ## FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | ## DEALINGS IN THE SOFTWARE.## 23 | robot: 'Franka Panda' 24 | 25 | collision_spheres: 26 | panda_link0: 27 | - "center": [-0.08, 0.0, 0.05] 28 | "radius": 0.06 29 | - "center": [-0.0, 0.0, 0.05] 30 | "radius": 0.08 31 | panda_link1: 32 | - "center": [0.0, -0.08, 0.0] 33 | "radius": 0.1 34 | - "center": [0.0, -0.03, 0.0] 35 | "radius": 0.1 36 | - "center": [0.0, 0.0, -0.12] 37 | "radius": 0.06 38 | - "center": [0.0, 0.0, -0.17] 39 | "radius": 0.06 40 | panda_link2: 41 | - "center": [0.0, 0.0, 0.03] 42 | "radius": 0.1 43 | - "center": [0.0, 0.0, 0.08] 44 | "radius": 0.1 45 | - "center": [0.0, -0.12, 0.0] 46 | "radius": 0.06 47 | - "center": [0.0, -0.17, 0.0] 48 | "radius": 0.06 49 | panda_link3: 50 | - "center": [0.0, 0.0, -0.06] 51 | "radius": 0.05 52 | - "center": [0.0, 0.0, -0.1] 53 | "radius": 0.06 54 | - "center": [0.08, 0.06, 0.0] 55 | "radius": 0.055 56 | - "center": [0.08, 0.02, 0.0] 57 | "radius": 0.055 58 | panda_link4: 59 | - "center": [0.0, 0.0, 0.02] 60 | "radius": 0.055 61 | - "center": [0.0, 0.0, 0.06] 62 | "radius": 0.055 63 | - "center": [-0.08, 0.095, 0.0] 64 | "radius": 0.06 65 | - "center": [-0.08, 0.06, 0.0] 66 | "radius": 0.055 67 | panda_link5: 68 | - "center": [0.0, 0.055, 0.0] 69 | "radius": 0.05 70 | - "center": [0.0, 0.085, 0.0] 71 | "radius": 0.055 72 | - "center": [0.0, 0.000, -0.22] 73 | "radius": 0.05 74 | - "center": [0.0, 0.05, -0.18] 75 | "radius": 0.045 76 | - "center": [0.015, 0.08, -0.14] 77 | "radius": 0.03 78 | - "center": [0.015, 0.085, -0.11] 79 | "radius": 0.03 80 | - "center": [0.015, 0.09, -0.08] 81 | "radius": 0.03 82 | - "center": [0.015, 0.095, -0.05] 83 | "radius": 0.03 84 | - "center": [-0.015, 0.08, -0.14] 85 | "radius": 0.03 86 | - "center": [-0.015, 0.085, -0.11] 87 | "radius": 0.03 88 | - "center": [-0.015, 0.09, -0.08] 89 | "radius": 0.03 90 | - "center": [-0.015, 0.095, -0.05] 91 | "radius": 0.03 92 | panda_link6: 93 | - "center": [0.0, 0.0, 0.0] 94 | "radius": 0.05 95 | - "center": [0.08, 0.035, 0.0] 96 | "radius": 0.052 97 | - "center": [0.08, -0.01, 0.0] 98 | "radius": 0.05 99 | panda_link7: 100 | - "center": [0.0, 0.0, 0.07] 101 | "radius": 0.05 102 | - "center": [0.02, 0.04, 0.08] 103 | "radius": 0.025 104 | - "center": [0.04, 0.02, 0.08] 105 | "radius": 0.025 106 | - "center": [0.04, 0.06, 0.085] 107 | "radius": 0.02 108 | - "center": [0.06, 0.04, 0.085] 109 | "radius": 0.02 110 | panda_hand: 111 | - "center": [0.0, -0.08, 0.01] 112 | "radius": 0.03 113 | - "center": [0.0, -0.045, 0.01] 114 | "radius": 0.03 115 | - "center": [0.0, -0.015, 0.01] 116 | "radius": 0.03 117 | - "center": [0.0, 0.015, 0.01] 118 | "radius": 0.03 119 | - "center": [0.0, 0.045, 0.01] 120 | "radius": 0.03 121 | - "center": [0.0, 0.08, 0.01] 122 | "radius": 0.03 123 | - "center": [0.0, 0.065, -0.02] 124 | "radius": 0.05 125 | - "center": [0.0, -0.08, 0.05] 126 | "radius": 0.05 127 | - "center": [0.0, -0.045, 0.05] 128 | "radius": 0.05 129 | - "center": [0.0, -0.015, 0.05] 130 | "radius": 0.05 131 | - "center": [0.0, 0.015, 0.05] 132 | "radius": 0.05 133 | - "center": [0.0, 0.045, 0.05] 134 | "radius": 0.05 135 | - "center": [0.0, 0.08, 0.05] 136 | "radius": 0.05 137 | - "center": [0.0, 0.08, 0.08] 138 | "radius": 0.05 139 | - "center": [0.0, -0.08, 0.08] 140 | "radius": 0.05 141 | - "center": [0.0, 0.05, 0.08] 142 | "radius": 0.05 143 | - "center": [0.0, -0.05, 0.08] 144 | "radius": 0.05 145 | - "center": [0.0, 0.0, 0.08] 146 | "radius": 0.05 147 | panda_leftfinger: 148 | - "center": [0.0, 0.01, 0.034] 149 | "radius": 0.02 150 | panda_rightfinger: 151 | - "center": [0.0, -0.01, 0.034] 152 | "radius": 0.02 153 | 154 | -------------------------------------------------------------------------------- /content/configs/robot/kinova_gen3.yml: -------------------------------------------------------------------------------- 1 | ## 2 | ## MIT License 3 | ## 4 | ## Copyright (c) 2020-2022 NVIDIA CORPORATION. 5 | ## 6 | ## Permission is hereby granted, free of charge, to any person obtaining a 7 | ## copy of this software and associated documentation files (the "Software"), 8 | ## to deal in the Software without restriction, including without limitation 9 | ## the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | ## and/or sell copies of the Software, and to permit persons to whom the 11 | ## Software is furnished to do so, subject to the following conditions: 12 | ## 13 | ## The above copyright notice and this permission notice shall be included in 14 | ## all copies or substantial portions of the Software. 15 | ## 16 | ## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | ## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | ## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | ## THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | ## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | ## FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | ## DEALINGS IN THE SOFTWARE.## 23 | robot: 'Kinova Gen3' 24 | 25 | collision_spheres: 26 | base_link: 27 | - "center": [0.0, 0.0, 0.0] 28 | "radius": 0.05 29 | - "center": [0.0, 0.0, 0.1] 30 | "radius": 0.05 31 | shoulder_link: 32 | - "center": [0.0, 0.0, -0.1] 33 | "radius": 0.06 34 | - "center": [0.0, 0.0, -0.15] 35 | "radius": 0.05 36 | half_arm_1_link: 37 | - "center": [0.0, -0.0, 0.0] 38 | "radius": 0.055 39 | - "center": [0.0, -0.07, 0.0] 40 | "radius": 0.055 41 | - "center": [0.0, -0.15,0.0] 42 | "radius": 0.055 43 | half_arm_2_link: 44 | - "center": [0.0, -0.0, 0.0] 45 | "radius": 0.055 46 | - "center": [0.0, -0.0, -0.07] 47 | "radius": 0.055 48 | - "center": [0.0, -0.0,-0.15] 49 | "radius": 0.055 50 | - "center": [0.0, -0.0,-0.21] 51 | "radius": 0.055 52 | forearm_link: 53 | - "center": [0.0, -0.0, 0.0] 54 | "radius": 0.055 55 | - "center": [0.0, -0.07, -0.0] 56 | "radius": 0.055 57 | - "center": [0.0, -0.17,-0.0] 58 | "radius": 0.055 59 | spherical_wrist_1_link: 60 | - "center": [0.0, -0.0, 0.0] 61 | "radius": 0.055 62 | - "center": [0.0, -0.0, -0.085] 63 | "radius": 0.055 64 | spherical_wrist_2_link: 65 | - "center": [0.0, -0.0, 0.0] 66 | "radius": 0.05 67 | - "center": [0.0, -0.085, -0.0] 68 | "radius": 0.05 69 | bracelet_link: 70 | - "center": [0.0, -0.0, -0.05] 71 | "radius": 0.04 72 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the "Software"), 7 | # to deal in the Software without restriction, including without limitation 8 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 9 | # and/or sell copies of the Software, and to permit persons to whom the 10 | # Software is furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in 13 | # all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 18 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 21 | # DEALINGS IN THE SOFTWARE. 22 | # 23 | 24 | 25 | # Configuration file for the Sphinx documentation builder. 26 | # 27 | # This file only contains a selection of the most common options. For a full 28 | # list see the documentation: 29 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 30 | 31 | # -- Path setup -------------------------------------------------------------- 32 | 33 | # If extensions (or modules to document with autodoc) are in another directory, 34 | # add these directories to sys.path here. If the directory is relative to the 35 | # documentation root, use os.path.abspath to make it absolute, like shown here. 36 | # 37 | import sphinx_rtd_theme 38 | import os 39 | import sys 40 | sys.path.insert(0, os.path.abspath('..')) 41 | sys.path.insert(0, os.path.abspath('.')) 42 | 43 | # -- Project information ----------------------------------------------------- 44 | 45 | project = 'STORM Toolkit' 46 | copyright = '2021, NVIDIA' 47 | author = 'NVIDIA' 48 | 49 | 50 | # -- General configuration --------------------------------------------------- 51 | 52 | # Add any Sphinx extension module names here, as strings. They can be 53 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 54 | # ones. 55 | extensions = ['sphinx.ext.autodoc','sphinx.ext.napoleon','sphinx.ext.todo', 'sphinx_autodoc_annotation','sphinx.ext.autosummary', 'sphinx_rtd_theme','sphinx.ext.mathjax','sphinx.ext.viewcode', 'sphinx.ext.coverage','sphinx.ext.intersphinx', 'sphinx.ext.inheritance_diagram', 'sphinx.ext.graphviz', 'm2r2'] 56 | 57 | # Add any paths that contain templates here, relative to this directory. 58 | templates_path = ['_templates'] 59 | 60 | # List of patterns, relative to source directory, that match files and 61 | # directories to ignore when looking for source files. 62 | # This pattern also affects html_static_path and html_extra_path. 63 | exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store','setup.py'] 64 | 65 | 66 | # -- Options for HTML output ------------------------------------------------- 67 | 68 | # The theme to use for HTML and HTML Help pages. See the documentation for 69 | # a list of builtin themes. 70 | # 71 | html_theme = 'sphinx_rtd_theme' 72 | 73 | 74 | # Add any paths that contain custom static files (such as style sheets) here, 75 | # relative to this directory. They are copied after the builtin static files, 76 | # so a file named "default.css" will overwrite the builtin "default.css". 77 | html_static_path = ['_static'] 78 | autosummary_generate = True 79 | add_module_names = False 80 | autoclass_content = "both" 81 | #numpydoc_show_class_members = False 82 | intersphinx_mapping = { 83 | 'torch': ('https://pytorch.org/docs/stable/', None), 84 | } 85 | #$inheritance_graph_attrs = dict(rankdir="TB", size='""') 86 | source_suffix = ['.rst','.md'] 87 | -------------------------------------------------------------------------------- /docs/generate_docs.sh: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a 6 | # copy of this software and associated documentation files (the "Software"), 7 | # to deal in the Software without restriction, including without limitation 8 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 9 | # and/or sell copies of the Software, and to permit persons to whom the 10 | # Software is furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in 13 | # all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 18 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 21 | # DEALINGS IN THE SOFTWARE. 22 | # 23 | 24 | sphinx-apidoc -o . .. -M -P -f -e -l -q 25 | make html 26 | xdg-open _build/html/index.html 27 | 28 | -------------------------------------------------------------------------------- /docs/images/coll_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/docs/images/coll_demo.gif -------------------------------------------------------------------------------- /docs/images/constrained_storm.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/docs/images/constrained_storm.gif -------------------------------------------------------------------------------- /docs/images/mpc_approach.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/docs/images/mpc_approach.png -------------------------------------------------------------------------------- /docs/images/mpc_framework.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/storm/aacf70d0d18b159f8c37003dac071e3c28ba7c0b/docs/images/mpc_framework.png -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | .. storm documentation master file, created by 2 | sphinx-quickstart on Wed Dec 2 09:22:22 2020. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | STORM 7 | ===== 8 | 9 | **Stochastic Tensor Optimization for Robot Motion** - *A GPU Robot Motion Toolkit* 10 | 11 | This package contains code for reactive robot motion leveraging parallel compute on the GPU. 12 | 13 | .. image:: images/coll_demo.gif 14 | :width: 500px 15 | 16 | The proposed control framework leverages MPPI ("control" block in below image) to optimize over sampled actions and their costs. The costs are computed by rolling out the forward model from the current state with the sampled actions. 17 | 18 | 19 | .. image:: images/mpc_approach.png 20 | :width: 500px 21 | 22 | .. mdinclude:: ../install_instructions.md 23 | .. include:: storm_kit.rst 24 | 25 | 26 | 27 | Indices and tables 28 | ================== 29 | 30 | * :ref:`genindex` 31 | * :ref:`modindex` 32 | * :ref:`search` 33 | -------------------------------------------------------------------------------- /environment.yml: -------------------------------------------------------------------------------- 1 | ## MIT License 2 | ## 3 | ## Copyright (c) 2020-2021 NVIDIA CORPORATION. 4 | ## 5 | ## Permission is hereby granted, free of charge, to any person obtaining a 6 | ## copy of this software and associated documentation files (the "Software"), 7 | ## to deal in the Software without restriction, including without limitation 8 | ## the rights to use, copy, modify, merge, publish, distribute, sublicense, 9 | ## and/or sell copies of the Software, and to permit persons to whom the 10 | ## Software is furnished to do so, subject to the following conditions: 11 | ## 12 | ## The above copyright notice and this permission notice shall be included in 13 | ## all copies or substantial portions of the Software. 14 | ## 15 | ## THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | ## IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | ## FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 18 | ## THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | ## LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | ## FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 21 | ## DEALINGS IN THE SOFTWARE.## 22 | 23 | name: storm_kit 24 | channels: 25 | - pytorch 26 | - conda-forge 27 | dependencies: 28 | - python=3.7 29 | - cudatoolkit=11.1 30 | - pytorch 31 | - torchvision 32 | - torchaudio 33 | - pyzmq 34 | - numpy 35 | - pytest 36 | - colorlog 37 | - pandas 38 | - tabulate 39 | - pip 40 | - sk-video 41 | - matplotlib 42 | - quaternion 43 | - pip: 44 | - pytorch3d 45 | - ghalton 46 | - argcomplete 47 | - click 48 | - cloudpickle 49 | - imageio-ffmpeg 50 | - pyyaml 51 | - scipy 52 | - tqdm 53 | - urdfpy 54 | - wheel 55 | - hydra-core 56 | - urdf_parser_py 57 | - sphinx-rtd-theme 58 | - sphinx 59 | - sphinx-autodoc-annotation 60 | - hydra-core 61 | - urdf_parser_py 62 | - numpy-quaternion 63 | - opencv-python 64 | - open3d 65 | - m2r2 66 | 67 | -------------------------------------------------------------------------------- /install_instructions.md: -------------------------------------------------------------------------------- 1 | ## Install Instructions 2 | 3 | ### System Dependencies: 4 | - Conda version >= 4.9 5 | - NVIDIA driver >= 460.32 6 | - Cuda toolkit >= 11.0 7 | 8 | Steps: 9 | 10 | 1. Create a new conda environment with: conda env create -f environment.yml 11 | 12 | 2. Install python bindings for isaacgym: https://developer.nvidia.com/isaac-gym 13 | 14 | 3. run the following command from this directory: pip install -e . 15 | 16 | ### Running Example 17 | 18 | 1. run scripts/train_self_collision.py to get weights for robot self collision checking. 19 | 20 | 2. Run python franka_reacher.py, which will launch isaac gym with a franka robot trying to reach a red mug. In the isaac gym gui, search for "ee_target" and toggle "Edit DOF", now you can move the target pose by using the sliders. 21 | 22 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | """A setuptools based setup module. 2 | See: 3 | https://packaging.python.org/guides/distributing-packages-using-setuptools/ 4 | https://github.com/pypa/sampleproject 5 | """ 6 | 7 | # Always prefer setuptools over distutils 8 | from setuptools import setup, find_packages 9 | import pathlib 10 | 11 | here = pathlib.Path(__file__).parent.resolve() 12 | 13 | # Get the long description from the README file 14 | long_description = (here / 'README.md').read_text(encoding='utf-8') 15 | 16 | # Arguments marked as "Required" below must be included for upload to PyPI. 17 | # Fields marked as "Optional" may be commented out. 18 | 19 | setup( 20 | name='storm_kit', 21 | version='0.1', 22 | description='Package for GPU Robot Control', 23 | long_description=long_description, 24 | long_description_content_type='text/markdown', 25 | 26 | classifiers=[ # Optional 27 | # How mature is this project? Common values are 28 | # 3 - Alpha 29 | # 4 - Beta 30 | # 5 - Production/Stable 31 | 'Development Status :: 3 - Alpha', 32 | 33 | # Indicate who your project is intended for 34 | 'Intended Audience :: Developers', 35 | #'Topic :: Software Development :: Build Tools', 36 | 37 | # Pick your license as you wish 38 | #'License :: OSI Approved :: MIT License', 39 | 40 | # Specify the Python versions you support here. In particular, ensure 41 | # that you indicate you support Python 3. These classifiers are *not* 42 | # checked by 'pip install'. See instead 'python_requires' below. 43 | 'Programming Language :: Python :: 3', 44 | 'Programming Language :: Python :: 3.5', 45 | 'Programming Language :: Python :: 3.6', 46 | 'Programming Language :: Python :: 3.7', 47 | 'Programming Language :: Python :: 3.8', 48 | 'Programming Language :: Python :: 3 :: Only', 49 | ], 50 | 51 | keywords='isaac_gym, robot_control', # Optional 52 | packages=find_packages(where=''), # Required 53 | python_requires='>=3.5, <4', 54 | ) 55 | -------------------------------------------------------------------------------- /storm_kit/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. All rights reserved. 3 | # 4 | # Permission is hereby granted, free of charge, to any person obtaining a 5 | # copy of this software and associated documentation files (the "Software"), 6 | # to deal in the Software without restriction, including without limitation 7 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | # and/or sell copies of the Software, and to permit persons to whom the 9 | # Software is furnished to do so, subject to the following conditions: 10 | # 11 | # The above copyright notice and this permission notice shall be included in 12 | # all copies or substantial portions of the Software. 13 | # 14 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | # DEALINGS IN THE SOFTWARE. 21 | # 22 | -------------------------------------------------------------------------------- /storm_kit/differentiable_robot_model/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2020-2021 NVIDIA CORPORATION. All rights reserved. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a 4 | copy of this software and associated documentation files (the "Software"), 5 | to deal in the Software without restriction, including without limitation 6 | the rights to use, copy, modify, merge, publish, distribute, sublicense, 7 | and/or sell copies of the Software, and to permit persons to whom the 8 | Software is furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in 11 | all copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 18 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 19 | DEALINGS IN THE SOFTWARE. 20 | 21 | ********************************************************************** 22 | The first version of many files were licensed as 23 | "Original Source License"(see below). Several enhancements and bug fixes 24 | were done at NVIDIA CORPORATION since obtaining the first version. 25 | 26 | 27 | 28 | Original Source License: 29 | 30 | MIT License 31 | 32 | Copyright (c) Facebook, Inc. and its affiliates. 33 | 34 | Permission is hereby granted, free of charge, to any person obtaining a copy 35 | of this software and associated documentation files (the "Software"), to deal 36 | in the Software without restriction, including without limitation the rights 37 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 38 | copies of the Software, and to permit persons to whom the Software is 39 | furnished to do so, subject to the following conditions: 40 | 41 | The above copyright notice and this permission notice shall be included in all 42 | copies or substantial portions of the Software. 43 | 44 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 45 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 46 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 47 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 48 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 49 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 50 | SOFTWARE. -------------------------------------------------------------------------------- /storm_kit/differentiable_robot_model/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE. 23 | # 24 | # ********************************************************************** 25 | # The first version was licensed as "Original Source License"(see below). 26 | # Several enhancements and bug fixes were done at NVIDIA CORPORATION 27 | # since obtaining the first version. 28 | # 29 | # 30 | # 31 | # Original Source License: 32 | # 33 | # MIT License 34 | # 35 | # Copyright (c) Facebook, Inc. and its affiliates. 36 | # 37 | # Permission is hereby granted, free of charge, to any person obtaining a copy 38 | # of this software and associated documentation files (the "Software"), to deal 39 | # in the Software without restriction, including without limitation the rights 40 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 41 | # copies of the Software, and to permit persons to whom the Software is 42 | # furnished to do so, subject to the following conditions: 43 | # 44 | # The above copyright notice and this permission notice shall be included in all 45 | # copies or substantial portions of the Software. 46 | # 47 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 48 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 49 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 50 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 51 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 52 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 53 | # SOFTWARE.# 54 | from .differentiable_robot_model import (DifferentiableRobotModel, 55 | LearnableRigidBodyConfig) 56 | -------------------------------------------------------------------------------- /storm_kit/differentiable_robot_model/utils.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE. 23 | # 24 | # ********************************************************************** 25 | # The first version was licensed as "Original Source License"(see below). 26 | # Several enhancements and bug fixes were done at NVIDIA CORPORATION 27 | # since obtaining the first version. 28 | # 29 | # 30 | # 31 | # Original Source License: 32 | # 33 | # MIT License 34 | # 35 | # Copyright (c) Facebook, Inc. and its affiliates. 36 | # 37 | # Permission is hereby granted, free of charge, to any person obtaining a copy 38 | # of this software and associated documentation files (the "Software"), to deal 39 | # in the Software without restriction, including without limitation the rights 40 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 41 | # copies of the Software, and to permit persons to whom the Software is 42 | # furnished to do so, subject to the following conditions: 43 | # 44 | # The above copyright notice and this permission notice shall be included in all 45 | # copies or substantial portions of the Software. 46 | # 47 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 48 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 49 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 50 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 51 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 52 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 53 | # SOFTWARE.# 54 | 55 | # Copyright (c) Facebook, Inc. and its affiliates. 56 | 57 | import random 58 | from contextlib import contextmanager 59 | 60 | import numpy as np 61 | import timeit 62 | import torch 63 | import operator 64 | from functools import reduce 65 | 66 | 67 | prod = lambda l: reduce(operator.mul, l, 1) 68 | torch.set_default_tensor_type(torch.DoubleTensor) 69 | 70 | def cross_product(vec3a, vec3b): 71 | vec3a = convert_into_at_least_2d_pytorch_tensor(vec3a) 72 | vec3b = convert_into_at_least_2d_pytorch_tensor(vec3b) 73 | skew_symm_mat_a = vector3_to_skew_symm_matrix(vec3a) 74 | return (skew_symm_mat_a @ vec3b.unsqueeze(2)).squeeze(2) 75 | 76 | 77 | def bfill_lowertriangle(A: torch.Tensor, vec: torch.Tensor): 78 | ii, jj = np.tril_indices(A.size(-2), k=-1, m=A.size(-1)) 79 | A[..., ii, jj] = vec 80 | return A 81 | 82 | 83 | def bfill_diagonal(A: torch.Tensor, vec: torch.Tensor): 84 | ii, jj = np.diag_indices(min(A.size(-2), A.size(-1))) 85 | A[..., ii, jj] = vec 86 | return A 87 | 88 | 89 | def vector3_to_skew_symm_matrix(vec3): 90 | vec3 = convert_into_at_least_2d_pytorch_tensor(vec3) 91 | batch_size = vec3.shape[0] 92 | skew_symm_mat = vec3.new_zeros((batch_size, 3, 3)) 93 | skew_symm_mat[:, 0, 1] = -vec3[:, 2] 94 | skew_symm_mat[:, 0, 2] = vec3[:, 1] 95 | skew_symm_mat[:, 1, 0] = vec3[:, 2] 96 | skew_symm_mat[:, 1, 2] = -vec3[:, 0] 97 | skew_symm_mat[:, 2, 0] = -vec3[:, 1] 98 | skew_symm_mat[:, 2, 1] = vec3[:, 0] 99 | return skew_symm_mat 100 | 101 | 102 | def torch_square(x): 103 | return x * x 104 | 105 | 106 | def exp_map_so3(omega, epsilon=1.0e-14): 107 | omegahat = vector3_to_skew_symm_matrix(omega).squeeze() 108 | 109 | norm_omega = torch.norm(omega, p=2) 110 | exp_omegahat = (torch.eye(3) + 111 | ((torch.sin(norm_omega) / (norm_omega + epsilon)) * omegahat) + 112 | (((1.0 - torch.cos(norm_omega)) / (torch_square(norm_omega + epsilon))) * 113 | (omegahat @ omegahat)) 114 | ) 115 | return exp_omegahat 116 | 117 | 118 | 119 | def convert_into_pytorch_tensor(variable): 120 | if isinstance(variable, torch.Tensor): 121 | return variable 122 | elif isinstance(variable, np.ndarray): 123 | return torch.Tensor(variable) 124 | else: 125 | return torch.Tensor(variable) 126 | 127 | 128 | def convert_into_at_least_2d_pytorch_tensor(variable): 129 | tensor_var = convert_into_pytorch_tensor(variable) 130 | if len(tensor_var.shape) == 1: 131 | return tensor_var.unsqueeze(0) 132 | else: 133 | return tensor_var 134 | 135 | -------------------------------------------------------------------------------- /storm_kit/geom/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | 24 | """ 25 | This module contains code for geometric processing such as pointcloud processing, analytic signed distance computation for the environment, and also signed distance computation between robot and the environment. These functions can be used for robot self collision checking and also for robot environment collision checking. The module also contains neural networks for learned signed distance fields. 26 | """ 27 | -------------------------------------------------------------------------------- /storm_kit/geom/geom_types.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | 25 | from ..differentiable_robot_model.coordinate_transform import CoordinateTransform 26 | 27 | def tensor_circle(pt, radius, tensor=None, tensor_args={'device':"cpu", 'dtype':torch.float32}): 28 | if(tensor is None): 29 | tensor = torch.empty(3, **tensor_args) 30 | tensor[:2] = torch.as_tensor(pt, **tensor_args) 31 | tensor[2] = radius 32 | return tensor 33 | 34 | def tensor_sphere(pt, radius, tensor=None, tensor_args={'device':"cpu", 'dtype':torch.float32}): 35 | if(tensor is None): 36 | tensor = torch.empty(4, **tensor_args) 37 | tensor[:3] = torch.as_tensor(pt, **tensor_args) 38 | tensor[3] = radius 39 | return tensor 40 | 41 | def tensor_capsule(base, tip, radius, tensor=None, tensor_args={'device':"cpu", 'dtype':torch.float32}): 42 | if(tensor is None): 43 | tensor = torch.empty(7, **tensor_args) 44 | tensor[:3] = torch.as_tensor(base, **tensor_args) 45 | tensor[3:6] = torch.as_tensor(tip, **tensor_args) 46 | tensor[6] = radius 47 | return tensor 48 | 49 | 50 | def tensor_cube(pose, dims, tensor_args={'device':"cpu", 'dtype':torch.float32}): 51 | w_T_b = CoordinateTransform(pose=pose, tensor_args=tensor_args) 52 | b_T_w = w_T_b.inverse() 53 | dims_t = torch.tensor([dims[0], dims[1], dims[2]], **tensor_args) 54 | cube = {'trans': w_T_b.translation(), 'rot': w_T_b.rotation(), 55 | 'inv_trans': b_T_w.translation(), 'inv_rot': b_T_w.rotation(), 56 | 'dims':dims_t} 57 | cube = [w_T_b.translation(), w_T_b.rotation(), 58 | b_T_w.translation(), b_T_w.rotation(), 59 | dims_t] 60 | return cube 61 | -------------------------------------------------------------------------------- /storm_kit/geom/nn_model/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | """ 24 | Contains neural networks for learned signed distance fields. 25 | """ 26 | -------------------------------------------------------------------------------- /storm_kit/geom/nn_model/robot_self_collision.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | 24 | import torch 25 | from torch import nn 26 | from torch.nn import Sequential as Seq, Linear as Lin, ReLU, ELU, ReLU6 27 | from .network_macros import MLPRegression, scale_to_base, scale_to_net 28 | from ...util_file import get_weights_path, join_path 29 | 30 | 31 | class RobotSelfCollisionNet(): 32 | """This class loads a network to predict the signed distance given a robot joint config.""" 33 | 34 | def __init__(self, n_joints=0): 35 | """initialize class 36 | 37 | Args: 38 | n_joints (int, optional): Number of joints, same as number of channels for nn input. Defaults to 0. 39 | """ 40 | 41 | super().__init__() 42 | act_fn = ReLU6 43 | in_channels = n_joints 44 | 45 | out_channels = 1 46 | dropout_ratio = 0.1 47 | mlp_layers = [256, 64] 48 | self.model = MLPRegression(in_channels, out_channels, mlp_layers, 49 | dropout_ratio, batch_norm=False, act_fn=act_fn, 50 | layer_norm=False, nerf=True) 51 | 52 | def load_weights(self, f_name, tensor_args): 53 | """Loads pretrained network weights if available. 54 | 55 | Args: 56 | f_name (str): file name, this is relative to weights folder in this repo. 57 | tensor_args (Dict): device and dtype for pytorch tensors 58 | """ 59 | try: 60 | chk = torch.load(join_path(get_weights_path(), f_name)) 61 | self.model.load_state_dict(chk["model_state_dict"]) 62 | self.norm_dict = chk["norm"] 63 | for k in self.norm_dict.keys(): 64 | self.norm_dict[k]['mean'] = self.norm_dict[k]['mean'].to(**tensor_args) 65 | self.norm_dict[k]['std'] = self.norm_dict[k]['std'].to(**tensor_args) 66 | except Exception: 67 | print('WARNING: Weights not loaded') 68 | self.model = self.model.to(**tensor_args) 69 | self.tensor_args = tensor_args 70 | self.model.eval() 71 | 72 | 73 | def compute_signed_distance(self, q): 74 | """Compute the signed distance given the joint config. 75 | 76 | Args: 77 | q (tensor): input batch of joint configs [b, n_joints] 78 | 79 | Returns: 80 | [tensor]: largest signed distance between any two non-consecutive links of the robot. 81 | """ 82 | with torch.no_grad(): 83 | q_scale = scale_to_net(q, self.norm_dict,'x') 84 | dist = self.model.forward(q_scale) 85 | dist_scale = scale_to_base(dist, self.norm_dict, 'y') 86 | return dist_scale 87 | 88 | def check_collision(self, q): 89 | """Check collision given joint config. Requires classifier like training. 90 | 91 | Args: 92 | q (tensor): input batch of joint configs [b, n_joints] 93 | 94 | Returns: 95 | [tensor]: probability of collision of links, from sigmoid value. 96 | """ 97 | with torch.no_grad(): 98 | q_scale = scale_to_net(q, self.norm_dict,'x') 99 | dist = torch.sigmoid(self.model.forward(q_scale)) 100 | return dist 101 | -------------------------------------------------------------------------------- /storm_kit/geom/sdf/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | 24 | # 25 | """ 26 | Contains sdf functions and wrappers for collision checking, signed distance between robot and environment and also between links of a robot. 27 | """ 28 | -------------------------------------------------------------------------------- /storm_kit/geom/utils.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | 24 | import numpy as np 25 | import open3d as o3d 26 | 27 | def get_open3d_pointcloud(points, color=[1,0,0], translation=np.zeros(3),rot=None): 28 | pcd = o3d.geometry.PointCloud() 29 | 30 | if(rot is not None): 31 | # project points: 32 | 33 | points = np.dot(points,rot.T) 34 | 35 | data = o3d.utility.Vector3dVector(points + translation) 36 | 37 | pcd.points = data 38 | color_array = np.array([color for x in range(len(points))]) 39 | color_data = o3d.utility.Vector3dVector(color_array) 40 | 41 | pcd.colors = color_data 42 | return pcd 43 | 44 | 45 | def get_pointcloud_from_depth(camera_data={'proj_matrix':None, 'segmentation':None, 46 | 'depth':None}): 47 | proj_matrix = camera_data['proj_matrix'] 48 | 49 | fu = 2 / proj_matrix[0, 0] 50 | fv = 2 / proj_matrix[1, 1] 51 | seg_buffer = camera_data['segmentation'] 52 | depth_buffer = camera_data['depth'] 53 | cam_width = camera_data['depth'].shape[1] 54 | cam_height = camera_data['depth'].shape[0] 55 | points = [] 56 | # Ignore any points which originate from ground plane or empty space 57 | depth_buffer[seg_buffer == 0] = -10001 58 | #print(cam_width) 59 | vinv = np.linalg.inv(np.matrix(camera_data['view_matrix'])) 60 | 61 | #print(vinv) 62 | centerU = cam_width / 2 63 | centerV = cam_height / 2 64 | pc_seg = [] 65 | for i in range(cam_width): 66 | for j in range(cam_height): 67 | if depth_buffer[j, i] < -10000: 68 | continue 69 | u = -(i-centerU)/(cam_width) # image-space coordinate 70 | v = (j-centerV)/(cam_height) # image-space coordinate 71 | d = depth_buffer[j, i] # depth buffer value 72 | X2 = np.matrix([d*fu*u, d*fv*v, d, 1])#.T # deprojection vector 73 | #p2 = X2 74 | 75 | #print(vinv.shape, X2.shape) 76 | p2 = X2 * vinv #(vinv * X2).T # Inverse camera view to get world coordinates 77 | #print(p2) 78 | points.append([p2[0,0], p2[0,1],p2[0,2]]) 79 | #points.append([p2[0,2], p2[0,0], p2[0,1]]) 80 | pc_seg.append(seg_buffer[j,i]) 81 | camera_data['pc'] = points#np.matrix(points) 82 | camera_data['pc_seg'] = np.ravel(pc_seg) 83 | return camera_data 84 | -------------------------------------------------------------------------------- /storm_kit/gym/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | -------------------------------------------------------------------------------- /storm_kit/gym/helpers.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | 24 | 25 | def load_struct_from_dict(struct_instance, dict_instance): 26 | """This function populates a struct recursively from a dictionary. 27 | 28 | Assumptions: 29 | Currently the struct cannot have a dictionary for one of the objects as that will start recursing. 30 | """ 31 | #print(dict_instance) 32 | for key in dict_instance.keys(): 33 | if(hasattr(struct_instance, key)): 34 | if(isinstance(dict_instance[key],dict)): 35 | sub_struct = load_struct_from_dict(getattr(struct_instance,key), dict_instance[key]) 36 | setattr(struct_instance,key,sub_struct) 37 | else: 38 | setattr(struct_instance,key,dict_instance[key]) 39 | return struct_instance 40 | -------------------------------------------------------------------------------- /storm_kit/gym/kdl_parser.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # MIT License 4 | # 5 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a 8 | # copy of this software and associated documentation files (the "Software"), 9 | # to deal in the Software without restriction, including without limitation 10 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 11 | # and/or sell copies of the Software, and to permit persons to whom the 12 | # Software is furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in 15 | # all copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 20 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 23 | # DEALINGS IN THE SOFTWARE.# 24 | 25 | import numpy as np 26 | 27 | import PyKDL as kdl 28 | from urdf_parser_py.urdf import Robot 29 | 30 | def euler_to_quat(r, p, y): 31 | sr, sp, sy = np.sin(r/2.0), np.sin(p/2.0), np.sin(y/2.0) 32 | cr, cp, cy = np.cos(r/2.0), np.cos(p/2.0), np.cos(y/2.0) 33 | return [sr*cp*cy - cr*sp*sy, 34 | cr*sp*cy + sr*cp*sy, 35 | cr*cp*sy - sr*sp*cy, 36 | cr*cp*cy + sr*sp*sy] 37 | 38 | def urdf_pose_to_kdl_frame(pose): 39 | pos = [0., 0., 0.] 40 | rot = [0., 0., 0.] 41 | if pose is not None: 42 | if pose.position is not None: 43 | pos = pose.position 44 | if pose.rotation is not None: 45 | rot = pose.rotation 46 | return kdl.Frame(kdl.Rotation.Quaternion(*euler_to_quat(*rot)), 47 | kdl.Vector(*pos)) 48 | 49 | def urdf_joint_to_kdl_joint(jnt): 50 | origin_frame = urdf_pose_to_kdl_frame(jnt.origin) 51 | if jnt.joint_type == 'fixed': 52 | return kdl.Joint(jnt.name, kdl.Joint.Fixed) 53 | axis = kdl.Vector(*jnt.axis) 54 | if jnt.joint_type == 'revolute': 55 | return kdl.Joint(jnt.name, origin_frame.p, 56 | origin_frame.M * axis, kdl.Joint.RotAxis) 57 | if jnt.joint_type == 'continuous': 58 | return kdl.Joint(jnt.name, origin_frame.p, 59 | origin_frame.M * axis, kdl.Joint.RotAxis) 60 | if jnt.joint_type == 'prismatic': 61 | return kdl.Joint(jnt.name, origin_frame.p, 62 | origin_frame.M * axis, kdl.Joint.TransAxis) 63 | print ("Unknown joint type: %s." % jnt.joint_type) 64 | return kdl.Joint(jnt.name, kdl.Joint.Fixed) # TODO: currently unknown joints are set as fixed 65 | 66 | def urdf_inertial_to_kdl_rbi(i): 67 | origin = urdf_pose_to_kdl_frame(i.origin) 68 | rbi = kdl.RigidBodyInertia(i.mass, origin.p, 69 | kdl.RotationalInertia(i.inertia.ixx, 70 | i.inertia.iyy, 71 | i.inertia.izz, 72 | i.inertia.ixy, 73 | i.inertia.ixz, 74 | i.inertia.iyz)) 75 | return origin.M * rbi 76 | 77 | ## 78 | # Returns a PyKDL.Tree generated from a urdf_parser_py.urdf.URDF object. 79 | def kdl_tree_from_urdf_model(urdf): 80 | root = urdf.get_root() 81 | tree = kdl.Tree(root) 82 | def add_children_to_tree(parent): 83 | if parent in urdf.child_map: 84 | for joint, child_name in urdf.child_map[parent]: 85 | child = urdf.link_map[child_name] 86 | if child.inertial is not None: 87 | kdl_inert = urdf_inertial_to_kdl_rbi(child.inertial) 88 | else: 89 | kdl_inert = kdl.RigidBodyInertia() 90 | kdl_jnt = urdf_joint_to_kdl_joint(urdf.joint_map[joint]) 91 | kdl_origin = urdf_pose_to_kdl_frame(urdf.joint_map[joint].origin) 92 | kdl_sgm = kdl.Segment(child_name, kdl_jnt, 93 | kdl_origin, kdl_inert) 94 | tree.addSegment(kdl_sgm, parent) 95 | add_children_to_tree(child_name) 96 | add_children_to_tree(root) 97 | return tree 98 | -------------------------------------------------------------------------------- /storm_kit/mpc/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | 24 | """ 25 | Different components of our model predictive control framework is described below. 26 | 27 | .. image:: images/mpc_framework.png 28 | """ 29 | -------------------------------------------------------------------------------- /storm_kit/mpc/control/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | from .mppi import MPPI 24 | 25 | #__all__ = ["Controller", "OLGaussianMPC", "MPPI"] 26 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | from .dist_cost import DistCost 24 | from .finite_difference_cost import FiniteDifferenceCost 25 | from .jacobian_cost import JacobianCost 26 | from .pose_cost import PoseCost 27 | from .stop_cost import StopCost 28 | from .projected_dist_cost import ProjectedDistCost 29 | from .null_costs import get_inv_null_cost, get_transpose_null_cost 30 | from .zero_cost import ZeroCost 31 | from .ee_vel_cost import EEVelCost 32 | 33 | from .collision_cost import CollisionCost 34 | from .primitive_collision_cost import PrimitiveCollisionCost 35 | from .voxel_collision_cost import VoxelCollisionCost 36 | 37 | try: 38 | True 39 | #from .scene_nn_collision_cost import SceneNNCollisionCost 40 | except ImportError: 41 | pass 42 | 43 | __all__ = ['DistCost', 'FiniteDifferenceCost', 'JacobianCost', 'PoseCost', 'ProjectedDistCost', \ 44 | 'ZeroCost', 'get_inv_null_cost','get_transpose_null_cost'] 45 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/bound_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | # import torch.nn.functional as F 26 | from .gaussian_projection import GaussianProjection 27 | 28 | class BoundCost(nn.Module): 29 | def __init__(self, tensor_args={'device':torch.device('cpu'), 'dtype':torch.float64}, 30 | bounds=[], weight=1.0, gaussian_params={}, bound_thresh=0.1): 31 | super(BoundCost, self).__init__() 32 | self.tensor_args = tensor_args 33 | self.weight = torch.as_tensor(weight, **tensor_args) 34 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 35 | 36 | self.bounds = torch.as_tensor(bounds, **tensor_args) 37 | self.bnd_range = (self.bounds[:,1] - self.bounds[:,0]) / 2.0 38 | self.t_mat = None 39 | self.bound_thresh = bound_thresh * self.bnd_range 40 | self.bounds[:,1] -= self.bound_thresh 41 | self.bounds[:,0] += self.bound_thresh 42 | def forward(self, state_batch): 43 | inp_device = state_batch.device 44 | 45 | bound_mask = torch.logical_and(state_batch < self.bounds[:,1], 46 | state_batch > self.bounds[:,0]) 47 | 48 | cost = torch.minimum(torch.square(state_batch - self.bounds[:,0]),torch.square(self.bounds[:,1] - state_batch)) 49 | 50 | cost[bound_mask] = 0.0 51 | 52 | cost = (torch.sum(cost, dim=-1)) 53 | cost = self.weight * self.proj_gaussian(torch.sqrt(cost)) 54 | 55 | return cost.to(inp_device) 56 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/capsule_collision_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | # import torch.nn.functional as F 26 | 27 | from .gaussian_projection import GaussianProjection 28 | 29 | class CapsuleCollisionCost(nn.Module): 30 | def __init__(self, weight=None, world_params=None, robot_params=None, gaussian_params={}, device=torch.device('cpu'), float_dtype=torch.float32): 31 | super(CapsuleCollisionCost, self).__init__() 32 | self.device = device 33 | self.float_dtype = float_dtype 34 | self.tensor_args = {'device':device, 'dtype':float_dtype} 35 | self.weight = torch.as_tensor(weight, device=device, dtype=float_dtype) 36 | 37 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 38 | # BUILD world spheres: 39 | self.radius = [] 40 | self.position = [] 41 | for obj in world_params['world_model']['coll_objs'].keys(): 42 | self.radius.append(torch.tensor(world_params['world_model']['coll_objs'][obj]['radius'] + 0.1, **self.tensor_args)) 43 | self.position.append(torch.tensor(world_params['world_model']['coll_objs'][obj]['position'], **self.tensor_args)) 44 | 45 | 46 | 47 | def forward(self, position): 48 | inp_device = position.device 49 | position = position.to(self.device) 50 | i = 0 51 | cost = torch.norm(position - self.position[i],dim=-1) - self.radius[i] 52 | cost[cost > 0.0] = 0.0 53 | 54 | for i in range(1,len(self.position)): 55 | t_cost = torch.norm(position - self.position[i],dim=-1) - self.radius[i] 56 | t_cost[t_cost > 0.0] = 0.0 57 | cost += t_cost 58 | 59 | cost[cost < 0.0] = self.weight 60 | 61 | return cost.to(inp_device) 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/circle_collision_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | import yaml 26 | # import torch.nn.functional as F 27 | from ...geom.geom_types import tensor_circle 28 | from .gaussian_projection import GaussianProjection 29 | from ...util_file import get_configs_path, get_gym_configs_path, join_path, load_yaml, get_assets_path 30 | 31 | 32 | 33 | class CircleCollisionCost(nn.Module): 34 | def __init__(self, weight=None, collision_model=None,gaussian_params={}, tensor_args={'device':torch.device('cpu'), 'dtype':torch.float32}): 35 | super(CircleCollisionCost, self).__init__() 36 | 37 | self.tensor_args = tensor_args 38 | self.weight = torch.as_tensor(weight,**self.tensor_args) 39 | 40 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 41 | 42 | # BUILD world and robot: 43 | world_yml = join_path(get_gym_configs_path(), collision_model) 44 | with open(world_yml) as file: 45 | world_params = yaml.load(file, Loader=yaml.FullLoader) 46 | w_model = world_params['world_model']['coll_objs'] 47 | self.world_spheres = torch.zeros((len(w_model.keys()),3), **tensor_args) 48 | for i,key in enumerate(w_model.keys()): 49 | d = w_model[key] 50 | 51 | self.world_spheres[i,:] = tensor_circle(pt=d['position'], radius=d['radius'], 52 | tensor_args=self.tensor_args) 53 | 54 | self.dist = None 55 | self.t_mat = None 56 | def forward(self, pos_seq): 57 | 58 | inp_device = pos_seq.device 59 | batch_size = pos_seq.shape[0] 60 | horizon = pos_seq.shape[1] 61 | pos_batch = pos_seq.view(batch_size * horizon, 2) 62 | 63 | if(self.dist is None or self.dist.shape[0] != pos_batch.shape[0]): 64 | self.dist = torch.empty((pos_batch.shape[0],self.world_spheres.shape[0]), **self.tensor_args) 65 | for i in range(self.world_spheres.shape[0]): 66 | rel_position = torch.norm(pos_batch - self.world_spheres[i,:2], dim=-1) 67 | self.dist[:, i] = rel_position - self.world_spheres[i,2] 68 | 69 | dist = self.dist 70 | dist = dist.view(batch_size, horizon, self.world_spheres.shape[0]) 71 | # cost only when dist is less 72 | 73 | dist[dist > 0.0] = 0.0 74 | dist *= -1.0 75 | 76 | cost = self.weight * dist.sum(dim=-1) 77 | res = cost 78 | if(self.t_mat is None or self.t_mat.shape[0] != res.shape[1]): 79 | self.t_mat = torch.ones((res.shape[1], res.shape[1]), **self.tensor_args).tril() 80 | 81 | 82 | 83 | 84 | return res.to(inp_device) 85 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/collision_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | # import torch.nn.functional as F 26 | 27 | from .gaussian_projection import GaussianProjection 28 | 29 | class CollisionCost(nn.Module): 30 | def __init__(self, weight=None, world_params=None, gaussian_params={}, device=torch.device('cpu'), float_dtype=torch.float32): 31 | super(CollisionCost, self).__init__() 32 | self.device = device 33 | self.float_dtype = float_dtype 34 | self.tensor_args = {'device':device, 'dtype':float_dtype} 35 | self.weight = torch.as_tensor(weight, device=device, dtype=float_dtype) 36 | 37 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 38 | 39 | self.radius = [] 40 | self.position = [] 41 | for obj in world_params['world_model']['coll_objs'].keys(): 42 | self.radius.append(torch.tensor(world_params['world_model']['coll_objs'][obj]['radius'] + 0.1, **self.tensor_args)) 43 | self.position.append(torch.tensor(world_params['world_model']['coll_objs'][obj]['position'], **self.tensor_args)) 44 | def forward(self, position): 45 | inp_device = position.device 46 | position = position.to(self.device) 47 | i = 0 48 | cost = torch.norm(position - self.position[i],dim=-1) - self.radius[i] 49 | cost[cost > 0.0] = 0.0 50 | 51 | 52 | for i in range(1,len(self.position)): 53 | t_cost = torch.norm(position - self.position[i],dim=-1) - self.radius[i] 54 | t_cost[t_cost > 0.0] = 0.0 55 | cost += t_cost 56 | 57 | cost[cost < 0.0] = self.weight 58 | 59 | return cost.to(inp_device) 60 | 61 | 62 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/cost_base.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/dist_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | # import torch.nn.functional as F 26 | 27 | from .gaussian_projection import GaussianProjection 28 | 29 | class DistCost(nn.Module): 30 | def __init__(self, weight=None, vec_weight=None, gaussian_params={}, device=torch.device('cpu'), float_dtype=torch.float32, **kwargs): 31 | super(DistCost, self).__init__() 32 | self.device = device 33 | self.float_dtype = float_dtype 34 | self.weight = torch.as_tensor(weight, device=device, dtype=float_dtype) 35 | if(vec_weight is not None): 36 | self.vec_weight = torch.as_tensor(vec_weight, device=device, dtype=float_dtype) 37 | else: 38 | self.vec_weight = 1.0 39 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 40 | 41 | def forward(self, disp_vec, dist_type="l2", beta=1.0, RETURN_GOAL_DIST=False): 42 | inp_device = disp_vec.device 43 | disp_vec = self.vec_weight * disp_vec.to(self.device) 44 | 45 | if dist_type == 'l2': 46 | dist = torch.norm(disp_vec, p=2, dim=-1,keepdim=False) 47 | elif dist_type == 'squared_l2': 48 | 49 | dist = (torch.sum(torch.square(disp_vec), dim=-1,keepdim=False)) 50 | elif dist_type == 'l1': 51 | dist = torch.norm(disp_vec, p=1, dim=-1,keepdim=False) 52 | elif dist_type == 'smooth_l1': 53 | l1_dist = torch.norm(disp_vec, p=1, dim=-1) 54 | dist = None 55 | raise NotImplementedError 56 | 57 | cost = self.weight * self.proj_gaussian(dist) 58 | 59 | if(RETURN_GOAL_DIST): 60 | return cost.to(inp_device), dist.to(inp_device) 61 | return cost.to(inp_device) 62 | 63 | 64 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/ee_vel_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | 24 | 25 | import torch 26 | import torch.nn as nn 27 | from .gaussian_projection import GaussianProjection 28 | class EEVelCost(nn.Module): 29 | def __init__(self, ndofs, device, float_dtype, weight=1.0, vec_weight=[], gaussian_params={}): 30 | super(EEVelCost, self).__init__() 31 | self.ndofs = ndofs 32 | self.device = device 33 | self.float_dtype = float_dtype 34 | self.vel_idxs = torch.arange(self.ndofs,2*self.ndofs, dtype=torch.long, device=self.device) 35 | # self.I = torch.eye(6, device=device) 36 | self.I = torch.eye(ndofs, device=device, dtype=self.float_dtype) 37 | self.vec_weight = torch.as_tensor(vec_weight, device=device, dtype=float_dtype) 38 | self.weight = weight 39 | self.gaussian_projection = GaussianProjection(gaussian_params=gaussian_params) 40 | 41 | 42 | def forward(self, state_batch, jac_batch): 43 | 44 | inp_device = state_batch.device 45 | jac_batch = jac_batch.to(self.device) 46 | 47 | 48 | #use jacobian to get desired delta_q 49 | J = jac_batch 50 | qdot = state_batch[:,:,self.ndofs:2 * self.ndofs] 51 | 52 | xdot_current = torch.matmul(J, qdot.unsqueeze(-1)).squeeze(-1) 53 | 54 | error = torch.sum(torch.square(self.vec_weight * xdot_current), dim=-1) 55 | 56 | cost = self.weight * self.gaussian_projection(error) 57 | return cost.to(inp_device) 58 | 59 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/finite_difference_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import matplotlib 24 | matplotlib.use('tkagg') 25 | import matplotlib.pyplot as plt 26 | 27 | import torch 28 | import torch.nn as nn 29 | # import torch.nn.functional as F 30 | from .gaussian_projection import GaussianProjection 31 | from ..model.integration_utils import build_fd_matrix 32 | class FiniteDifferenceCost(nn.Module): 33 | def __init__(self, tensor_args={'device':torch.device('cpu'), 'dtype':torch.float32}, weight=1.0, order=1, gaussian_params={}, **kwargs): 34 | super(FiniteDifferenceCost, self).__init__() 35 | 36 | self.order = order 37 | for _ in range(order): 38 | weight *= weight 39 | self.weight = weight 40 | self.tensor_args = tensor_args 41 | # build FD matrix 42 | 43 | self.fd_mat = None 44 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 45 | self.t_mat = None 46 | def forward(self, ctrl_seq, dt): 47 | """ 48 | ctrl_seq: [B X H X d_act] 49 | """ 50 | dt[dt == 0.0] = 0.0 #dt[-1] 51 | dt = 1 / dt 52 | 53 | #dt = dt / torch.max(dt) 54 | dt = torch.abs(dt) 55 | 56 | #print(dt) 57 | dt[dt == float("Inf")] = 0 58 | 59 | dt[dt > 10] = 10 60 | #dt = dt / torch.max(dt) 61 | 62 | dt[dt != dt] = 0.0 63 | #for _ in range(self.order-1): 64 | # dt = dt * dt 65 | #print(dt) 66 | inp_device = ctrl_seq.device 67 | ctrl_seq = ctrl_seq.to(**self.tensor_args) 68 | 69 | B, H, _ = ctrl_seq.shape 70 | H = H - self.order 71 | dt = dt[:H] 72 | # 73 | if(self.fd_mat is None or self.fd_mat.shape[0] != H): 74 | self.fd_mat = build_fd_matrix(H,device=self.tensor_args['device'], dtype=self.tensor_args['dtype'], order=self.order, PREV_STATE=True) 75 | 76 | 77 | 78 | diff = torch.matmul(self.fd_mat,ctrl_seq) 79 | 80 | res = torch.abs(diff) 81 | 82 | cost = res[:,:,-1] 83 | 84 | cost[cost < 0.0001] = 0.0 85 | cost = self.weight * cost 86 | 87 | 88 | return cost 89 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/gaussian_projection.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | import math 26 | 27 | class GaussianProjection(nn.Module): 28 | """ 29 | Gaussian projection of weights following relaxedIK approach 30 | """ 31 | def __init__(self, gaussian_params={'n':0,'c':0,'s':0,'r':0}): 32 | super(GaussianProjection, self).__init__() 33 | 34 | #self.tensor_args = tensor_args 35 | # model parameters: omega 36 | self.omega = gaussian_params 37 | self._ws = gaussian_params['s'] 38 | self._wc = gaussian_params['c'] 39 | self._wn = gaussian_params['n'] 40 | self._wr = gaussian_params['r'] 41 | 42 | 43 | if(len(self.omega.keys()) > 0): 44 | self.n_pow = math.pow(-1.0, self.omega['n']) 45 | def forward(self, cost_value): 46 | if(self._wc == 0.0): 47 | return cost_value 48 | exp_term = torch.div(-1.0 * (cost_value - self._ws)**2, 2.0 * (self._wc**2)) 49 | #print(self.omega['s'], cost_value) 50 | #print(torch.pow(-1.0, self.omega['n'])) 51 | 52 | cost = 1.0 - self.n_pow * torch.exp(exp_term) + self._wr * torch.pow(cost_value - self._ws, 4) 53 | #cost = cost_value 54 | return cost 55 | 56 | 57 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/image_collision_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | import yaml 26 | # import torch.nn.functional as F 27 | from ...geom.geom_types import tensor_circle 28 | from .gaussian_projection import GaussianProjection 29 | from ...util_file import get_configs_path, get_gym_configs_path, join_path, load_yaml, get_assets_path 30 | from ...geom.sdf.world import WorldImageCollision 31 | 32 | 33 | class ImageCollisionCost(nn.Module): 34 | def __init__(self, weight=None, collision_file=None, bounds=[], dist_thresh=0.01, gaussian_params={}, tensor_args={'device':torch.device('cpu'), 'dtype':torch.float32}): 35 | super(ImageCollisionCost, self).__init__() 36 | 37 | self.tensor_args = tensor_args 38 | self.weight = torch.as_tensor(weight,**self.tensor_args) 39 | 40 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 41 | 42 | # BUILD world and robot: 43 | world_image = join_path(get_assets_path(), collision_file) 44 | 45 | 46 | self.world_coll = WorldImageCollision(bounds=bounds, tensor_args=tensor_args) 47 | self.world_coll.update_world(world_image) 48 | self.dist_thresh = dist_thresh # meters 49 | 50 | self.t_mat = None 51 | def forward(self, pos_seq): 52 | 53 | inp_device = pos_seq.device 54 | batch_size = pos_seq.shape[0] 55 | horizon = pos_seq.shape[1] 56 | pos_batch = pos_seq.view(batch_size * horizon, 2) 57 | 58 | # query sdf for points: 59 | dist = self.world_coll.get_pt_value(pos_batch) 60 | 61 | 62 | dist = dist.view(batch_size, horizon, 1) 63 | # cost only when dist is less 64 | 65 | # values are signed distance: positive inside object, negative outside 66 | dist += self.dist_thresh 67 | dist[dist < 0.0] = 0.0 68 | dist[dist > 0.0] = 1.0 69 | 70 | 71 | res = self.weight * dist 72 | 73 | if(self.t_mat is None or self.t_mat.shape[0] != res.shape[1]): 74 | self.t_mat = torch.ones((res.shape[1], res.shape[1]), **self.tensor_args).tril() 75 | 76 | t_mat = self.t_mat 77 | 78 | res = res.squeeze(-1) 79 | 80 | return res.to(inp_device) 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/jacobian_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | """ 24 | Distance cost projected into the null-space of the Jacobian 25 | """ 26 | 27 | import torch 28 | import torch.nn as nn 29 | 30 | from ...differentiable_robot_model.coordinate_transform import matrix_to_euler_angles 31 | 32 | class JacobianCost(nn.Module): 33 | def __init__(self, ndofs, device, float_dtype, retract_weight): 34 | self.ndofs = ndofs 35 | self.device = device 36 | self.float_dtype = float_dtype 37 | self.vel_idxs = torch.arange(self.ndofs,2*self.ndofs, dtype=torch.long, device=self.device) 38 | 39 | self.I = torch.eye(ndofs, device=device, dtype=self.float_dtype) 40 | 41 | self.retract_weight = torch.as_tensor(retract_weight, dtype=self.float_dtype, device=self.device) 42 | super(JacobianCost, self).__init__() 43 | 44 | def forward(self, state_batch, ee_pos_batch, ee_rot_batch, 45 | goal_ee_pos, goal_ee_rot, jac_batch, dt, 46 | proj_type="transpose", dist_type="l2", weight=1.0, beta=1.0, 47 | retract_state=None): 48 | 49 | inp_device = ee_pos_batch.device 50 | ee_pos_batch = ee_pos_batch.to(self.device) 51 | ee_rot_batch = ee_rot_batch.to(self.device) 52 | goal_ee_pos = goal_ee_pos.to(self.device) 53 | goal_ee_rot = goal_ee_rot.to(self.device) 54 | jac_batch = jac_batch.to(self.device) 55 | 56 | #calculate desired x_dot (position+orientation) 57 | ee_pos_disp = (ee_pos_batch - goal_ee_pos) 58 | 59 | # ee_euler_batch = matrix_to_euler_angles(ee_rot_batch, convention="XYZ") 60 | # goal_euler = matrix_to_euler_angles(goal_ee_rot, convention="XYZ") 61 | # ee_rot_disp = ee_euler_batch - goal_euler 62 | 63 | R_g_ee, _ = self.get_relative_transform(ee_pos_batch, ee_rot_batch, 64 | goal_ee_pos, goal_ee_rot) 65 | #print(R_g_ee.shape, state_batch.shape) 66 | ee_rot_disp = matrix_to_euler_angles(R_g_ee) * 0.0 67 | 68 | 69 | xdot_des = torch.cat((ee_pos_disp, ee_rot_disp), dim=-1) / dt 70 | 71 | # xdot_des = ee_pos_disp 72 | 73 | #use jacobian to get desired delta_q 74 | J_t = jac_batch.transpose(-2,-1) 75 | #print(xdot_des.unsqueeze(-1).shape, J_t.shape) 76 | qdot_des = torch.matmul(J_t, xdot_des.unsqueeze(-1)).squeeze(-1) 77 | #print(qdot_des.shape) 78 | # compute null space force and add: 79 | qdot = state_batch[:,:,self.ndofs:2*self.ndofs] 80 | # input('...') 81 | 82 | disp_vec = qdot - qdot_des# - qdot 83 | error = (0.5 * torch.sum(torch.square(disp_vec), dim=-1)) 84 | cost = weight * error 85 | 86 | return cost.to(inp_device) 87 | 88 | 89 | def get_relative_transform(self, ee_pos_batch, ee_rot_batch, 90 | goal_ee_pos, goal_ee_rot): 91 | 92 | #Inverse of goal transform 93 | R_g_t = goal_ee_rot.transpose(-2,-1) 94 | R_g_t_d = (-1.0* R_g_t @ goal_ee_pos.t()).transpose(-2,-1) 95 | 96 | #ee to goal transform 97 | #Rotation part 98 | R_g_ee = R_g_t @ ee_rot_batch 99 | #Translation part 100 | term1 = (R_g_t @ ee_pos_batch.transpose(-2,-1)).transpose(-2,-1) 101 | d_g_ee = term1 + R_g_t_d 102 | 103 | return R_g_ee, d_g_ee 104 | 105 | 106 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/manipulability_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | 24 | import torch 25 | import torch.nn as nn 26 | 27 | 28 | from .gaussian_projection import GaussianProjection 29 | 30 | eps = 0.01 31 | 32 | 33 | 34 | class ManipulabilityCost(nn.Module): 35 | def __init__(self, ndofs, weight=None, gaussian_params={}, device=torch.device('cpu'), float_dtype=torch.float32, thresh=0.1): 36 | super(ManipulabilityCost, self).__init__() 37 | self.device = device 38 | self.float_dtype = float_dtype 39 | self.weight = torch.as_tensor(weight, device=device, dtype=float_dtype) 40 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 41 | 42 | self.ndofs = ndofs 43 | self.thresh = thresh 44 | self.i_mat = torch.ones((6,1), device=self.device, dtype=self.float_dtype) 45 | def forward(self, jac_batch): 46 | inp_device = jac_batch.device 47 | 48 | 49 | 50 | with torch.cuda.amp.autocast(enabled=False): 51 | 52 | J_J_t = torch.matmul(jac_batch, jac_batch.transpose(-2,-1)) 53 | score = torch.sqrt(torch.det(J_J_t)) 54 | score[score != score] = 0.0 55 | 56 | 57 | score[score > self.thresh] = self.thresh #1.0 58 | score = (self.thresh - score) / self.thresh 59 | 60 | cost = self.weight * score 61 | 62 | return cost.to(inp_device) 63 | 64 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/null_costs.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | 25 | def get_inv_null_cost(J_full, goal_state, state_batch, device='cpu'): 26 | rho = 1e-2 27 | J_full_t = J_full.transpose(-2,-1) 28 | 29 | 30 | 31 | J_J_t = torch.matmul(J_full, J_full_t) 32 | J_J_t_inv = torch.inverse(J_J_t + (rho**2)*torch.eye(3, device=device).expand_as(J_J_t)) 33 | J_pinv = torch.matmul(J_full_t, J_J_t_inv) 34 | 35 | J_pinv_J = torch.matmul(J_pinv, J_full) 36 | 37 | null_proj = torch.eye(6, device=device).expand_as(J_pinv_J) - J_pinv_J 38 | 39 | null_disp = (state_batch[:,:, 0:6]-goal_state[:,0:6]) 40 | null_disp_cost = torch.norm(torch.matmul(null_proj, null_disp.unsqueeze(-1)), dim=-2).squeeze(-1) 41 | return null_disp_cost 42 | 43 | def get_transpose_null_cost(J_full, goal_state, state_batch, device='cpu'): 44 | rho = 1e-2 45 | J_full_t = J_full.transpose(-2,-1) 46 | J_t_J = torch.matmul(J_full_t, J_full) 47 | 48 | null_proj = torch.eye(6, device=device).expand_as(J_t_J) - J_t_J 49 | 50 | 51 | null_disp = (state_batch[:,:, 0:6]-goal_state[:,0:6]) 52 | null_disp_cost = torch.norm(torch.matmul(null_proj, null_disp.unsqueeze(-1)), dim=-2).squeeze(-1) 53 | return null_disp_cost 54 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/pose_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | # import torch.nn.functional as F 26 | 27 | from .gaussian_projection import GaussianProjection 28 | 29 | class PoseCost(nn.Module): 30 | """ Rotation cost 31 | 32 | .. math:: 33 | 34 | r &= \sum_{i=0}^{num_rows} (R^{i,:} - R_{g}^{i,:})^2 \\ 35 | cost &= \sum w \dot r 36 | 37 | 38 | """ 39 | def __init__(self, weight, vec_weight=[], position_gaussian_params={}, orientation_gaussian_params={}, tensor_args={'device':"cpu", 'dtype':torch.float32}, hinge_val=100.0, 40 | convergence_val=[0.0,0.0]): 41 | 42 | super(PoseCost, self).__init__() 43 | self.tensor_args = tensor_args 44 | self.I = torch.eye(3,3, **tensor_args) 45 | self.weight = weight 46 | self.vec_weight = torch.as_tensor(vec_weight, **tensor_args) 47 | self.rot_weight = self.vec_weight[0:3] 48 | self.pos_weight = self.vec_weight[3:6] 49 | 50 | self.px = torch.tensor([1.0,0.0,0.0], **self.tensor_args).T 51 | self.py = torch.tensor([0.0,1.0,0.0], **self.tensor_args).T 52 | self.pz = torch.tensor([0.0,0.0,1.0], **self.tensor_args).T 53 | 54 | self.I = torch.eye(3,3,**self.tensor_args) 55 | self.Z = torch.zeros(1, **self.tensor_args) 56 | 57 | 58 | self.position_gaussian = GaussianProjection(gaussian_params=position_gaussian_params) 59 | self.orientation_gaussian = GaussianProjection(gaussian_params=orientation_gaussian_params) 60 | self.hinge_val = hinge_val 61 | self.convergence_val = convergence_val 62 | self.dtype = self.tensor_args['dtype'] 63 | self.device = self.tensor_args['device'] 64 | 65 | 66 | def forward(self, ee_pos_batch, ee_rot_batch, ee_goal_pos, ee_goal_rot): 67 | 68 | 69 | inp_device = ee_pos_batch.device 70 | ee_pos_batch = ee_pos_batch.to(device=self.device, 71 | dtype=self.dtype) 72 | ee_rot_batch = ee_rot_batch.to(device=self.device, 73 | dtype=self.dtype) 74 | ee_goal_pos = ee_goal_pos.to(device=self.device, 75 | dtype=self.dtype) 76 | ee_goal_rot = ee_goal_rot.to(device=self.device, 77 | dtype=self.dtype) 78 | 79 | #Inverse of goal transform 80 | R_g_t = ee_goal_rot.transpose(-2,-1) # w_R_g -> g_R_w 81 | R_g_t_d = (-1.0 * R_g_t @ ee_goal_pos.t()).transpose(-2,-1) # -g_R_w * w_d_g -> g_d_g 82 | 83 | 84 | #Rotation part 85 | R_g_ee = R_g_t @ ee_rot_batch # g_R_w * w_R_ee -> g_R_ee 86 | 87 | 88 | #Translation part 89 | # transpose is done for matmul 90 | term1 = (R_g_t @ ee_pos_batch.transpose(-2,-1)).transpose(-2,-1) # g_R_w * w_d_ee -> g_d_ee 91 | d_g_ee = term1 + R_g_t_d # g_d_g + g_d_ee 92 | goal_dist = torch.norm(self.pos_weight * d_g_ee, p=2, dim=-1, keepdim=True) 93 | 94 | position_err = (torch.sum(torch.square(self.pos_weight * d_g_ee),dim=-1)) 95 | #compute projection error 96 | rot_err = self.I - R_g_ee 97 | rot_err = torch.norm(rot_err, dim=-1) 98 | rot_err_norm = torch.norm(torch.sum(self.rot_weight * rot_err,dim=-1), p=2, dim=-1, keepdim=True) 99 | 100 | rot_err = torch.square(torch.sum(self.rot_weight * rot_err, dim=-1)) 101 | 102 | 103 | if(self.hinge_val > 0.0): 104 | rot_err = torch.where(goal_dist.squeeze(-1) <= self.hinge_val, rot_err, self.Z) #hard hinge 105 | 106 | rot_err[rot_err < self.convergence_val[0]] = 0.0 107 | position_err[position_err < self.convergence_val[1]] = 0.0 108 | cost = self.weight[0] * self.orientation_gaussian(torch.sqrt(rot_err)) + self.weight[1] * self.position_gaussian(torch.sqrt(position_err)) 109 | 110 | # dimension should be bacth * traj_length 111 | return cost.to(inp_device), rot_err_norm, goal_dist 112 | 113 | 114 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/primitive_collision_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | # import torch.nn.functional as F 26 | from ...geom.sdf.robot_world import RobotWorldCollisionPrimitive 27 | from .gaussian_projection import GaussianProjection 28 | 29 | class PrimitiveCollisionCost(nn.Module): 30 | def __init__(self, weight=None, world_params=None, robot_params=None, gaussian_params={}, 31 | distance_threshold=0.1, tensor_args={'device':torch.device('cpu'), 'dtype':torch.float32}): 32 | super(PrimitiveCollisionCost, self).__init__() 33 | 34 | self.tensor_args = tensor_args 35 | self.weight = torch.as_tensor(weight,**self.tensor_args) 36 | 37 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 38 | 39 | robot_collision_params = robot_params['robot_collision_params'] 40 | self.batch_size = -1 41 | # BUILD world and robot: 42 | self.robot_world_coll = RobotWorldCollisionPrimitive(robot_collision_params, 43 | world_params['world_model'], 44 | tensor_args=self.tensor_args, 45 | bounds=robot_params['world_collision_params']['bounds'], 46 | grid_resolution=robot_params['world_collision_params']['grid_resolution']) 47 | 48 | self.n_world_objs = self.robot_world_coll.world_coll.n_objs 49 | self.t_mat = None 50 | self.distance_threshold = distance_threshold 51 | def forward(self, link_pos_seq, link_rot_seq): 52 | 53 | 54 | inp_device = link_pos_seq.device 55 | batch_size = link_pos_seq.shape[0] 56 | horizon = link_pos_seq.shape[1] 57 | n_links = link_pos_seq.shape[2] 58 | 59 | if(self.batch_size != batch_size): 60 | self.batch_size = batch_size 61 | self.robot_world_coll.build_batch_features(self.batch_size * horizon, clone_pose=True, clone_points=True) 62 | 63 | link_pos_batch = link_pos_seq.view(batch_size * horizon, n_links, 3) 64 | link_rot_batch = link_rot_seq.view(batch_size * horizon, n_links, 3, 3) 65 | dist = self.robot_world_coll.check_robot_sphere_collisions(link_pos_batch, 66 | link_rot_batch) 67 | dist = dist.view(batch_size, horizon, n_links)#, self.n_world_objs) 68 | # cost only when dist is less 69 | dist += self.distance_threshold 70 | 71 | dist[dist <= 0.0] = 0.0 72 | dist[dist > 0.2] = 0.2 73 | dist = dist / 0.25 74 | 75 | cost = torch.sum(dist, dim=-1) 76 | 77 | 78 | cost = self.weight * cost 79 | 80 | return cost.to(inp_device) 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/projected_dist_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | """ 24 | Distance cost projected into the null-space of the Jacobian 25 | """ 26 | 27 | import torch 28 | import torch.nn as nn 29 | 30 | from .dist_cost import DistCost 31 | 32 | 33 | eps = 0.01 34 | 35 | 36 | class ProjectedDistCost(DistCost): 37 | def __init__(self, ndofs, weight=None, vec_weight=None, gaussian_params={}, device=torch.device('cpu'), float_dtype=torch.float32): 38 | super(ProjectedDistCost, self).__init__(weight, gaussian_params=gaussian_params, device=device, float_dtype=float_dtype) 39 | self.ndofs = ndofs 40 | self.float_dtype = float_dtype 41 | self.I = torch.eye(ndofs, device=device, dtype=self.float_dtype) 42 | self.task_I = torch.eye(6, device=device, dtype=self.float_dtype) 43 | self.vec_weight = torch.as_tensor(vec_weight, device=device, dtype=float_dtype) 44 | def forward(self, disp_vec, jac_batch, proj_type="transpose", dist_type="squared_l2", beta=1.0): 45 | inp_device = disp_vec.device 46 | disp_vec = self.vec_weight * disp_vec.to(self.device) 47 | 48 | if proj_type == "transpose": 49 | disp_vec_projected = self.get_transpose_null_disp(disp_vec, jac_batch) 50 | elif proj_type == "pseudo_inverse": 51 | disp_vec_projected = self.get_pinv_null_disp(disp_vec, jac_batch) 52 | elif proj_type == "identity": 53 | disp_vec_projected = disp_vec 54 | 55 | 56 | 57 | 58 | return super().forward(disp_vec_projected, dist_type, beta) 59 | 60 | 61 | def get_transpose_null_disp(self, disp_vec, jac_batch): 62 | J_t_J = torch.matmul(jac_batch.transpose(-2,-1), jac_batch) 63 | J_J_t = torch.matmul(jac_batch, jac_batch.transpose(-2,-1)) 64 | score = 1.0 / (torch.sqrt(torch.det(J_J_t)) + 0.0001) 65 | return score 66 | 67 | 68 | 69 | def get_pinv_null_disp(self, disp_vec, jac_batch): 70 | 71 | jac_batch_t = jac_batch.transpose(-2,-1) 72 | 73 | 74 | J_J_t = torch.matmul(jac_batch, jac_batch_t) 75 | 76 | J_pinv = jac_batch_t @ torch.inverse(J_J_t + eps * self.task_I.expand_as(J_J_t)) 77 | 78 | J_pinv_J = torch.matmul(J_pinv, jac_batch) 79 | 80 | 81 | null_proj = self.I.expand_as(J_pinv_J) - J_pinv_J 82 | 83 | 84 | null_disp = torch.matmul(null_proj, disp_vec.unsqueeze(-1)).squeeze(-1) 85 | return null_disp 86 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/robot_self_collision_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | # import torch.nn.functional as F 26 | 27 | from ...differentiable_robot_model.coordinate_transform import CoordinateTransform, quaternion_to_matrix 28 | 29 | from ...util_file import get_assets_path, join_path 30 | from ...geom.sdf.robot import RobotSphereCollision 31 | from .gaussian_projection import GaussianProjection 32 | 33 | class RobotSelfCollisionCost(nn.Module): 34 | def __init__(self, weight=None, robot_params=None, 35 | gaussian_params={}, distance_threshold=-0.01, 36 | batch_size=2, tensor_args={'device':torch.device('cpu'), 'dtype':torch.float32}): 37 | super(RobotSelfCollisionCost, self).__init__() 38 | self.tensor_args = tensor_args 39 | self.device = tensor_args['device'] 40 | self.float_dtype = tensor_args['dtype'] 41 | self.distance_threshold = distance_threshold 42 | self.weight = torch.as_tensor(weight, **self.tensor_args) 43 | 44 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 45 | 46 | 47 | # load robot model: 48 | robot_collision_params = robot_params['robot_collision_params'] 49 | robot_collision_params['urdf'] = join_path(get_assets_path(), 50 | robot_collision_params['urdf']) 51 | 52 | 53 | # load nn params: 54 | label_map = robot_params['world_collision_params']['label_map'] 55 | bounds = robot_params['world_collision_params']['bounds'] 56 | self.distance_threshold = distance_threshold 57 | self.batch_size = batch_size 58 | 59 | # initialize NN model: 60 | self.coll = RobotSphereCollision(robot_collision_params, self.batch_size, 61 | tensor_args=self.tensor_args) 62 | 63 | 64 | self.coll.build_batch_features(batch_size=self.batch_size, clone_pose=True, clone_objs=True) 65 | 66 | self.res = None 67 | self.t_mat = None 68 | 69 | def distance(self, link_pos_seq, link_rot_seq): 70 | batch_size = link_pos_seq.shape[0] 71 | horizon = link_pos_seq.shape[1] 72 | n_links = link_pos_seq.shape[2] 73 | link_pos = link_pos_seq.view(batch_size * horizon, n_links, 3) 74 | link_rot = link_rot_seq.view(batch_size * horizon, n_links, 3, 3) 75 | 76 | if(self.batch_size != batch_size): 77 | self.batch_size = batch_size 78 | self.coll.build_batch_features(batch_size=self.batch_size * horizon, clone_pose=True, clone_objs=True) 79 | 80 | res = self.coll.check_self_collisions(link_pos, link_rot) 81 | 82 | self.res = res 83 | res = res.view(batch_size, horizon, n_links) 84 | res = torch.max(res, dim=-1)[0] 85 | return res 86 | 87 | def forward(self, q): 88 | batch_size = q.shape[0] 89 | horizon = q.shape[1] 90 | q = q.view(batch_size * horizon, q.shape[2]) 91 | 92 | res = self.coll.check_self_collisions_nn(q) 93 | 94 | res = res.view(batch_size, horizon) 95 | res += self.distance_threshold 96 | res[res <= 0.0] = 0.0 97 | 98 | res[res >= 0.5] = 0.5 99 | 100 | # rescale: 101 | res = res / 0.25 102 | 103 | 104 | 105 | cost = res 106 | 107 | cost = self.weight * self.proj_gaussian(cost) 108 | 109 | return cost 110 | 111 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/stop_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | # import torch.nn.functional as F 26 | from .gaussian_projection import GaussianProjection 27 | 28 | class StopCost(nn.Module): 29 | def __init__(self, tensor_args={'device':torch.device('cpu'), 'dtype':torch.float64}, 30 | max_limit=None, max_nlimit=None, weight=1.0, gaussian_params={}, 31 | traj_dt=None,**kwargs): 32 | super(StopCost, self).__init__() 33 | self.tensor_args = tensor_args 34 | self.weight = torch.as_tensor(weight, **tensor_args) 35 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 36 | self.traj_dt = traj_dt 37 | 38 | # compute max velocity across horizon: 39 | self.horizon = self.traj_dt.shape[0] 40 | sum_matrix = torch.tril(torch.ones((self.horizon, self.horizon), **self.tensor_args)).T 41 | 42 | if(max_nlimit is not None): 43 | # every timestep max acceleration: 44 | sum_matrix = torch.tril(torch.ones((self.horizon, self.horizon), **self.tensor_args)).T 45 | delta_vel = self.traj_dt * max_nlimit 46 | self.max_vel = ((sum_matrix @ delta_vel).unsqueeze(-1)) 47 | elif(max_limit is not None): 48 | sum_matrix = torch.tril(torch.ones((self.horizon, self.horizon), **self.tensor_args)).T 49 | delta_vel = torch.ones_like(self.traj_dt) * max_limit 50 | self.max_vel = ((sum_matrix @ delta_vel).unsqueeze(-1)) 51 | 52 | def forward(self, vels): 53 | inp_device = vels.device 54 | 55 | 56 | vel_abs = torch.abs(vels.to(**self.tensor_args)) 57 | 58 | 59 | # max velocity threshold: 60 | 61 | 62 | vel_abs = vel_abs - self.max_vel 63 | vel_abs[vel_abs < 0.0] = 0.0 64 | 65 | cost = self.weight * self.proj_gaussian(((torch.sum(torch.square(vel_abs), dim=-1)))) 66 | 67 | 68 | return cost.to(inp_device) 69 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/voxel_collision_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | # import torch.nn.functional as F 26 | 27 | from ...differentiable_robot_model.coordinate_transform import CoordinateTransform, quaternion_to_matrix 28 | 29 | from ...util_file import get_assets_path, join_path 30 | from ...geom.sdf.robot_world import RobotWorldCollisionVoxel 31 | from .gaussian_projection import GaussianProjection 32 | 33 | class VoxelCollisionCost(nn.Module): 34 | def __init__(self, weight=None, robot_params=None, 35 | gaussian_params={}, grid_resolution=0.05, distance_threshold=-0.01, 36 | batch_size=2, tensor_args={'device':torch.device('cpu'), 'dtype':torch.float32}): 37 | super(VoxelCollisionCost, self).__init__() 38 | self.tensor_args = tensor_args 39 | self.device = tensor_args['device'] 40 | self.float_dtype = tensor_args['dtype'] 41 | self.distance_threshold = distance_threshold 42 | self.weight = torch.as_tensor(weight, **self.tensor_args) 43 | 44 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 45 | 46 | 47 | # load robot model: 48 | robot_collision_params = robot_params['collision_params'] 49 | robot_collision_params['urdf'] = join_path(get_assets_path(), 50 | robot_collision_params['urdf']) 51 | 52 | 53 | # load nn params: 54 | label_map = robot_params['world_collision_params']['label_map'] 55 | bounds = robot_params['world_collision_params']['bounds'] 56 | #model_path = robot_params['world_collision_params']['model_path'] 57 | self.threshold = robot_params['collision_params']['threshold'] 58 | self.batch_size = batch_size 59 | 60 | # initialize NN model: 61 | self.coll = RobotWorldCollisionVoxel(robot_collision_params, self.batch_size, 62 | label_map, bounds, grid_resolution=grid_resolution, 63 | tensor_args=self.tensor_args) 64 | 65 | #self.coll.set_robot_objects() 66 | self.coll.build_batch_features(self.batch_size, clone_pose=True, clone_points=True) 67 | 68 | self.COLL_INIT = False 69 | self.SCENE_INIT = False 70 | self.camera_data = None 71 | self.res = None 72 | self.t_mat = None 73 | def first_run(self, camera_data): 74 | 75 | # set world transforms: 76 | quat = camera_data['robot_camera_pose'][3:] 77 | rot = quaternion_to_matrix(torch.as_tensor([quat[3],quat[0], quat[1], quat[2]]).unsqueeze(0)) 78 | 79 | robot_camera_trans = torch.tensor(camera_data['robot_camera_pose'][0:3]).unsqueeze(0) 80 | robot_camera_rot = torch.tensor(rot) 81 | 82 | robot_table_trans = torch.tensor([0.0,-0.35,-0.24]).unsqueeze(0) 83 | robot_table_rot = torch.eye(3).unsqueeze(0) 84 | self.coll.set_world_transform(robot_table_trans, robot_table_rot, 85 | robot_camera_trans, robot_camera_rot) 86 | 87 | self.coll.set_scene(camera_data['pc'], camera_data['pc_seg']) 88 | 89 | self.COLL_INIT = True 90 | 91 | def set_scene(self, camera_data): 92 | #if(not self.COLL_INIT): 93 | self.first_run(camera_data) 94 | self.camera_data = camera_data 95 | self.coll.set_scene(camera_data['pc'], camera_data['pc_seg']) 96 | self.SCENE_INIT = True 97 | 98 | 99 | def forward(self, link_pos_seq, link_rot_seq): 100 | batch_size = link_pos_seq.shape[0] 101 | horizon = link_pos_seq.shape[1] 102 | n_links = link_pos_seq.shape[2] 103 | link_pos = link_pos_seq.view(batch_size * horizon, n_links, 3) 104 | link_rot = link_rot_seq.view(batch_size * horizon, n_links, 3, 3) 105 | #print(link_pos.shape, link_rot.shape) 106 | if(self.batch_size != batch_size): 107 | self.batch_size = batch_size 108 | self.coll.build_batch_features(self.batch_size*horizon, clone_pose=True, clone_points=True) 109 | 110 | res = self.coll.check_robot_sphere_collisions(link_pos, link_rot) 111 | self.res = res 112 | res = res.view(batch_size, horizon, n_links) 113 | # res = [batch,link] 114 | 115 | # negative res is outside mesh (not colliding) 116 | res += self.distance_threshold 117 | res[res <= 0.0] = 0.0 118 | 119 | res[res >= 0.5] = 0.5 120 | 121 | # rescale: 122 | res = res / 0.25 123 | 124 | # all values are positive now 125 | res = torch.sum(res, dim=-1) 126 | 127 | 128 | cost = res 129 | 130 | cost = self.weight * self.proj_gaussian(cost) 131 | 132 | return cost 133 | -------------------------------------------------------------------------------- /storm_kit/mpc/cost/zero_cost.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.nn as nn 25 | 26 | from .gaussian_projection import GaussianProjection 27 | 28 | class ZeroCost(nn.Module): 29 | def __init__(self, device=torch.device('cpu'), float_dtype=torch.float64, 30 | hinge_val=100.0, weight=1.0, gaussian_params={}, max_vel=0.01): 31 | super(ZeroCost, self).__init__() 32 | self.device = device 33 | self.float_dtype = float_dtype 34 | self.Z = torch.zeros(1, device=self.device, dtype=self.float_dtype) 35 | self.weight = torch.as_tensor(weight, device=device, dtype=float_dtype) 36 | self.proj_gaussian = GaussianProjection(gaussian_params=gaussian_params) 37 | self.hinge_val = hinge_val 38 | self.max_vel = max_vel 39 | def forward(self, vels, goal_dist): 40 | inp_device = vels.device 41 | vel_err = torch.abs(vels.to(self.device)) 42 | goal_dist = goal_dist.to(self.device) 43 | 44 | 45 | # max velocity threshold: 46 | vel_err[vel_err < self.max_vel] = 0.0 47 | 48 | if(self.hinge_val > 0.0): 49 | vel_err = torch.where(goal_dist <= self.hinge_val, vel_err, 0.0 * vel_err / goal_dist) #soft hinge 50 | 51 | cost = self.weight * self.proj_gaussian((torch.sum(torch.square(vel_err), dim=-1))) 52 | 53 | 54 | return cost.to(inp_device) 55 | -------------------------------------------------------------------------------- /storm_kit/mpc/model/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | from .model_base import DynamicsModelBase 24 | from .urdf_kinematic_model import URDFKinematicModel 25 | from .urdf_kinematic_model_baseline import URDFKinematicModelBaseline 26 | from .simple_model import HolonomicModel 27 | 28 | #__all__ = ['DynamicsModelBase', 'URDFKinematicModel', 'HolonomicModel'] 29 | -------------------------------------------------------------------------------- /storm_kit/mpc/model/model_base.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | from abc import ABC, abstractmethod 24 | 25 | class DynamicsModelBase(ABC): 26 | def __init__(self): 27 | pass 28 | 29 | #@abstractmethod 30 | #def step(self, state, act): 31 | # pass 32 | 33 | @abstractmethod 34 | def rollout_open_loop(self, start_state, act_seq): 35 | pass 36 | 37 | #Rendering 38 | #def render(self): 39 | # pass 40 | 41 | #def render_trajs(self, trajectories): 42 | # pass 43 | 44 | @abstractmethod 45 | def get_next_state(self, curr_state, act, dt): 46 | pass 47 | -------------------------------------------------------------------------------- /storm_kit/mpc/rollout/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | """ 24 | This folder contains rollout functions for common robot tasks. Each rollout function consists of a forward dynamics function and a cost function. 25 | """ 26 | -------------------------------------------------------------------------------- /storm_kit/mpc/rollout/arm_reacher.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import torch.autograd.profiler as profiler 25 | 26 | from ...differentiable_robot_model.coordinate_transform import matrix_to_quaternion, quaternion_to_matrix 27 | from ..cost import DistCost, PoseCost, ZeroCost, FiniteDifferenceCost 28 | from ...mpc.rollout.arm_base import ArmBase 29 | 30 | class ArmReacher(ArmBase): 31 | """ 32 | This rollout function is for reaching a cartesian pose for a robot 33 | 34 | Todo: 35 | 1. Update exp_params to be kwargs 36 | """ 37 | 38 | def __init__(self, exp_params, tensor_args={'device':"cpu", 'dtype':torch.float32}, world_params=None): 39 | super(ArmReacher, self).__init__(exp_params=exp_params, 40 | tensor_args=tensor_args, 41 | world_params=world_params) 42 | self.goal_state = None 43 | self.goal_ee_pos = None 44 | self.goal_ee_rot = None 45 | 46 | device = self.tensor_args['device'] 47 | float_dtype = self.tensor_args['dtype'] 48 | self.dist_cost = DistCost(**self.exp_params['cost']['joint_l2'], device=device,float_dtype=float_dtype) 49 | 50 | self.goal_cost = PoseCost(**exp_params['cost']['goal_pose'], 51 | tensor_args=self.tensor_args) 52 | 53 | 54 | def cost_fn(self, state_dict, action_batch, no_coll=False, horizon_cost=True, return_dist=False): 55 | 56 | cost = super(ArmReacher, self).cost_fn(state_dict, action_batch, no_coll, horizon_cost) 57 | ee_pos_batch, ee_rot_batch = state_dict['ee_pos_seq'], state_dict['ee_rot_seq'] 58 | 59 | state_batch = state_dict['state_seq'] 60 | goal_ee_pos = self.goal_ee_pos 61 | goal_ee_rot = self.goal_ee_rot 62 | retract_state = self.retract_state 63 | goal_state = self.goal_state 64 | 65 | 66 | goal_cost, rot_err_norm, goal_dist = self.goal_cost.forward(ee_pos_batch, ee_rot_batch, 67 | goal_ee_pos, goal_ee_rot) 68 | 69 | 70 | cost += goal_cost 71 | 72 | # joint l2 cost 73 | if(self.exp_params['cost']['joint_l2']['weight'] > 0.0 and goal_state is not None): 74 | disp_vec = state_batch[:,:,0:self.n_dofs] - goal_state[:,0:self.n_dofs] 75 | cost += self.dist_cost.forward(disp_vec) 76 | 77 | if(return_dist): 78 | return cost, rot_err_norm, goal_dist 79 | 80 | 81 | if self.exp_params['cost']['zero_acc']['weight'] > 0: 82 | cost += self.zero_acc_cost.forward(state_batch[:, :, self.n_dofs*2:self.n_dofs*3], goal_dist=goal_dist) 83 | 84 | if self.exp_params['cost']['zero_vel']['weight'] > 0: 85 | cost += self.zero_vel_cost.forward(state_batch[:, :, self.n_dofs:self.n_dofs*2], goal_dist=goal_dist) 86 | 87 | return cost 88 | 89 | 90 | def update_params(self, retract_state=None, goal_state=None, goal_ee_pos=None, goal_ee_rot=None, goal_ee_quat=None): 91 | """ 92 | Update params for the cost terms and dynamics model. 93 | goal_state: n_dofs 94 | goal_ee_pos: 3 95 | goal_ee_rot: 3,3 96 | goal_ee_quat: 4 97 | 98 | """ 99 | 100 | super(ArmReacher, self).update_params(retract_state=retract_state) 101 | 102 | if(goal_ee_pos is not None): 103 | self.goal_ee_pos = torch.as_tensor(goal_ee_pos, **self.tensor_args).unsqueeze(0) 104 | self.goal_state = None 105 | if(goal_ee_rot is not None): 106 | self.goal_ee_rot = torch.as_tensor(goal_ee_rot, **self.tensor_args).unsqueeze(0) 107 | self.goal_ee_quat = matrix_to_quaternion(self.goal_ee_rot) 108 | self.goal_state = None 109 | if(goal_ee_quat is not None): 110 | self.goal_ee_quat = torch.as_tensor(goal_ee_quat, **self.tensor_args).unsqueeze(0) 111 | self.goal_ee_rot = quaternion_to_matrix(self.goal_ee_quat) 112 | self.goal_state = None 113 | if(goal_state is not None): 114 | self.goal_state = torch.as_tensor(goal_state, **self.tensor_args).unsqueeze(0) 115 | self.goal_ee_pos, self.goal_ee_rot = self.dynamics_model.robot_model.compute_forward_kinematics(self.goal_state[:,0:self.n_dofs], self.goal_state[:,self.n_dofs:2*self.n_dofs], link_name=self.exp_params['model']['ee_link_name']) 116 | self.goal_ee_quat = matrix_to_quaternion(self.goal_ee_rot) 117 | 118 | return True 119 | 120 | -------------------------------------------------------------------------------- /storm_kit/mpc/rollout/rollout_base.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | 24 | #import torch 25 | 26 | 27 | class RolloutBase: 28 | def __init__(self): 29 | pass 30 | def cost_fn(self, state, act): 31 | pass 32 | def rollout_fn(self, state, act): 33 | pass 34 | def current_cost(self, current_state): 35 | pass 36 | def update_params(self): 37 | pass 38 | 39 | 40 | -------------------------------------------------------------------------------- /storm_kit/mpc/task/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | from .task_base import BaseTask 24 | from .reacher_task import ReacherTask 25 | 26 | #__all__=['BaseTask', 'ReacherTask'] 27 | -------------------------------------------------------------------------------- /storm_kit/mpc/task/arm_task.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import yaml 25 | import numpy as np 26 | 27 | from ...util_file import get_mpc_configs_path as mpc_configs_path 28 | from ...mpc.rollout.arm_reacher import ArmBase 29 | from ...mpc.control import MPPI 30 | from ...mpc.utils.state_filter import JointStateFilter 31 | from ...mpc.utils.mpc_process_wrapper import ControlProcess 32 | from ...util_file import get_assets_path, join_path, load_yaml, get_gym_configs_path 33 | from .task_base import BaseTask 34 | 35 | 36 | class ArmTask(BaseTask): 37 | def __init__(self, task_file='ur10.yml', robot_file='ur10_reacher.yml', world_file='collision_env.yml', tensor_args={'device':"cpu", 'dtype':torch.float32}): 38 | 39 | super().__init__(tensor_args=tensor_args) 40 | 41 | 42 | self.controller = self.init_mppi(task_file, robot_file, world_file) 43 | self.init_aux() 44 | 45 | def get_rollout_fn(self, **kwargs): 46 | rollout_fn = ArmBase(**kwargs) 47 | return rollout_fn 48 | 49 | def init_mppi(self, task_file, robot_file, collision_file): 50 | robot_yml = join_path(get_gym_configs_path(), robot_file) 51 | 52 | with open(robot_yml) as file: 53 | robot_params = yaml.load(file, Loader=yaml.FullLoader) 54 | 55 | world_yml = join_path(get_gym_configs_path(), collision_file) 56 | with open(world_yml) as file: 57 | world_params = yaml.load(file, Loader=yaml.FullLoader) 58 | 59 | mpc_yml_file = join_path(mpc_configs_path(), task_file) 60 | 61 | with open(mpc_yml_file) as file: 62 | exp_params = yaml.load(file, Loader=yaml.FullLoader) 63 | exp_params['robot_params'] = exp_params['model'] #robot_params 64 | 65 | 66 | rollout_fn = self.get_rollout_fn(exp_params=exp_params, tensor_args=self.tensor_args, world_params=world_params) 67 | 68 | mppi_params = exp_params['mppi'] 69 | dynamics_model = rollout_fn.dynamics_model 70 | mppi_params['d_action'] = dynamics_model.d_action 71 | mppi_params['action_lows'] = -exp_params['model']['max_acc'] * torch.ones(dynamics_model.d_action, **self.tensor_args) 72 | mppi_params['action_highs'] = exp_params['model']['max_acc'] * torch.ones(dynamics_model.d_action, **self.tensor_args) 73 | init_q = torch.tensor(exp_params['model']['init_state'], **self.tensor_args) 74 | init_action = torch.zeros((mppi_params['horizon'], dynamics_model.d_action), **self.tensor_args) 75 | init_action[:,:] += init_q 76 | if(exp_params['control_space'] == 'acc'): 77 | mppi_params['init_mean'] = init_action * 0.0 # device=device) 78 | elif(exp_params['control_space'] == 'pos'): 79 | mppi_params['init_mean'] = init_action 80 | mppi_params['rollout_fn'] = rollout_fn 81 | mppi_params['tensor_args'] = self.tensor_args 82 | controller = MPPI(**mppi_params) 83 | self.exp_params = exp_params 84 | return controller 85 | 86 | -------------------------------------------------------------------------------- /storm_kit/mpc/task/reacher_task.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import yaml 25 | import numpy as np 26 | 27 | from ...util_file import get_mpc_configs_path as mpc_configs_path 28 | from ...mpc.rollout.arm_reacher import ArmReacher 29 | from ...mpc.control import MPPI 30 | from ...mpc.utils.state_filter import JointStateFilter 31 | from ...mpc.utils.mpc_process_wrapper import ControlProcess 32 | from ...util_file import get_assets_path, join_path, load_yaml, get_gym_configs_path 33 | from .arm_task import ArmTask 34 | 35 | 36 | class ReacherTask(ArmTask): 37 | """ 38 | .. inheritance-diagram:: ReacherTask 39 | :parts: 1 40 | 41 | """ 42 | def __init__(self, task_file='ur10.yml', robot_file='ur10_reacher.yml', world_file='collision_env.yml', tensor_args={'device':"cpu", 'dtype':torch.float32}): 43 | 44 | super().__init__(task_file=task_file, robot_file=robot_file, 45 | world_file=world_file, tensor_args=tensor_args) 46 | 47 | def get_rollout_fn(self, **kwargs): 48 | rollout_fn = ArmReacher(**kwargs) 49 | return rollout_fn 50 | 51 | -------------------------------------------------------------------------------- /storm_kit/mpc/task/simple_task.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | import yaml 25 | import numpy as np 26 | 27 | from ...util_file import get_mpc_configs_path as mpc_configs_path 28 | from ...mpc.rollout.simple_reacher import SimpleReacher 29 | from ...mpc.control import MPPI 30 | from ...mpc.utils.state_filter import JointStateFilter 31 | from ...mpc.utils.mpc_process_wrapper import ControlProcess 32 | from ...util_file import get_assets_path, join_path, load_yaml, get_gym_configs_path 33 | from .task_base import BaseTask 34 | 35 | 36 | class SimpleTask(BaseTask): 37 | """ 38 | .. inheritance-diagram:: SimpleTask 39 | :parts: 1 40 | 41 | """ 42 | def __init__(self, robot_file='simple_reacher.yml', tensor_args={'device':"cpu", 'dtype':torch.float32}): 43 | 44 | super().__init__(tensor_args=tensor_args) 45 | 46 | self.controller = self.init_mppi(robot_file) 47 | 48 | self.init_aux() 49 | 50 | def get_rollout_fn(self, **kwargs): 51 | rollout_fn = SimpleReacher(**kwargs) 52 | return rollout_fn 53 | 54 | def init_mppi(self, robot_file): 55 | mpc_yml_file = join_path(mpc_configs_path(), robot_file) 56 | 57 | with open(mpc_yml_file) as file: 58 | exp_params = yaml.load(file, Loader=yaml.FullLoader) 59 | 60 | rollout_fn = self.get_rollout_fn(exp_params=exp_params, tensor_args=self.tensor_args) 61 | 62 | 63 | 64 | mppi_params = exp_params['mppi'] 65 | dynamics_model = rollout_fn.dynamics_model 66 | mppi_params['d_action'] = dynamics_model.d_action 67 | mppi_params['action_lows'] = -exp_params['model']['max_action'] * torch.ones(dynamics_model.d_action, **self.tensor_args) 68 | mppi_params['action_highs'] = exp_params['model']['max_action'] * torch.ones(dynamics_model.d_action, **self.tensor_args) 69 | init_action = torch.zeros((mppi_params['horizon'], dynamics_model.d_action), **self.tensor_args) 70 | mppi_params['init_mean'] = init_action 71 | mppi_params['rollout_fn'] = rollout_fn 72 | mppi_params['tensor_args'] = self.tensor_args 73 | controller = MPPI(**mppi_params) 74 | self.exp_params = exp_params 75 | return controller 76 | -------------------------------------------------------------------------------- /storm_kit/mpc/task/task_base.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | 24 | import torch 25 | import numpy as np 26 | 27 | from ...mpc.utils.state_filter import JointStateFilter 28 | from ...mpc.utils.mpc_process_wrapper import ControlProcess 29 | 30 | class BaseTask(): 31 | def __init__(self, tensor_args={'device':"cpu", 'dtype':torch.float32}): 32 | self.tensor_args = tensor_args 33 | self.prev_qdd_des = None 34 | def init_aux(self): 35 | self.state_filter = JointStateFilter(filter_coeff=self.exp_params['state_filter_coeff'], dt=self.exp_params['control_dt']) 36 | 37 | self.command_filter = JointStateFilter(filter_coeff=self.exp_params['cmd_filter_coeff'], dt=self.exp_params['control_dt']) 38 | self.control_process = ControlProcess(self.controller) 39 | self.n_dofs = self.controller.rollout_fn.dynamics_model.n_dofs 40 | self.zero_acc = np.zeros(self.n_dofs) 41 | 42 | def get_rollout_fn(self, **kwargs): 43 | raise NotImplementedError 44 | 45 | def init_mppi(self, **kwargs): 46 | raise NotImplementedError 47 | 48 | def update_params(self, **kwargs): 49 | self.controller.rollout_fn.update_params(**kwargs) 50 | self.control_process.update_params(**kwargs) 51 | return True 52 | 53 | 54 | def get_command(self, t_step, curr_state, control_dt, WAIT=False): 55 | 56 | # predict forward from previous action and previous state: 57 | #self.state_filter.predict_internal_state(self.prev_qdd_des) 58 | 59 | if(self.state_filter.cmd_joint_state is None): 60 | curr_state['velocity'] *= 0.0 61 | filt_state = self.state_filter.filter_joint_state(curr_state) 62 | state_tensor = self._state_to_tensor(filt_state) 63 | 64 | if(WAIT): 65 | next_command, val, info, best_action = self.control_process.get_command_debug(t_step, state_tensor.numpy(), control_dt=control_dt) 66 | else: 67 | next_command, val, info, best_action = self.control_process.get_command(t_step, state_tensor.numpy(), control_dt=control_dt) 68 | 69 | qdd_des = next_command 70 | self.prev_qdd_des = qdd_des 71 | cmd_des = self.state_filter.integrate_acc(qdd_des) 72 | 73 | return cmd_des 74 | 75 | 76 | 77 | def _state_to_tensor(self, state): 78 | state_tensor = np.concatenate((state['position'], state['velocity'], state['acceleration'])) 79 | 80 | state_tensor = torch.tensor(state_tensor) 81 | return state_tensor 82 | def get_current_error(self, curr_state): 83 | state_tensor = self._state_to_tensor(curr_state).to(**self.controller.tensor_args).unsqueeze(0) 84 | 85 | 86 | ee_error,_ = self.controller.rollout_fn.current_cost(state_tensor) 87 | ee_error = [x.detach().cpu().item() for x in ee_error] 88 | return ee_error 89 | 90 | @property 91 | def mpc_dt(self): 92 | return self.control_process.mpc_dt 93 | @property 94 | def opt_dt(self): 95 | return self.control_process.opt_dt 96 | 97 | def close(self): 98 | self.control_process.close() 99 | @property 100 | def top_trajs(self): 101 | return self.control_process.top_trajs 102 | 103 | -------------------------------------------------------------------------------- /storm_kit/mpc/utils/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | from . import helpers -------------------------------------------------------------------------------- /storm_kit/mpc/utils/helpers.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | from collections import defaultdict 24 | 25 | def default_to_regular(d): 26 | if isinstance(d, defaultdict): 27 | d = {k: default_to_regular(v) for k, v in d.items()} 28 | return d -------------------------------------------------------------------------------- /storm_kit/mpc/utils/torch_utils.py: -------------------------------------------------------------------------------- 1 | # 2 | # MIT License 3 | # 4 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a 7 | # copy of this software and associated documentation files (the "Software"), 8 | # to deal in the Software without restriction, including without limitation 9 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 10 | # and/or sell copies of the Software, and to permit persons to whom the 11 | # Software is furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 19 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | # DEALINGS IN THE SOFTWARE.# 23 | import torch 24 | 25 | def find_first_idx(array, value): 26 | f_idx = torch.nonzero(array > value, as_tuple=False)[0].item() 27 | return f_idx 28 | 29 | def find_last_idx(array, value): 30 | print(array,value) 31 | f_idx = torch.nonzero(array <= value, as_tuple=False)[-1].item() 32 | return f_idx 33 | -------------------------------------------------------------------------------- /storm_kit/util_file.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2020-2021 NVIDIA CORPORATION. All rights reserved. 3 | # 4 | # Permission is hereby granted, free of charge, to any person obtaining a 5 | # copy of this software and associated documentation files (the "Software"), 6 | # to deal in the Software without restriction, including without limitation 7 | # the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | # and/or sell copies of the Software, and to permit persons to whom the 9 | # Software is furnished to do so, subject to the following conditions: 10 | # 11 | # The above copyright notice and this permission notice shall be included in 12 | # all copies or substantial portions of the Software. 13 | # 14 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | # DEALINGS IN THE SOFTWARE.# 21 | 22 | import os 23 | import yaml 24 | 25 | # get paths 26 | def get_module_path(): 27 | path = os.path.dirname(__file__) 28 | return path 29 | 30 | def get_root_path(): 31 | path = os.path.dirname(get_module_path()) 32 | return path 33 | 34 | def get_content_path(): 35 | root_path = get_root_path() 36 | path = os.path.join(root_path,'content') 37 | return path 38 | 39 | def get_configs_path(): 40 | content_path = get_content_path() 41 | path = os.path.join(content_path,'configs') 42 | return path 43 | def get_assets_path(): 44 | content_path = get_content_path() 45 | path = os.path.join(content_path,'assets') 46 | return path 47 | 48 | def get_weights_path(): 49 | content_path = get_root_path() 50 | path = os.path.join(content_path,'weights') 51 | return path 52 | 53 | def join_path(path1,path2): 54 | return os.path.join(path1,path2) 55 | 56 | def load_yaml(file_path): 57 | with open(file_path) as file: 58 | yaml_params = yaml.load(file, Loader=yaml.FullLoader) 59 | return yaml_params 60 | 61 | # get paths for urdf 62 | def get_urdf_path(): 63 | content_path = get_content_path() 64 | path = os.path.join(content_path,'assets','urdf') 65 | return path 66 | 67 | def get_gym_configs_path(): 68 | config_path = get_configs_path() 69 | path = os.path.join(config_path, 'gym') 70 | return path 71 | 72 | def get_mpc_configs_path(): 73 | config_path = get_configs_path() 74 | path = os.path.join(config_path, 'mpc') 75 | return path 76 | -------------------------------------------------------------------------------- /weights/robot_self/README.md: -------------------------------------------------------------------------------- 1 | Contains MLP weights for robot self collision checking --------------------------------------------------------------------------------