├── .gitignore ├── .gitmodules ├── README.md ├── assets ├── 3D_printed_parts │ ├── wrist_yaw_motor_holder.stl │ └── zed_camera_mount.stl └── av-aloha.gif ├── data_collection_scripts ├── .DS_Store ├── activate.sh ├── assets │ ├── LICENSE │ ├── aloha_real.xml │ ├── aloha_sim.xml │ ├── joint_position_actuators.xml │ ├── meshes │ │ ├── angled_extrusion.stl │ │ ├── corner_bracket.stl │ │ ├── d405_solid.stl │ │ ├── extrusion_1000.stl │ │ ├── extrusion_1220.stl │ │ ├── extrusion_150.stl │ │ ├── extrusion_2040_1000.stl │ │ ├── extrusion_2040_880.stl │ │ ├── extrusion_600.stl │ │ ├── interbotix_black.png │ │ ├── overhead_mount.stl │ │ ├── small_meta_table_diffuse.png │ │ ├── tablelegs.obj │ │ ├── tabletop.obj │ │ ├── vx300s_1_base.stl │ │ ├── vx300s_2_shoulder.stl │ │ ├── vx300s_3_upper_arm.stl │ │ ├── vx300s_4_upper_forearm.stl │ │ ├── vx300s_5_lower_forearm.stl │ │ ├── vx300s_6_wrist.stl │ │ ├── vx300s_7_gripper.stl │ │ ├── vx300s_7_gripper_bar.stl │ │ ├── vx300s_7_gripper_camera.stl │ │ ├── vx300s_7_gripper_prop.stl │ │ ├── vx300s_7_gripper_prop_bar.stl │ │ ├── vx300s_7_gripper_wrist_mount.stl │ │ ├── vx300s_7dof_wrist_1.stl │ │ ├── vx300s_7dof_wrist_2.stl │ │ ├── vx300s_7dof_wrist_3.stl │ │ ├── vx300s_8_custom_finger_left.stl │ │ ├── vx300s_8_custom_finger_right.stl │ │ ├── wormseye_mount.stl │ │ └── zedm.stl │ ├── old │ │ ├── task_insert_peg_board.xml │ │ ├── task_marble_game.xml │ │ ├── task_open_box.xml │ │ ├── task_open_floss.xml │ │ ├── task_passthrough_slot.xml │ │ ├── task_separate_peg.xml │ │ ├── task_slide_in_tray.xml │ │ ├── task_transfer_balls.xml │ │ ├── task_transfer_cube.xml │ │ └── task_unbox_cube.xml │ ├── scene.xml │ ├── single_arm.xml │ ├── task_hook_package.xml │ ├── task_insert_peg.xml │ ├── task_sew_needle.xml │ ├── task_slot_insertion.xml │ └── task_tube_transfer.xml ├── constants.py ├── diff_ik.py ├── grad_ik.py ├── headset_control.py ├── headset_utils.py ├── image_recorders.py ├── kinematics.py ├── launch_robot.sh ├── real_env.py ├── record_episodes.py ├── record_sim_episodes.py ├── replay_episode.py ├── replay_sim_episode.py ├── robot_utils.py ├── sim_env.py ├── sleep.py ├── test_scripts │ ├── check_dataset_reward.py │ ├── move_grippers.py │ ├── read_grippers.py │ ├── read_joint_states.py │ ├── read_master_grippers.py │ ├── test_sim_reward.py │ ├── test_step_joints.py │ ├── test_zed.py │ ├── webrtc_receiver.py │ └── webrtc_sender.py ├── transform_utils.py ├── visualize_all_episodes.py ├── visualize_episodes.py └── webrtc_headset.py ├── eval_scripts ├── .gitignore ├── README.md ├── activate.sh ├── assets │ ├── LICENSE │ ├── aloha_real.xml │ ├── aloha_sim.xml │ ├── joint_position_actuators.xml │ ├── meshes │ │ ├── angled_extrusion.stl │ │ ├── corner_bracket.stl │ │ ├── d405_solid.stl │ │ ├── extrusion_1000.stl │ │ ├── extrusion_1220.stl │ │ ├── extrusion_150.stl │ │ ├── extrusion_2040_1000.stl │ │ ├── extrusion_2040_880.stl │ │ ├── extrusion_600.stl │ │ ├── interbotix_black.png │ │ ├── overhead_mount.stl │ │ ├── small_meta_table_diffuse.png │ │ ├── tablelegs.obj │ │ ├── tabletop.obj │ │ ├── vx300s_1_base.stl │ │ ├── vx300s_2_shoulder.stl │ │ ├── vx300s_3_upper_arm.stl │ │ ├── vx300s_4_upper_forearm.stl │ │ ├── vx300s_5_lower_forearm.stl │ │ ├── vx300s_6_wrist.stl │ │ ├── vx300s_7_gripper.stl │ │ ├── vx300s_7_gripper_bar.stl │ │ ├── vx300s_7_gripper_camera.stl │ │ ├── vx300s_7_gripper_prop.stl │ │ ├── vx300s_7_gripper_prop_bar.stl │ │ ├── vx300s_7_gripper_wrist_mount.stl │ │ ├── vx300s_7dof_wrist_1.stl │ │ ├── vx300s_7dof_wrist_2.stl │ │ ├── vx300s_7dof_wrist_3.stl │ │ ├── vx300s_8_custom_finger_left.stl │ │ ├── vx300s_8_custom_finger_right.stl │ │ ├── wormseye_mount.stl │ │ └── zedm.stl │ ├── scene.xml │ ├── single_arm.xml │ ├── task_hook_package.xml │ ├── task_insert_peg.xml │ ├── task_sew_needle.xml │ ├── task_slot_insertion.xml │ └── task_tube_transfer.xml ├── constants.py ├── eval.py ├── image_recorders.py ├── launch_robot.sh ├── real_env.py ├── robot_utils.py ├── save_policy.py ├── sleep.py └── transform_utils.py ├── guided_vision_ros ├── guided_vision │ ├── CMakeLists.txt │ ├── config │ │ ├── master_modes_left.yaml │ │ ├── master_modes_right.yaml │ │ ├── puppet_modes_left.yaml │ │ ├── puppet_modes_middle.yaml │ │ └── puppet_modes_right.yaml │ ├── launch │ │ ├── 3arms_teleop.launch │ │ └── 5arms_teleop.launch │ └── package.xml └── vx300s_7dof_description │ ├── 3D_printed_parts │ ├── wrist_yaw_motor_holder.stl │ └── zed_camera_mount.stl │ ├── CMakeLists.txt │ ├── config │ ├── modes.yaml │ └── vx300s_7dof.yaml │ ├── launch │ ├── xsarm_control.launch │ └── xsarm_description.launch │ ├── meshes │ ├── interbotix_black.png │ ├── vx300s_1_base.stl │ ├── vx300s_2_shoulder.stl │ ├── vx300s_3_upper_arm.stl │ ├── vx300s_4_upper_forearm.stl │ ├── vx300s_5_lower_forearm.stl │ ├── vx300s_7dof_wrist_1.stl │ ├── vx300s_7dof_wrist_2.stl │ ├── vx300s_7dof_wrist_3.stl │ └── zedm.stl │ ├── package.xml │ ├── rviz │ └── xsarm_description.rviz │ └── urdf │ ├── vx300s_7dof.urdf │ ├── vx300s_7dof.urdf.xacro │ └── zedm_macro.urdf.xacro ├── gym_guided_vision ├── .gitignore ├── README.md ├── gym_guided_vision │ ├── __init__.py │ ├── assets │ │ ├── LICENSE │ │ ├── aloha_sim.xml │ │ ├── joint_position_actuators.xml │ │ ├── meshes │ │ │ ├── angled_extrusion.stl │ │ │ ├── corner_bracket.stl │ │ │ ├── d405_solid.stl │ │ │ ├── extrusion_1000.stl │ │ │ ├── extrusion_1220.stl │ │ │ ├── extrusion_150.stl │ │ │ ├── extrusion_2040_1000.stl │ │ │ ├── extrusion_2040_880.stl │ │ │ ├── extrusion_600.stl │ │ │ ├── overhead_mount.stl │ │ │ ├── small_meta_table_diffuse.png │ │ │ ├── tablelegs.obj │ │ │ ├── tabletop.obj │ │ │ ├── vx300s_1_base.stl │ │ │ ├── vx300s_2_shoulder.stl │ │ │ ├── vx300s_3_upper_arm.stl │ │ │ ├── vx300s_4_upper_forearm.stl │ │ │ ├── vx300s_5_lower_forearm.stl │ │ │ ├── vx300s_6_wrist.stl │ │ │ ├── vx300s_7_gripper.stl │ │ │ ├── vx300s_7_gripper_bar.stl │ │ │ ├── vx300s_7_gripper_camera.stl │ │ │ ├── vx300s_7_gripper_prop.stl │ │ │ ├── vx300s_7_gripper_prop_bar.stl │ │ │ ├── vx300s_7_gripper_wrist_mount.stl │ │ │ ├── vx300s_7dof_wrist_1.stl │ │ │ ├── vx300s_7dof_wrist_2.stl │ │ │ ├── vx300s_7dof_wrist_3.stl │ │ │ ├── vx300s_8_custom_finger_left.stl │ │ │ ├── vx300s_8_custom_finger_right.stl │ │ │ ├── wormseye_mount.stl │ │ │ └── zedm.stl │ │ ├── scene.xml │ │ ├── task_hook_package.xml │ │ ├── task_insert_peg.xml │ │ ├── task_sew_needle.xml │ │ ├── task_slot_insertion.xml │ │ └── task_tube_transfer.xml │ ├── constants.py │ ├── env.py │ └── kinematics │ │ ├── diff_ik.py │ │ ├── grad_ik.py │ │ └── kinematics.py ├── pyproject.toml └── scripts │ ├── check_dataset_reward.py │ ├── replay_sim_episode.py │ ├── test_env.py │ ├── visualize_all_episodes.py │ └── visualize_episodes.py └── requirements.txt /.gitignore: -------------------------------------------------------------------------------- 1 | data 2 | outputs 3 | serviceAccountKey.json 4 | signalingSettings.json 5 | 6 | # Byte-compiled / optimized / DLL files 7 | __pycache__/ 8 | *.py[cod] 9 | *$py.class 10 | 11 | # C extensions 12 | *.so 13 | 14 | # Distribution / packaging 15 | .Python 16 | build/ 17 | develop-eggs/ 18 | dist/ 19 | downloads/ 20 | eggs/ 21 | .eggs/ 22 | lib/ 23 | lib64/ 24 | parts/ 25 | sdist/ 26 | var/ 27 | wheels/ 28 | share/python-wheels/ 29 | *.egg-info/ 30 | .installed.cfg 31 | *.egg 32 | MANIFEST 33 | 34 | # PyInstaller 35 | # Usually these files are written by a python script from a template 36 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 37 | *.manifest 38 | *.spec 39 | 40 | # Installer logs 41 | pip-log.txt 42 | pip-delete-this-directory.txt 43 | 44 | # Unit test / coverage reports 45 | htmlcov/ 46 | .tox/ 47 | .nox/ 48 | .coverage 49 | .coverage.* 50 | .cache 51 | nosetests.xml 52 | coverage.xml 53 | *.cover 54 | *.py,cover 55 | .hypothesis/ 56 | .pytest_cache/ 57 | cover/ 58 | 59 | # Translations 60 | *.mo 61 | *.pot 62 | 63 | # Django stuff: 64 | *.log 65 | local_settings.py 66 | db.sqlite3 67 | db.sqlite3-journal 68 | 69 | # Flask stuff: 70 | instance/ 71 | .webassets-cache 72 | 73 | # Scrapy stuff: 74 | .scrapy 75 | 76 | # Sphinx documentation 77 | docs/_build/ 78 | 79 | # PyBuilder 80 | .pybuilder/ 81 | target/ 82 | 83 | # Jupyter Notebook 84 | .ipynb_checkpoints 85 | 86 | # IPython 87 | profile_default/ 88 | ipython_config.py 89 | 90 | # pyenv 91 | # For a library or package, you might want to ignore these files since the code is 92 | # intended to run in multiple environments; otherwise, check them in: 93 | # .python-version 94 | 95 | # pipenv 96 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 97 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 98 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 99 | # install all needed dependencies. 100 | #Pipfile.lock 101 | 102 | # poetry 103 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 104 | # This is especially recommended for binary packages to ensure reproducibility, and is more 105 | # commonly ignored for libraries. 106 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 107 | #poetry.lock 108 | 109 | # pdm 110 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 111 | #pdm.lock 112 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 113 | # in version control. 114 | # https://pdm.fming.dev/latest/usage/project/#working-with-version-control 115 | .pdm.toml 116 | .pdm-python 117 | .pdm-build/ 118 | 119 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 120 | __pypackages__/ 121 | 122 | # Celery stuff 123 | celerybeat-schedule 124 | celerybeat.pid 125 | 126 | # SageMath parsed files 127 | *.sage.py 128 | 129 | # Environments 130 | .env 131 | .venv 132 | env/ 133 | venv/ 134 | ENV/ 135 | env.bak/ 136 | venv.bak/ 137 | 138 | # Spyder project settings 139 | .spyderproject 140 | .spyproject 141 | 142 | # Rope project settings 143 | .ropeproject 144 | 145 | # mkdocs documentation 146 | /site 147 | 148 | # mypy 149 | .mypy_cache/ 150 | .dmypy.json 151 | dmypy.json 152 | 153 | # Pyre type checker 154 | .pyre/ 155 | 156 | # pytype static type analyzer 157 | .pytype/ 158 | 159 | # Cython debug symbols 160 | cython_debug/ 161 | 162 | # PyCharm 163 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 164 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 165 | # and can be added to the global gitignore or merged into this file. For a more nuclear 166 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 167 | #.idea/ -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "lerobot"] 2 | path = lerobot 3 | url = https://github.com/ian-chuang/lerobot.git 4 | -------------------------------------------------------------------------------- /assets/3D_printed_parts/wrist_yaw_motor_holder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/assets/3D_printed_parts/wrist_yaw_motor_holder.stl -------------------------------------------------------------------------------- /assets/3D_printed_parts/zed_camera_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/assets/3D_printed_parts/zed_camera_mount.stl -------------------------------------------------------------------------------- /assets/av-aloha.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/assets/av-aloha.gif -------------------------------------------------------------------------------- /data_collection_scripts/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/.DS_Store -------------------------------------------------------------------------------- /data_collection_scripts/activate.sh: -------------------------------------------------------------------------------- 1 | conda deactivate 2 | conda deactivate 3 | conda deactivate 4 | source /opt/ros/noetic/setup.sh && source ~/interbotix_ws/devel/setup.sh 5 | conda activate lerobot -------------------------------------------------------------------------------- /data_collection_scripts/assets/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2023, Trossen Robotics 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | 3. Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/joint_position_actuators.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 40 | 41 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/angled_extrusion.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/angled_extrusion.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/corner_bracket.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/corner_bracket.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/d405_solid.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/d405_solid.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/extrusion_1000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/extrusion_1000.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/extrusion_1220.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/extrusion_1220.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/extrusion_150.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/extrusion_150.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/extrusion_2040_1000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/extrusion_2040_1000.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/extrusion_2040_880.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/extrusion_2040_880.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/extrusion_600.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/extrusion_600.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/interbotix_black.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/interbotix_black.png -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/overhead_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/overhead_mount.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/small_meta_table_diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/small_meta_table_diffuse.png -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/tabletop.obj: -------------------------------------------------------------------------------- 1 | mtllib material_0.mtl 2 | usemtl material_0 3 | v -0.37953900 -0.60982800 0.73674500 4 | v -0.37953900 -0.60982800 0.74887200 5 | v -0.38100000 -0.60839100 0.73674500 6 | v -0.38100000 -0.60839100 0.74887200 7 | v -0.18977000 -0.60982800 0.74887200 8 | v -0.18977000 -0.60839100 0.75078500 9 | v -0.37953900 -0.60839100 0.75078700 10 | v -0.38100000 0.60793400 0.74887200 11 | v -0.37953900 0.60793400 0.75078700 12 | v 0.38100000 -0.60839100 0.73674500 13 | v 0.38100000 -0.60839100 0.74887200 14 | v 0.37953900 -0.60982800 0.73674500 15 | v 0.37953900 -0.60982800 0.74887200 16 | v 0.37953900 0.60793400 0.75078700 17 | v 0.37953900 -0.60839100 0.75078700 18 | v 0.38100000 0.60793400 0.74887200 19 | v -0.38100000 0.60793400 0.73674500 20 | v -0.37953900 0.60937200 0.73674500 21 | v -0.37953900 0.60937200 0.74887200 22 | v -0.18977000 0.60793400 0.75078700 23 | v -0.18977000 0.60937200 0.74887200 24 | v 0.37953900 0.60937200 0.73674500 25 | v 0.37953900 0.60937200 0.74887200 26 | v 0.38100000 0.60793400 0.73674500 27 | v -0.37953900 -0.60839100 0.73483100 28 | v -0.37953900 0.60793400 0.73483100 29 | v -0.18977000 0.60937200 0.73674500 30 | v -0.18977000 0.60793400 0.73483100 31 | v 0.37953900 -0.60839100 0.73483100 32 | v 0.37953900 0.60793400 0.73483100 33 | v -0.18977000 -0.60839100 0.73483100 34 | v -0.18977000 -0.60982800 0.73674500 35 | v 0.18977000 0.60793400 0.75078700 36 | v 0.18977000 -0.60839100 0.75078700 37 | v 0.18977000 0.60937200 0.74887200 38 | v 0.18977000 0.60937200 0.73674500 39 | v 0.18977000 0.60793400 0.73483100 40 | v 0.18977000 -0.60839100 0.73483100 41 | v 0.18977000 -0.60982800 0.73674500 42 | v 0.18977000 -0.60982800 0.74887200 43 | v 0.00000000 0.60793400 0.75078700 44 | v 0.00000000 -0.60839100 0.75078500 45 | v 0.00000000 0.60937200 0.74887200 46 | v 0.00000000 0.60937200 0.73674500 47 | v 0.00000000 0.60793400 0.73483100 48 | v 0.00000000 -0.60839100 0.73483100 49 | v 0.00000000 -0.60982800 0.73674500 50 | v 0.00000000 -0.60982800 0.74887200 51 | vt 0.75019800 0.57261000 52 | vt 0.74519900 0.57261000 53 | vt 0.74382100 0.56603500 54 | vt 0.74382100 0.57103100 55 | vt 0.74519900 0.67519800 56 | vt 0.74362100 0.67519800 57 | vt 0.74362100 0.57261000 58 | vt 0.12297800 0.57103200 59 | vt 0.12297900 0.57261000 60 | vt 0.74362100 0.15331600 61 | vt 0.74382100 0.98454000 62 | vt 0.75019800 0.98296000 63 | vt 0.74519900 0.98296000 64 | vt 0.12381100 0.98296000 65 | vt 0.74362100 0.98296000 66 | vt 0.12381700 0.98453800 67 | vt 0.11445600 0.57182100 68 | vt 0.11448400 0.57261000 69 | vt 0.12160500 0.57261000 70 | vt 0.12318700 0.67519800 71 | vt 0.12181300 0.67519800 72 | vt 0.11531600 0.98296000 73 | vt 0.12243700 0.98296000 74 | vt 0.11524200 0.98375000 75 | vt 0.74362100 0.56445500 76 | vt 0.12296000 0.56445500 77 | vt 0.11469200 0.67519800 78 | vt 0.11331800 0.67519800 79 | vt 0.74362100 0.15410500 80 | vt 0.12256600 0.15410500 81 | vt 0.74362100 0.46186700 82 | vt 0.74441000 0.46186700 83 | vt 0.12360300 0.88037300 84 | vt 0.74362100 0.88037300 85 | vt 0.12222900 0.88037300 86 | vt 0.11510800 0.88037300 87 | vt 0.11373500 0.88037300 88 | vt 0.74362100 0.25669200 89 | vt 0.74441000 0.25669200 90 | vt 0.74519900 0.88037300 91 | vt 0.12339500 0.77778500 92 | vt 0.74362100 0.77778500 93 | vt 0.12202100 0.77778500 94 | vt 0.11490000 0.77778500 95 | vt 0.11352700 0.77778500 96 | vt 0.74362100 0.35928000 97 | vt 0.74441000 0.35928000 98 | vt 0.74519900 0.77778500 99 | f 1/1 2/2 3/3 100 | f 3/3 2/2 4/4 101 | f 5/5 6/6 2/2 102 | f 2/2 6/6 7/7 103 | f 8/8 4/4 9/9 104 | f 9/9 4/4 7/7 105 | f 10/10 11/11 12/12 106 | f 12/12 11/11 13/13 107 | f 14/14 15/15 16/16 108 | f 16/16 15/15 11/11 109 | f 17/17 8/8 18/18 110 | f 18/18 8/8 19/19 111 | f 20/20 21/21 9/9 112 | f 9/9 21/21 19/19 113 | f 22/22 23/23 24/24 114 | f 24/24 23/23 16/16 115 | f 3/3 17/17 25/25 116 | f 25/25 17/17 26/26 117 | f 27/27 28/28 18/18 118 | f 18/18 28/28 26/26 119 | f 29/29 30/30 10/10 120 | f 10/10 30/30 24/24 121 | f 31/31 32/32 25/25 122 | f 25/25 32/32 1/1 123 | f 9/9 7/7 20/20 124 | f 20/20 7/7 6/6 125 | f 18/18 19/19 27/27 126 | f 27/27 19/19 21/21 127 | f 25/25 26/26 31/31 128 | f 31/31 26/26 28/28 129 | f 2/2 1/1 5/5 130 | f 5/5 1/1 32/32 131 | f 16/16 11/11 24/24 132 | f 24/24 11/11 10/10 133 | f 17/17 3/3 8/8 134 | f 8/8 3/3 4/4 135 | f 2/2 7/7 4/4 136 | f 11/11 15/15 13/13 137 | f 8/8 9/9 19/19 138 | f 23/23 14/14 16/16 139 | f 17/17 18/18 26/26 140 | f 30/30 22/22 24/24 141 | f 3/3 25/25 1/1 142 | f 12/12 29/29 10/10 143 | f 33/33 34/34 14/14 144 | f 14/14 34/34 15/15 145 | f 23/23 35/35 14/14 146 | f 14/14 35/35 33/33 147 | f 36/36 35/35 22/22 148 | f 22/22 35/35 23/23 149 | f 30/30 37/37 22/22 150 | f 22/22 37/37 36/36 151 | f 38/38 37/37 29/29 152 | f 29/29 37/37 30/30 153 | f 12/12 39/39 29/29 154 | f 29/29 39/39 38/38 155 | f 40/40 39/39 13/13 156 | f 13/13 39/39 12/12 157 | f 15/15 34/34 13/13 158 | f 13/13 34/34 40/40 159 | f 41/41 42/42 33/33 160 | f 33/33 42/42 34/34 161 | f 33/33 35/35 41/41 162 | f 41/41 35/35 43/43 163 | f 44/44 43/43 36/36 164 | f 36/36 43/43 35/35 165 | f 36/36 37/37 44/44 166 | f 44/44 37/37 45/45 167 | f 46/46 45/45 38/38 168 | f 38/38 45/45 37/37 169 | f 38/38 39/39 46/46 170 | f 46/46 39/39 47/47 171 | f 48/48 47/47 40/40 172 | f 40/40 47/47 39/39 173 | f 40/40 34/34 48/48 174 | f 48/48 34/34 42/42 175 | f 20/20 6/6 41/41 176 | f 41/41 6/6 42/42 177 | f 41/41 43/43 20/20 178 | f 20/20 43/43 21/21 179 | f 27/27 21/21 44/44 180 | f 44/44 21/21 43/43 181 | f 44/44 45/45 27/27 182 | f 27/27 45/45 28/28 183 | f 31/31 28/28 46/46 184 | f 46/46 28/28 45/45 185 | f 46/46 47/47 31/31 186 | f 31/31 47/47 32/32 187 | f 5/5 32/32 48/48 188 | f 48/48 32/32 47/47 189 | f 48/48 42/42 5/5 190 | f 5/5 42/42 6/6 191 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_1_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_1_base.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_2_shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_2_shoulder.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_3_upper_arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_3_upper_arm.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_4_upper_forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_4_upper_forearm.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_5_lower_forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_5_lower_forearm.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_6_wrist.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_6_wrist.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_7_gripper.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_7_gripper.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_7_gripper_bar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_7_gripper_bar.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_7_gripper_camera.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_7_gripper_camera.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_7_gripper_prop.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_7_gripper_prop.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_7_gripper_prop_bar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_7_gripper_prop_bar.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_7_gripper_wrist_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_7_gripper_wrist_mount.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_7dof_wrist_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_7dof_wrist_1.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_7dof_wrist_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_7dof_wrist_2.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_7dof_wrist_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_7dof_wrist_3.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_8_custom_finger_left.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_8_custom_finger_left.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/vx300s_8_custom_finger_right.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/vx300s_8_custom_finger_right.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/wormseye_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/wormseye_mount.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/meshes/zedm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/meshes/zedm.stl -------------------------------------------------------------------------------- /data_collection_scripts/assets/old/task_insert_peg_board.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/old/task_marble_game.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/old/task_open_box.xml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/old/task_open_box.xml -------------------------------------------------------------------------------- /data_collection_scripts/assets/old/task_open_floss.xml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/old/task_open_floss.xml -------------------------------------------------------------------------------- /data_collection_scripts/assets/old/task_passthrough_slot.xml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/old/task_passthrough_slot.xml -------------------------------------------------------------------------------- /data_collection_scripts/assets/old/task_separate_peg.xml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/old/task_separate_peg.xml -------------------------------------------------------------------------------- /data_collection_scripts/assets/old/task_slide_in_tray.xml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/data_collection_scripts/assets/old/task_slide_in_tray.xml -------------------------------------------------------------------------------- /data_collection_scripts/assets/old/task_transfer_balls.xml: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/old/task_transfer_cube.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/old/task_unbox_cube.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/task_hook_package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/task_insert_peg.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/task_sew_needle.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/task_slot_insertion.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /data_collection_scripts/assets/task_tube_transfer.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 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 | -------------------------------------------------------------------------------- /data_collection_scripts/headset_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numba import jit, float64 3 | from numba.types import UniTuple 4 | from scipy.spatial.transform import Rotation as R 5 | from transform_utils import ( 6 | pose2mat, 7 | mat2pose, 8 | ) 9 | 10 | TRANSFORM_TO_WORLD = np.ascontiguousarray(np.eye(4)) 11 | TRANSFORM_TO_WORLD[:3, :3] = R.from_euler('xyz', [-90, 0, -90], degrees=True).as_matrix() 12 | WORLD_TO_TRANSFORM = np.ascontiguousarray(np.linalg.inv(TRANSFORM_TO_WORLD)) 13 | 14 | class HeadsetData: 15 | h_pos = np.zeros(3) 16 | h_quat = np.zeros(4) 17 | l_pos = np.zeros(3) 18 | l_quat = np.zeros(4) 19 | l_thumbstick_x = 0 20 | l_thumbstick_y = 0 21 | l_index_trigger = 0 22 | l_hand_trigger = 0 23 | l_button_one = False 24 | l_button_two = False 25 | l_button_thumbstick = False 26 | r_pos = np.zeros(3) 27 | r_quat = np.zeros(4) 28 | r_thumbstick_x = 0 29 | r_thumbstick_y = 0 30 | r_index_trigger = 0 31 | r_hand_trigger = 0 32 | r_button_one = False 33 | r_button_two = False 34 | r_button_thumbstick = False 35 | 36 | class HeadsetFeedback: 37 | head_out_of_sync = False 38 | left_out_of_sync = False 39 | right_out_of_sync = False 40 | info = "" 41 | left_arm_position = np.zeros(3) 42 | left_arm_rotation = np.zeros(4) 43 | right_arm_position = np.zeros(3) 44 | right_arm_rotation = np.zeros(4) 45 | middle_arm_position = np.zeros(3) 46 | middle_arm_rotation = np.zeros(4) 47 | 48 | @jit(UniTuple(float64[:], 2)(float64[:], float64[:]), nopython=True, fastmath=True, cache=True) 49 | def convert_left_to_right_coordinates(left_pos, left_quat): 50 | 51 | x = left_pos[0] 52 | y = -left_pos[1] # flip y from left to right 53 | z = left_pos[2] 54 | qx = -left_quat[0] # flip rotation from left to right 55 | qy = left_quat[1] 56 | qz = -left_quat[2] # flip rotation from left to right 57 | qw = left_quat[3] 58 | 59 | transform = pose2mat(np.array([x, y, z]), np.array([qx, qy, qz, qw])) 60 | 61 | transform = np.ascontiguousarray(transform) 62 | 63 | transform = TRANSFORM_TO_WORLD @ transform 64 | 65 | right_pos, right_quat = mat2pose(transform) 66 | 67 | return right_pos, right_quat 68 | 69 | @jit(UniTuple(float64[:], 2)(float64[:], float64[:]), nopython=True, fastmath=True, cache=True) 70 | def convert_right_to_left_coordinates(right_pos, right_quat): 71 | 72 | transform = pose2mat(right_pos, right_quat) 73 | 74 | transform = np.ascontiguousarray(transform) 75 | 76 | transform = WORLD_TO_TRANSFORM @ transform 77 | 78 | pos, quat = mat2pose(transform) 79 | 80 | x = pos[0] 81 | y = -pos[1] # flip y from right to left 82 | z = pos[2] 83 | qx = -quat[0] # flip rotation from right to left 84 | qy = quat[1] 85 | qz = -quat[2] # flip rotation from right to left 86 | qw = quat[3] 87 | 88 | return np.array([x, y, z]), np.array([qx, qy, qz, qw]) -------------------------------------------------------------------------------- /data_collection_scripts/launch_robot.sh: -------------------------------------------------------------------------------- 1 | conda deactivate 2 | conda deactivate 3 | conda deactivate 4 | source /opt/ros/noetic/setup.sh && source ~/interbotix_ws/devel/setup.sh 5 | roslaunch guided_vision 3arms_teleop.launch -------------------------------------------------------------------------------- /data_collection_scripts/replay_episode.py: -------------------------------------------------------------------------------- 1 | import os 2 | import h5py 3 | from robot_utils import move_grippers 4 | import argparse 5 | from real_env import make_real_env 6 | from robot_utils import sleep 7 | from constants import REAL_DT 8 | import time 9 | 10 | import IPython 11 | e = IPython.embed 12 | 13 | def main(args): 14 | 15 | dataset_dir = args['dataset_dir'] 16 | episode_idx = args['episode_idx'] 17 | dataset_name = f'episode_{episode_idx}' 18 | 19 | dataset_path = os.path.join(dataset_dir, dataset_name + '.hdf5') 20 | if not os.path.isfile(dataset_path): 21 | print(f'Dataset does not exist at \n{dataset_path}\n') 22 | exit() 23 | 24 | with h5py.File(dataset_path, 'r') as root: 25 | actions = root['/action'][()] 26 | 27 | 28 | env = make_real_env(init_node=True) 29 | env.reset() 30 | for action in actions: 31 | 32 | step_start = time.time() 33 | 34 | 35 | env.step(action, all_joints=True) # print(action) shows all 21 actions.. 36 | 37 | # Rudimentary time keeping, will drift relative to wall clock. 38 | time_until_next_step = REAL_DT - (time.time() - step_start) 39 | time.sleep(max(0, time_until_next_step)) 40 | 41 | 42 | sleep(env.left_bot, env.right_bot, env.middle_bot) 43 | 44 | 45 | if __name__ == '__main__': 46 | parser = argparse.ArgumentParser() 47 | parser.add_argument('--dataset_dir', action='store', type=str, help='Dataset dir.', required=True) 48 | parser.add_argument('--episode_idx', action='store', type=int, help='Episode index.', required=False) 49 | main(vars(parser.parse_args())) 50 | 51 | 52 | -------------------------------------------------------------------------------- /data_collection_scripts/replay_sim_episode.py: -------------------------------------------------------------------------------- 1 | import os 2 | import h5py 3 | import argparse 4 | from sim_env import make_sim_env 5 | from constants import SIM_DT, SIM_TASK_CONFIGS 6 | import time 7 | import numpy as np 8 | import glob 9 | from tqdm import tqdm 10 | 11 | def save_episode(data_dict, save_path): 12 | 13 | camera_names = [key.split('/')[-1] for key in data_dict.keys() if 'images' in key] 14 | max_timesteps = len(data_dict['/observations/qpos']) 15 | 16 | # HDF5 17 | try: 18 | if len(camera_names) > 0: 19 | image_shapes = {cam_name: data_dict[f'/observations/images/{cam_name}'][0].shape for cam_name in camera_names} 20 | qpos_len = len(data_dict['/observations/qpos'][0]) 21 | qvel_len = len(data_dict['/observations/qvel'][0]) 22 | action_len = len(data_dict['/action'][0]) 23 | except IndexError: 24 | print('Empty episode, skipping...') 25 | return False 26 | t0 = time.time() 27 | with h5py.File(save_path, 'w', rdcc_nbytes=1024 ** 2 * 2) as root: 28 | root.attrs['sim'] = True 29 | obs = root.create_group('observations') 30 | if len(camera_names) > 0: 31 | image = obs.create_group('images') 32 | for cam_name in camera_names: 33 | _ = image.create_dataset(cam_name, (max_timesteps, *image_shapes[cam_name]), dtype='uint8', 34 | chunks=(1, *image_shapes[cam_name]), ) 35 | # compression='gzip',compression_opts=2,) 36 | # compression=32001, compression_opts=(0, 0, 0, 0, 9, 1, 1), shuffle=False) 37 | data_qpos = obs.create_dataset('qpos', (max_timesteps, qpos_len)) 38 | data_qvel = obs.create_dataset('qvel', (max_timesteps, qvel_len)) 39 | data_action = root.create_dataset('action', (max_timesteps, action_len)) 40 | 41 | for name, array in data_dict.items(): 42 | root[name][...] = array 43 | # print(f'Saving: {time.time() - t0:.1f} secs\n') 44 | return True 45 | 46 | 47 | def replay_episode(env, dataset_path, num_arms, camera_names): 48 | if not os.path.isfile(dataset_path): 49 | print(f'Dataset does not exist at \n{dataset_path}\n') 50 | exit() 51 | 52 | with h5py.File(dataset_path, 'r') as root: 53 | qpos = root['/observations/qpos'][()] 54 | qvel = root['/observations/qvel'][()] 55 | all_qpos = root['/observations/all_qpos'][()] 56 | action = root['/action'][()] 57 | 58 | if num_arms == 2: 59 | env.hide_middle_arm() 60 | data_dict = { 61 | '/observations/qpos': qpos[:,:14], 62 | '/observations/qvel': qvel[:,:14], 63 | '/action': action[:,:14], 64 | } 65 | else: 66 | data_dict = { 67 | '/observations/qpos': qpos, 68 | '/observations/qvel': qvel, 69 | '/action': action, 70 | } 71 | for cam_name in camera_names: 72 | data_dict[f'/observations/images/{cam_name}'] = [] 73 | 74 | 75 | ts, info = env.reset() 76 | 77 | for qpos in all_qpos: 78 | 79 | step_start = time.time() 80 | 81 | # Apply the action 82 | env.set_qpos(qpos) 83 | ts = env.get_obs() 84 | 85 | # Save the images 86 | for cam_name in camera_names: 87 | data_dict[f'/observations/images/{cam_name}'].append(ts['images'][cam_name]) 88 | 89 | # print(f'Step time: {time.time() - step_start:.2f} secs') 90 | 91 | # Save the episode add prefix to the path specifying the number of arms 92 | save_path = dataset_path.replace('episode', f'{num_arms}arms/episode') 93 | save_episode(data_dict, save_path) 94 | 95 | 96 | def main(args): 97 | dataset_dir = SIM_TASK_CONFIGS[args["task_name"]]["dataset_dir"] 98 | camera_names = SIM_TASK_CONFIGS[args["task_name"]]["camera_names"] 99 | # camera_names = ['zed_cam'] 100 | episode_idx = args["episode_idx"] 101 | num_arms = args["num_arms"] 102 | try: 103 | os.mkdir(os.path.join(dataset_dir, f'{num_arms}arms')) 104 | except: 105 | pass 106 | 107 | if num_arms not in [2, 3]: 108 | raise ValueError('Number of arms must be 2 or 3.') 109 | 110 | 111 | if num_arms == 2: 112 | # remove 'zed_cam' from camera_names 113 | camera_names = [cam_name for cam_name in camera_names if cam_name != 'zed_cam'] 114 | 115 | 116 | env = make_sim_env(args["task_name"], cameras=camera_names) 117 | 118 | if episode_idx is None: 119 | dataset_paths = glob.glob(os.path.join(dataset_dir, f'episode_*.hdf5')) 120 | dataset_paths.sort() 121 | else: 122 | dataset_paths = glob.glob(os.path.join(dataset_dir, f'episode_{episode_idx}.hdf5')) 123 | 124 | for dataset_path in tqdm(dataset_paths): 125 | replay_episode(env, dataset_path, num_arms, camera_names) 126 | 127 | if __name__ == '__main__': 128 | parser = argparse.ArgumentParser() 129 | parser.add_argument('--task_name', action='store', type=str, help='Task name.', required=True) 130 | parser.add_argument('--episode_idx', action='store', type=int, help='Episode index.', required=False) 131 | parser.add_argument('--num_arms', action='store', type=int, help='Number of arms.', default=3) 132 | 133 | main(vars(parser.parse_args())) 134 | 135 | 136 | -------------------------------------------------------------------------------- /data_collection_scripts/robot_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import time 3 | from constants import REAL_DT 4 | from interbotix_xs_msgs.msg import JointSingleCommand 5 | import time 6 | 7 | DT = REAL_DT 8 | 9 | import IPython 10 | e = IPython.embed 11 | 12 | def get_arm_joint_positions(bot): 13 | if hasattr(bot, 'gripper') is False: 14 | return bot.arm.core.joint_states.position[:7] 15 | return bot.arm.core.joint_states.position[:6] 16 | 17 | def get_arm_gripper_positions(bot): 18 | # check if bot has attr gripper 19 | if hasattr(bot, 'gripper') is False: 20 | raise NotImplementedError("This bot does not have a gripper") 21 | joint_position = bot.gripper.core.joint_states.position[6] 22 | return joint_position 23 | 24 | def move_arms(bot_list, target_pose_list, move_time=1): 25 | num_steps = int(move_time / DT) 26 | curr_pose_list = [get_arm_joint_positions(bot) for bot in bot_list] 27 | traj_list = [np.linspace(curr_pose, target_pose, num_steps) for curr_pose, target_pose in zip(curr_pose_list, target_pose_list)] 28 | for t in range(num_steps): 29 | for bot_id, bot in enumerate(bot_list): 30 | bot.arm.set_joint_positions(traj_list[bot_id][t], blocking=False) 31 | time.sleep(DT) 32 | 33 | def move_grippers(bot_list, target_pose_list, move_time): 34 | gripper_command = JointSingleCommand(name="gripper") 35 | num_steps = int(move_time / DT) 36 | curr_pose_list = [get_arm_gripper_positions(bot) for bot in bot_list] 37 | traj_list = [np.linspace(curr_pose, target_pose, num_steps) for curr_pose, target_pose in zip(curr_pose_list, target_pose_list)] 38 | for t in range(num_steps): 39 | for bot_id, bot in enumerate(bot_list): 40 | gripper_command.cmd = traj_list[bot_id][t] 41 | bot.gripper.core.pub_single.publish(gripper_command) 42 | time.sleep(DT) 43 | 44 | def setup_puppet_bot(bot): 45 | if hasattr(bot, 'gripper') is False: 46 | bot.dxl.robot_set_operating_modes("group", "arm", "position") 47 | else: 48 | bot.dxl.robot_reboot_motors("single", "gripper", True) 49 | bot.dxl.robot_set_operating_modes("group", "arm", "position") 50 | bot.dxl.robot_set_operating_modes("single", "gripper", "current_based_position") 51 | torque_on(bot) 52 | 53 | def setup_master_bot(bot): 54 | bot.dxl.robot_set_operating_modes("group", "arm", "pwm") 55 | bot.dxl.robot_set_operating_modes("single", "gripper", "current_based_position") 56 | torque_off(bot) 57 | 58 | def set_standard_pid_gains(bot): 59 | bot.dxl.robot_set_motor_registers("group", "arm", 'Position_P_Gain', 800) 60 | bot.dxl.robot_set_motor_registers("group", "arm", 'Position_I_Gain', 0) 61 | 62 | def set_low_pid_gains(bot): 63 | bot.dxl.robot_set_motor_registers("group", "arm", 'Position_P_Gain', 100) 64 | bot.dxl.robot_set_motor_registers("group", "arm", 'Position_I_Gain', 0) 65 | 66 | def torque_off(bot): 67 | bot.dxl.robot_torque_enable("group", "arm", False) 68 | if hasattr(bot, 'gripper') is True: 69 | bot.dxl.robot_torque_enable("single", "gripper", False) 70 | 71 | 72 | def torque_on(bot): 73 | bot.dxl.robot_torque_enable("group", "arm", True) 74 | if hasattr(bot, 'gripper') is True: 75 | bot.dxl.robot_torque_enable("single", "gripper", True) 76 | 77 | def sleep(puppet_bot_left, puppet_bot_right, puppet_bot_middle): 78 | all_bots = [puppet_bot_left, puppet_bot_right, puppet_bot_middle] 79 | for bot in all_bots: 80 | torque_on(bot) 81 | 82 | left_puppet_sleep_position = (0, -1.7, 1.55, 0, 0.65, 0) 83 | right_puppet_sleep_position = (0, -1.7, 1.55, 0, 0.65, 0) 84 | middle_puppet_sleep_position = (0, -1.7, 1.55, 0, 0.65, 0, 0) 85 | move_arms(all_bots, [left_puppet_sleep_position, right_puppet_sleep_position, middle_puppet_sleep_position], move_time=3) 86 | 87 | left_puppet_sleep_position = (0, -1.85, 1.6, 0, 0.65, 0) 88 | right_puppet_sleep_position = (0, -1.85, 1.6, 0, 0.65, 0) 89 | middle_puppet_sleep_position = (0, -1.85, 1.6, 0, 0.65, 0, 0) 90 | move_arms(all_bots, [left_puppet_sleep_position, right_puppet_sleep_position, middle_puppet_sleep_position], move_time=3) 91 | -------------------------------------------------------------------------------- /data_collection_scripts/sleep.py: -------------------------------------------------------------------------------- 1 | from interbotix_xs_modules.arm import InterbotixManipulatorXS 2 | from robot_utils import sleep 3 | 4 | def main(): 5 | puppet_bot_left = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper", robot_name=f'puppet_left', init_node=True) 6 | puppet_bot_right = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper", robot_name=f'puppet_right', init_node=False) 7 | puppet_bot_middle = InterbotixManipulatorXS(robot_model="vx300s_7dof", group_name="arm", gripper_name=None, robot_name=f"puppet_middle", init_node=False) 8 | 9 | sleep(puppet_bot_left, puppet_bot_right, puppet_bot_middle) 10 | 11 | if __name__ == '__main__': 12 | main() 13 | -------------------------------------------------------------------------------- /data_collection_scripts/test_scripts/check_dataset_reward.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('../') 3 | 4 | import os 5 | import h5py 6 | import argparse 7 | from sim_env import make_sim_env 8 | from constants import SIM_DT, SIM_TASK_CONFIGS 9 | import time 10 | import numpy as np 11 | import glob 12 | 13 | def main(args): 14 | dataset_dir = SIM_TASK_CONFIGS[args["task_name"]]["dataset_dir"] 15 | num_episodes = SIM_TASK_CONFIGS[args["task_name"]]["num_episodes"] 16 | 17 | env = make_sim_env(args["task_name"], cameras=[]) 18 | 19 | 20 | for episode_idx in range(num_episodes): 21 | dataset_path = os.path.join(dataset_dir, f'episode_{episode_idx}.hdf5') 22 | with h5py.File(dataset_path, 'r') as root: 23 | all_qpos = root['/observations/all_qpos'][()] 24 | 25 | 26 | ts, info = env.reset() 27 | 28 | reward = [] 29 | 30 | for qpos in all_qpos: 31 | 32 | step_start = time.time() 33 | 34 | # Apply the action 35 | env.set_qpos(qpos) 36 | env.render_viewer() 37 | 38 | reward.append(env.get_reward()) 39 | 40 | # Rudimentary time keeping, will drift relative to wall clock. 41 | time_until_next_step = 0.001 - (time.time() - step_start) 42 | time.sleep(max(0, time_until_next_step)) 43 | 44 | if max(reward) != env.max_reward: 45 | print(f"Episode {episode_idx} has max reward {max(reward)}") 46 | 47 | 48 | 49 | if __name__ == '__main__': 50 | parser = argparse.ArgumentParser() 51 | parser.add_argument('--task_name', action='store', type=str, help='Task name.', required=True) 52 | 53 | main(vars(parser.parse_args())) 54 | 55 | 56 | -------------------------------------------------------------------------------- /data_collection_scripts/test_scripts/move_grippers.py: -------------------------------------------------------------------------------- 1 | from interbotix_xs_modules.arm import InterbotixManipulatorXS 2 | from ..robot_utils import move_grippers, torque_on 3 | from ..constants import ( 4 | LEFT_GRIPPER_JOINT_CLOSE, 5 | LEFT_GRIPPER_JOINT_OPEN, 6 | RIGHT_GRIPPER_JOINT_CLOSE, 7 | RIGHT_GRIPPER_JOINT_OPEN, 8 | ) 9 | import rospy 10 | 11 | def main(): 12 | puppet_bot_left = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper", robot_name=f'puppet_left', init_node=True) 13 | puppet_bot_right = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper", robot_name=f'puppet_right', init_node=False) 14 | 15 | puppet_bot_left.dxl.robot_reboot_motors("single", "gripper", True) 16 | puppet_bot_left.dxl.robot_set_operating_modes("single", "gripper", "current_based_position") 17 | puppet_bot_right.dxl.robot_reboot_motors("single", "gripper", True) 18 | puppet_bot_right.dxl.robot_set_operating_modes("single", "gripper", "current_based_position") 19 | 20 | torque_on(puppet_bot_left) 21 | torque_on(puppet_bot_right) 22 | 23 | move_grippers([puppet_bot_left, puppet_bot_right], [LEFT_GRIPPER_JOINT_OPEN, RIGHT_GRIPPER_JOINT_OPEN], move_time=1.0) 24 | rospy.sleep(1.0) 25 | move_grippers([puppet_bot_left, puppet_bot_right], [LEFT_GRIPPER_JOINT_CLOSE, RIGHT_GRIPPER_JOINT_CLOSE], move_time=1.0) 26 | 27 | if __name__ == '__main__': 28 | main() 29 | -------------------------------------------------------------------------------- /data_collection_scripts/test_scripts/read_grippers.py: -------------------------------------------------------------------------------- 1 | from interbotix_xs_modules.arm import InterbotixManipulatorXS 2 | import os, sys 3 | sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) 4 | from robot_utils import get_arm_gripper_positions, torque_off 5 | import rospy 6 | 7 | def main(): 8 | puppet_bot_left = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper", robot_name=f'puppet_left', init_node=True) 9 | puppet_bot_right = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper", robot_name=f'puppet_right', init_node=False) 10 | torque_off(puppet_bot_left) 11 | torque_off(puppet_bot_right) 12 | while not rospy.is_shutdown(): 13 | print(f"left gripper pos: {get_arm_gripper_positions(puppet_bot_left)}, right gripper pos: {get_arm_gripper_positions(puppet_bot_right)}") 14 | rospy.sleep(0.2) 15 | 16 | if __name__ == '__main__': 17 | main() 18 | -------------------------------------------------------------------------------- /data_collection_scripts/test_scripts/read_joint_states.py: -------------------------------------------------------------------------------- 1 | from interbotix_xs_modules.arm import InterbotixManipulatorXS 2 | from robot_utils import torque_off 3 | import rospy 4 | 5 | def main(): 6 | puppet_bot_left = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper", robot_name=f'puppet_left', init_node=True) 7 | 8 | torque_off(puppet_bot_left) 9 | 10 | while not rospy.is_shutdown(): 11 | start_time = rospy.Time.now() 12 | print("velocity", puppet_bot_left.arm.core.joint_states.velocity[:6]) 13 | print("position", puppet_bot_left.arm.core.joint_states.position[:6]) 14 | rospy.sleep(0.2) 15 | 16 | 17 | if __name__ == '__main__': 18 | main() 19 | -------------------------------------------------------------------------------- /data_collection_scripts/test_scripts/read_master_grippers.py: -------------------------------------------------------------------------------- 1 | from interbotix_xs_modules.arm import InterbotixManipulatorXS 2 | import os, sys 3 | sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) 4 | from robot_utils import get_arm_gripper_positions, torque_off 5 | import rospy 6 | 7 | def main(): 8 | master_bot_left = InterbotixManipulatorXS(robot_model="wx250s", group_name="arm", gripper_name="gripper", robot_name=f'master_left', init_node=True) 9 | master_bot_right = InterbotixManipulatorXS(robot_model="wx250s", group_name="arm", gripper_name="gripper", robot_name=f'master_right', init_node=False) 10 | torque_off(master_bot_left) 11 | torque_off(master_bot_right) 12 | while not rospy.is_shutdown(): 13 | print(f"left gripper pos: {get_arm_gripper_positions(master_bot_left)}, right gripper pos: {get_arm_gripper_positions(master_bot_right)}") 14 | rospy.sleep(0.2) 15 | 16 | if __name__ == '__main__': 17 | main() 18 | -------------------------------------------------------------------------------- /data_collection_scripts/test_scripts/test_sim_reward.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('../') 3 | 4 | import os 5 | import h5py 6 | import argparse 7 | from sim_env import make_sim_env 8 | from constants import SIM_DT, SIM_TASK_CONFIGS 9 | import time 10 | import numpy as np 11 | import glob 12 | 13 | def main(args): 14 | dataset_dir = SIM_TASK_CONFIGS[args["task_name"]]["dataset_dir"] 15 | episode_idx = args["episode_idx"] 16 | 17 | dataset_path = os.path.join(dataset_dir, f'episode_{episode_idx}.hdf5') 18 | with h5py.File(dataset_path, 'r') as root: 19 | all_qpos = root['/observations/all_qpos'][()] 20 | 21 | env = make_sim_env(args["task_name"], cameras=[]) 22 | 23 | ts, info = env.reset() 24 | 25 | for qpos in all_qpos: 26 | 27 | step_start = time.time() 28 | 29 | # Apply the action 30 | env.set_qpos(qpos) 31 | env.render_viewer() 32 | 33 | print(env.get_reward()) 34 | 35 | # Rudimentary time keeping, will drift relative to wall clock. 36 | time_until_next_step = SIM_DT - (time.time() - step_start) 37 | time.sleep(max(0, time_until_next_step)) 38 | 39 | if __name__ == '__main__': 40 | parser = argparse.ArgumentParser() 41 | parser.add_argument('--task_name', action='store', type=str, help='Task name.', required=True) 42 | parser.add_argument('--episode_idx', action='store', type=int, help='Episode index.', required=False) 43 | 44 | main(vars(parser.parse_args())) 45 | 46 | 47 | -------------------------------------------------------------------------------- /data_collection_scripts/test_scripts/test_step_joints.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('../') 3 | 4 | import os 5 | import h5py 6 | import argparse 7 | from sim_env import make_sim_env 8 | from constants import SIM_DT, SIM_TASK_CONFIGS 9 | import time 10 | import numpy as np 11 | import glob 12 | 13 | def main(args): 14 | dataset_dir = SIM_TASK_CONFIGS[args["task_name"]]["dataset_dir"] 15 | episode_idx = args["episode_idx"] 16 | 17 | dataset_path = os.path.join(dataset_dir, f'episode_{episode_idx}.hdf5') 18 | with h5py.File(dataset_path, 'r') as root: 19 | all_qpos = root['/observations/all_qpos'][()] 20 | actions = root['/action'][()] 21 | qpos = root['/observations/qpos'][()] 22 | 23 | env = make_sim_env(args["task_name"], cameras=[]) 24 | 25 | for i in range(10): 26 | ts, info = env.reset() 27 | 28 | env.set_qpos(all_qpos[0]) 29 | 30 | for action in actions: 31 | 32 | step_start = time.time() 33 | 34 | # Apply the action 35 | env.step_joints(action) 36 | env.render_viewer() 37 | 38 | print(env.get_reward()) 39 | 40 | # Rudimentary time keeping, will drift relative to wall clock. 41 | time_until_next_step = SIM_DT - (time.time() - step_start) 42 | time.sleep(max(0, time_until_next_step)) 43 | 44 | if __name__ == '__main__': 45 | parser = argparse.ArgumentParser() 46 | parser.add_argument('--task_name', action='store', type=str, help='Task name.', required=True) 47 | parser.add_argument('--episode_idx', action='store', type=int, help='Episode index.', required=False) 48 | 49 | main(vars(parser.parse_args())) 50 | 51 | 52 | -------------------------------------------------------------------------------- /data_collection_scripts/test_scripts/test_zed.py: -------------------------------------------------------------------------------- 1 | import pyzed.sl as sl 2 | 3 | # Create a ZED camera object 4 | zed = sl.Camera() 5 | 6 | # Set configuration parameters 7 | init_params = sl.InitParameters() 8 | init_params.camera_resolution = sl.RESOLUTION.HD1080 9 | init_params.camera_fps = 30 10 | 11 | # Open the camera 12 | err = zed.open(init_params) 13 | if err != sl.ERROR_CODE.SUCCESS: 14 | exit(-1) -------------------------------------------------------------------------------- /data_collection_scripts/test_scripts/webrtc_receiver.py: -------------------------------------------------------------------------------- 1 | from google.cloud import firestore 2 | import json 3 | import asyncio 4 | import cv2 5 | import numpy as np 6 | from aiortc import ( 7 | RTCIceCandidate, 8 | RTCPeerConnection, 9 | RTCSessionDescription, 10 | VideoStreamTrack, 11 | RTCConfiguration, 12 | RTCIceServer, 13 | ) 14 | from aiortc.contrib.media import MediaBlackhole, MediaPlayer, MediaRecorder 15 | from aiortc.contrib.signaling import BYE, add_signaling_arguments, create_signaling 16 | from aiortc.sdp import candidate_from_sdp 17 | 18 | class VideoDisplayTrack(VideoStreamTrack): 19 | def __init__(self, track, name="video_display_track"): 20 | super().__init__() 21 | self.track = track 22 | self.name = name 23 | self.image = None 24 | 25 | async def recv(self): 26 | frame = await self.track.recv() 27 | img = frame.to_ndarray(format="rgb24") # Convert frame to OpenCV format 28 | 29 | self.image = img 30 | 31 | return frame 32 | 33 | def get_image(self): 34 | return self.image 35 | 36 | left_video_track = None 37 | right_video_track = None 38 | counter =0 39 | 40 | async def run_answer(pc, db, signalingSettings): 41 | global left_video_track, right_video_track, counter 42 | 43 | @pc.on("datachannel") 44 | def on_datachannel(channel): 45 | print('Data channel opened') 46 | # @channel.on("message") 47 | # def on_message(message): 48 | # print('Message received:', message) 49 | 50 | @pc.on("track") 51 | def on_track(track): 52 | global left_video_track, right_video_track, counter 53 | print("Receiving %s" % track.kind) 54 | if track.kind == "video": 55 | if counter == 0: 56 | print('Left video track') 57 | left_video_track = VideoDisplayTrack(track, name="left_video_track") 58 | pc.addTrack(left_video_track) 59 | counter += 1 60 | else: 61 | print('Right video track') 62 | right_video_track = VideoDisplayTrack(track, name="right_video_track") 63 | pc.addTrack(right_video_track) 64 | 65 | call_doc = db.collection(signalingSettings['password']).document(signalingSettings['robotID']) 66 | 67 | data = call_doc.get().to_dict() 68 | 69 | await pc.setRemoteDescription(RTCSessionDescription( 70 | sdp=data['sdp'], 71 | type=data['type'] 72 | )) 73 | 74 | await pc.setLocalDescription(await pc.createAnswer()) 75 | 76 | call_doc.set( 77 | { 78 | 'sdp': pc.localDescription.sdp, 79 | 'type': pc.localDescription.type 80 | } 81 | ) 82 | 83 | if __name__ == "__main__": 84 | # Read firebase-creds.json 85 | with open('../serviceAccountKey.json') as f: 86 | serviceAccountKey = json.load(f) 87 | 88 | with open('../signalingSettings.json') as f: 89 | signalingSettings = json.load(f) 90 | 91 | db = firestore.Client.from_service_account_info(serviceAccountKey) 92 | 93 | pc = RTCPeerConnection( 94 | configuration=RTCConfiguration([ 95 | RTCIceServer("stun:stun1.l.google.com:19302"), 96 | RTCIceServer("stun:stun2.l.google.com:19302"), 97 | ]) 98 | ) 99 | 100 | 101 | import threading 102 | 103 | def run(loop: asyncio.AbstractEventLoop): 104 | asyncio.set_event_loop(loop) 105 | loop.run_until_complete(run_answer(pc, db, signalingSettings)) 106 | loop.run_forever() 107 | 108 | event_loop = asyncio.new_event_loop() 109 | thread = threading.Thread(target=run, args=(event_loop,)) 110 | thread.start() 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | try: 119 | while True: 120 | if left_video_track is not None and right_video_track is not None: 121 | left_frame = left_video_track.get_image() 122 | right_frame = right_video_track.get_image() 123 | 124 | 125 | 126 | if left_frame is None or right_frame is None: 127 | continue 128 | 129 | 130 | concat_image = np.concatenate((left_frame, right_frame), axis=1) 131 | cv2.imshow('Live Stream', concat_image) 132 | if cv2.waitKey(1) & 0xFF == ord('q'): 133 | 134 | break 135 | 136 | except KeyboardInterrupt: 137 | pass 138 | finally: 139 | 140 | # kill the thread 141 | 142 | import os 143 | os._exit(42) 144 | -------------------------------------------------------------------------------- /data_collection_scripts/test_scripts/webrtc_sender.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('..') 3 | 4 | from webrtc_headset import WebRTCHeadset 5 | from headset_utils import HeadsetFeedback 6 | import os 7 | import cv2 8 | import numpy as np 9 | 10 | def main(): 11 | headset = WebRTCHeadset(serviceAccountKeyFile='../serviceAccountKey.json', signalingSettingsFile='../signalingSettings.json') 12 | headset.run_in_thread() 13 | 14 | 15 | # Open a connection to the webcam (usually 0 is the default camera) 16 | cap = cv2.VideoCapture(0) 17 | 18 | # set the resolution to `720p 19 | cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) 20 | cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) 21 | 22 | # set to 1080p 23 | # cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920) 24 | # cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080) 25 | 26 | if not cap.isOpened(): 27 | print("Error: Could not open webcam.") 28 | return 29 | 30 | while True: 31 | # Capture frame-by-frame 32 | ret, frame = cap.read() 33 | 34 | 35 | if not ret: 36 | print("Error: Failed to capture image.") 37 | break 38 | 39 | left_frame = frame.copy() 40 | right_frame = frame.copy() 41 | 42 | # draw "left" in the left frame (put in center of picture) 43 | cv2.putText(left_frame, "Left", (int(left_frame.shape[1]/2)-50, int(left_frame.shape[0]/2)), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) 44 | cv2.putText(right_frame, "Right", (int(right_frame.shape[1]/2)-50, int(right_frame.shape[0]/2)), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) 45 | 46 | concat_image = np.concatenate((left_frame, right_frame), axis=1) 47 | 48 | cv2.imshow('Robot Side', concat_image) 49 | 50 | # convert to greyscale bgr 51 | # left_frame = cv2.cvtColor(left_frame, cv2.COLOR_BGR2GRAY) 52 | left_frame = cv2.cvtColor(left_frame, cv2.COLOR_RGB2BGR) 53 | # right_frame = cv2.cvtColor(right_frame, cv2.COLOR_BGR2GRAY) 54 | right_frame = cv2.cvtColor(right_frame, cv2.COLOR_RGB2BGR) 55 | 56 | headset.send_images(left_frame, right_frame) 57 | 58 | # Break the loop on 'q' key press 59 | if cv2.waitKey(1) & 0xFF == ord('q'): 60 | break 61 | 62 | # When everything is done, release the capture 63 | cap.release() 64 | cv2.destroyAllWindows() 65 | 66 | 67 | 68 | 69 | if __name__ == '__main__': 70 | try: 71 | main() 72 | except KeyboardInterrupt: 73 | print("Shutting down...") 74 | os._exit(42) 75 | 76 | -------------------------------------------------------------------------------- /data_collection_scripts/visualize_all_episodes.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | import cv2 4 | import h5py 5 | import argparse 6 | import glob 7 | import matplotlib.pyplot as plt 8 | from constants import REAL_DT, SIM_DT, LEFT_JOINT_NAMES, RIGHT_JOINT_NAMES, MIDDLE_JOINT_NAMES 9 | import re 10 | 11 | def load_hdf5(dataset_path): 12 | if not os.path.isfile(dataset_path): 13 | print(f'Dataset does not exist at \n{dataset_path}\n') 14 | exit() 15 | 16 | with h5py.File(dataset_path, 'r') as root: 17 | is_sim = root.attrs['sim'] 18 | qpos = root['/observations/qpos'][()] 19 | qvel = root['/observations/qvel'][()] 20 | action = root['/action'][()] 21 | image_dict = dict() 22 | for cam_name in root[f'/observations/images/'].keys(): 23 | image_dict[cam_name] = root[f'/observations/images/{cam_name}'][()] 24 | 25 | return qpos, qvel, action, image_dict 26 | 27 | def main(args): 28 | 29 | glob_path = args['glob_path'] 30 | 31 | episode_files = glob.glob(glob_path) 32 | 33 | if len(episode_files) == 0: 34 | print(f'No episodes found in {glob_path}') 35 | exit() 36 | 37 | # make sure files all in same directory 38 | dataset_dir = os.path.dirname(episode_files[0]) 39 | for episode_file in episode_files: 40 | if os.path.dirname(episode_file) != dataset_dir: 41 | print('All episodes must be in the same directory') 42 | exit() 43 | 44 | # make sure all files are .hdf5 45 | for episode_file in episode_files: 46 | if not episode_file.endswith('.hdf5'): 47 | print('All episodes must be .hdf5 files') 48 | exit() 49 | 50 | # sort episode files by episode number 51 | episode_files = sorted(episode_files, key=lambda x: int(re.search(r'\d+', x).group())) 52 | 53 | # make sure all episodes are present (continuously numbered) 54 | for i, episode_file in enumerate(episode_files): 55 | # check for string i in episode_file 56 | if str(i) not in episode_file: 57 | print(f'Missing episode_{i} in {dataset_dir}') 58 | exit() 59 | 60 | # if dataset_dir starts with sim_, use SIM_DT, else use REAL_DT 61 | DT = SIM_DT if dataset_dir.startswith('sim_') else REAL_DT 62 | 63 | out = None 64 | 65 | video_path = os.path.join(dataset_dir, 'all_episodes.mp4') 66 | 67 | for i in range(len(episode_files)): 68 | qpos, qvel, action, image_dict = load_hdf5(episode_files[i]) 69 | 70 | video = image_dict 71 | cam_names = list(video.keys()) 72 | all_cam_videos = [] 73 | 74 | min_h = np.inf 75 | total_w = 0 76 | for cam_name in cam_names: 77 | _, h, w, _ = video[cam_name].shape 78 | min_h = min(min_h, h) 79 | total_w += w 80 | 81 | for cam_name in cam_names: 82 | _, h, w, _ = video[cam_name].shape 83 | 84 | new_w = int(min_h * w / h) 85 | 86 | cam_video = video[cam_name] 87 | buffer = np.zeros((len(cam_video), min_h, new_w, 3), dtype=np.uint8) 88 | for ts in range(len(cam_video)): 89 | buffer[ts, :, :, :] = cv2.resize(cam_video[ts], (new_w, min_h)) 90 | all_cam_videos.append(buffer) 91 | all_cam_videos = np.concatenate(all_cam_videos, axis=2) # width dimension 92 | 93 | n_frames, h, w, _ = all_cam_videos.shape 94 | 95 | if out is None: 96 | fps = int(1 / DT / 10) 97 | out = cv2.VideoWriter(video_path, cv2.VideoWriter_fourcc(*'mp4v'), fps, (w, h)) 98 | 99 | for t in range(0, n_frames, 20): 100 | image = all_cam_videos[t] 101 | image = image[:, :, [2, 1, 0]] # swap B and R channel 102 | 103 | # write episode number on image 104 | font = cv2.FONT_HERSHEY_SIMPLEX 105 | bottomLeftCornerOfText = (10, 30) 106 | fontScale = 1 107 | fontColor = (255, 255, 255) 108 | lineType = 2 109 | image = cv2.putText(image.copy(), f'Episode {i}', bottomLeftCornerOfText, font, 110 | fontScale, fontColor, 2, cv2.LINE_AA) 111 | 112 | out.write(image) 113 | 114 | print(f'Processed episode_{i}') 115 | 116 | if out is not None: 117 | out.release() 118 | print(f'Saved video to: {video_path}') 119 | else: 120 | print('No episodes found to visualize') 121 | 122 | 123 | 124 | if __name__ == '__main__': 125 | parser = argparse.ArgumentParser() 126 | parser.add_argument('--glob_path', type=str, help='Dataset dir.', required=True) 127 | main(vars(parser.parse_args())) -------------------------------------------------------------------------------- /eval_scripts/.gitignore: -------------------------------------------------------------------------------- 1 | outputs -------------------------------------------------------------------------------- /eval_scripts/README.md: -------------------------------------------------------------------------------- 1 | ```bash 2 | # in one terminal 3 | source launch_robot.sh 4 | 5 | # another terminal 6 | source activate.sh 7 | 8 | python sleep.py 9 | 10 | python real_env.py 11 | 12 | python eval.py 13 | ``` -------------------------------------------------------------------------------- /eval_scripts/activate.sh: -------------------------------------------------------------------------------- 1 | conda deactivate 2 | conda deactivate 3 | conda deactivate 4 | source /opt/ros/noetic/setup.sh && source ~/interbotix_ws/devel/setup.sh 5 | conda activate lerobot -------------------------------------------------------------------------------- /eval_scripts/assets/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2023, Trossen Robotics 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | 3. Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /eval_scripts/assets/joint_position_actuators.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 40 | 41 | -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/angled_extrusion.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/angled_extrusion.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/corner_bracket.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/corner_bracket.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/d405_solid.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/d405_solid.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/extrusion_1000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/extrusion_1000.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/extrusion_1220.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/extrusion_1220.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/extrusion_150.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/extrusion_150.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/extrusion_2040_1000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/extrusion_2040_1000.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/extrusion_2040_880.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/extrusion_2040_880.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/extrusion_600.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/extrusion_600.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/interbotix_black.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/interbotix_black.png -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/overhead_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/overhead_mount.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/small_meta_table_diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/small_meta_table_diffuse.png -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/tabletop.obj: -------------------------------------------------------------------------------- 1 | mtllib material_0.mtl 2 | usemtl material_0 3 | v -0.37953900 -0.60982800 0.73674500 4 | v -0.37953900 -0.60982800 0.74887200 5 | v -0.38100000 -0.60839100 0.73674500 6 | v -0.38100000 -0.60839100 0.74887200 7 | v -0.18977000 -0.60982800 0.74887200 8 | v -0.18977000 -0.60839100 0.75078500 9 | v -0.37953900 -0.60839100 0.75078700 10 | v -0.38100000 0.60793400 0.74887200 11 | v -0.37953900 0.60793400 0.75078700 12 | v 0.38100000 -0.60839100 0.73674500 13 | v 0.38100000 -0.60839100 0.74887200 14 | v 0.37953900 -0.60982800 0.73674500 15 | v 0.37953900 -0.60982800 0.74887200 16 | v 0.37953900 0.60793400 0.75078700 17 | v 0.37953900 -0.60839100 0.75078700 18 | v 0.38100000 0.60793400 0.74887200 19 | v -0.38100000 0.60793400 0.73674500 20 | v -0.37953900 0.60937200 0.73674500 21 | v -0.37953900 0.60937200 0.74887200 22 | v -0.18977000 0.60793400 0.75078700 23 | v -0.18977000 0.60937200 0.74887200 24 | v 0.37953900 0.60937200 0.73674500 25 | v 0.37953900 0.60937200 0.74887200 26 | v 0.38100000 0.60793400 0.73674500 27 | v -0.37953900 -0.60839100 0.73483100 28 | v -0.37953900 0.60793400 0.73483100 29 | v -0.18977000 0.60937200 0.73674500 30 | v -0.18977000 0.60793400 0.73483100 31 | v 0.37953900 -0.60839100 0.73483100 32 | v 0.37953900 0.60793400 0.73483100 33 | v -0.18977000 -0.60839100 0.73483100 34 | v -0.18977000 -0.60982800 0.73674500 35 | v 0.18977000 0.60793400 0.75078700 36 | v 0.18977000 -0.60839100 0.75078700 37 | v 0.18977000 0.60937200 0.74887200 38 | v 0.18977000 0.60937200 0.73674500 39 | v 0.18977000 0.60793400 0.73483100 40 | v 0.18977000 -0.60839100 0.73483100 41 | v 0.18977000 -0.60982800 0.73674500 42 | v 0.18977000 -0.60982800 0.74887200 43 | v 0.00000000 0.60793400 0.75078700 44 | v 0.00000000 -0.60839100 0.75078500 45 | v 0.00000000 0.60937200 0.74887200 46 | v 0.00000000 0.60937200 0.73674500 47 | v 0.00000000 0.60793400 0.73483100 48 | v 0.00000000 -0.60839100 0.73483100 49 | v 0.00000000 -0.60982800 0.73674500 50 | v 0.00000000 -0.60982800 0.74887200 51 | vt 0.75019800 0.57261000 52 | vt 0.74519900 0.57261000 53 | vt 0.74382100 0.56603500 54 | vt 0.74382100 0.57103100 55 | vt 0.74519900 0.67519800 56 | vt 0.74362100 0.67519800 57 | vt 0.74362100 0.57261000 58 | vt 0.12297800 0.57103200 59 | vt 0.12297900 0.57261000 60 | vt 0.74362100 0.15331600 61 | vt 0.74382100 0.98454000 62 | vt 0.75019800 0.98296000 63 | vt 0.74519900 0.98296000 64 | vt 0.12381100 0.98296000 65 | vt 0.74362100 0.98296000 66 | vt 0.12381700 0.98453800 67 | vt 0.11445600 0.57182100 68 | vt 0.11448400 0.57261000 69 | vt 0.12160500 0.57261000 70 | vt 0.12318700 0.67519800 71 | vt 0.12181300 0.67519800 72 | vt 0.11531600 0.98296000 73 | vt 0.12243700 0.98296000 74 | vt 0.11524200 0.98375000 75 | vt 0.74362100 0.56445500 76 | vt 0.12296000 0.56445500 77 | vt 0.11469200 0.67519800 78 | vt 0.11331800 0.67519800 79 | vt 0.74362100 0.15410500 80 | vt 0.12256600 0.15410500 81 | vt 0.74362100 0.46186700 82 | vt 0.74441000 0.46186700 83 | vt 0.12360300 0.88037300 84 | vt 0.74362100 0.88037300 85 | vt 0.12222900 0.88037300 86 | vt 0.11510800 0.88037300 87 | vt 0.11373500 0.88037300 88 | vt 0.74362100 0.25669200 89 | vt 0.74441000 0.25669200 90 | vt 0.74519900 0.88037300 91 | vt 0.12339500 0.77778500 92 | vt 0.74362100 0.77778500 93 | vt 0.12202100 0.77778500 94 | vt 0.11490000 0.77778500 95 | vt 0.11352700 0.77778500 96 | vt 0.74362100 0.35928000 97 | vt 0.74441000 0.35928000 98 | vt 0.74519900 0.77778500 99 | f 1/1 2/2 3/3 100 | f 3/3 2/2 4/4 101 | f 5/5 6/6 2/2 102 | f 2/2 6/6 7/7 103 | f 8/8 4/4 9/9 104 | f 9/9 4/4 7/7 105 | f 10/10 11/11 12/12 106 | f 12/12 11/11 13/13 107 | f 14/14 15/15 16/16 108 | f 16/16 15/15 11/11 109 | f 17/17 8/8 18/18 110 | f 18/18 8/8 19/19 111 | f 20/20 21/21 9/9 112 | f 9/9 21/21 19/19 113 | f 22/22 23/23 24/24 114 | f 24/24 23/23 16/16 115 | f 3/3 17/17 25/25 116 | f 25/25 17/17 26/26 117 | f 27/27 28/28 18/18 118 | f 18/18 28/28 26/26 119 | f 29/29 30/30 10/10 120 | f 10/10 30/30 24/24 121 | f 31/31 32/32 25/25 122 | f 25/25 32/32 1/1 123 | f 9/9 7/7 20/20 124 | f 20/20 7/7 6/6 125 | f 18/18 19/19 27/27 126 | f 27/27 19/19 21/21 127 | f 25/25 26/26 31/31 128 | f 31/31 26/26 28/28 129 | f 2/2 1/1 5/5 130 | f 5/5 1/1 32/32 131 | f 16/16 11/11 24/24 132 | f 24/24 11/11 10/10 133 | f 17/17 3/3 8/8 134 | f 8/8 3/3 4/4 135 | f 2/2 7/7 4/4 136 | f 11/11 15/15 13/13 137 | f 8/8 9/9 19/19 138 | f 23/23 14/14 16/16 139 | f 17/17 18/18 26/26 140 | f 30/30 22/22 24/24 141 | f 3/3 25/25 1/1 142 | f 12/12 29/29 10/10 143 | f 33/33 34/34 14/14 144 | f 14/14 34/34 15/15 145 | f 23/23 35/35 14/14 146 | f 14/14 35/35 33/33 147 | f 36/36 35/35 22/22 148 | f 22/22 35/35 23/23 149 | f 30/30 37/37 22/22 150 | f 22/22 37/37 36/36 151 | f 38/38 37/37 29/29 152 | f 29/29 37/37 30/30 153 | f 12/12 39/39 29/29 154 | f 29/29 39/39 38/38 155 | f 40/40 39/39 13/13 156 | f 13/13 39/39 12/12 157 | f 15/15 34/34 13/13 158 | f 13/13 34/34 40/40 159 | f 41/41 42/42 33/33 160 | f 33/33 42/42 34/34 161 | f 33/33 35/35 41/41 162 | f 41/41 35/35 43/43 163 | f 44/44 43/43 36/36 164 | f 36/36 43/43 35/35 165 | f 36/36 37/37 44/44 166 | f 44/44 37/37 45/45 167 | f 46/46 45/45 38/38 168 | f 38/38 45/45 37/37 169 | f 38/38 39/39 46/46 170 | f 46/46 39/39 47/47 171 | f 48/48 47/47 40/40 172 | f 40/40 47/47 39/39 173 | f 40/40 34/34 48/48 174 | f 48/48 34/34 42/42 175 | f 20/20 6/6 41/41 176 | f 41/41 6/6 42/42 177 | f 41/41 43/43 20/20 178 | f 20/20 43/43 21/21 179 | f 27/27 21/21 44/44 180 | f 44/44 21/21 43/43 181 | f 44/44 45/45 27/27 182 | f 27/27 45/45 28/28 183 | f 31/31 28/28 46/46 184 | f 46/46 28/28 45/45 185 | f 46/46 47/47 31/31 186 | f 31/31 47/47 32/32 187 | f 5/5 32/32 48/48 188 | f 48/48 32/32 47/47 189 | f 48/48 42/42 5/5 190 | f 5/5 42/42 6/6 191 | -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_1_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_1_base.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_2_shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_2_shoulder.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_3_upper_arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_3_upper_arm.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_4_upper_forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_4_upper_forearm.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_5_lower_forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_5_lower_forearm.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_6_wrist.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_6_wrist.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_7_gripper.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_7_gripper.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_7_gripper_bar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_7_gripper_bar.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_7_gripper_camera.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_7_gripper_camera.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_7_gripper_prop.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_7_gripper_prop.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_7_gripper_prop_bar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_7_gripper_prop_bar.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_7_gripper_wrist_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_7_gripper_wrist_mount.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_7dof_wrist_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_7dof_wrist_1.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_7dof_wrist_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_7dof_wrist_2.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_7dof_wrist_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_7dof_wrist_3.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_8_custom_finger_left.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_8_custom_finger_left.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/vx300s_8_custom_finger_right.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/vx300s_8_custom_finger_right.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/wormseye_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/wormseye_mount.stl -------------------------------------------------------------------------------- /eval_scripts/assets/meshes/zedm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/eval_scripts/assets/meshes/zedm.stl -------------------------------------------------------------------------------- /eval_scripts/assets/scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /eval_scripts/assets/task_hook_package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /eval_scripts/assets/task_insert_peg.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /eval_scripts/assets/task_sew_needle.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /eval_scripts/assets/task_slot_insertion.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /eval_scripts/assets/task_tube_transfer.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 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 | -------------------------------------------------------------------------------- /eval_scripts/launch_robot.sh: -------------------------------------------------------------------------------- 1 | conda deactivate 2 | conda deactivate 3 | conda deactivate 4 | source /opt/ros/noetic/setup.sh && source ~/interbotix_ws/devel/setup.sh 5 | roslaunch guided_vision 3arms_teleop.launch -------------------------------------------------------------------------------- /eval_scripts/robot_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import time 3 | from constants import REAL_DT 4 | import time 5 | 6 | DT = REAL_DT 7 | 8 | import IPython 9 | e = IPython.embed 10 | 11 | from interbotix_xs_msgs.msg import JointSingleCommand 12 | 13 | def get_arm_joint_positions(bot): 14 | if hasattr(bot, 'gripper') is False: 15 | return bot.arm.core.joint_states.position[:7] 16 | return bot.arm.core.joint_states.position[:6] 17 | 18 | def get_arm_gripper_positions(bot): 19 | # check if bot has attr gripper 20 | if hasattr(bot, 'gripper') is False: 21 | raise NotImplementedError("This bot does not have a gripper") 22 | joint_position = bot.gripper.core.joint_states.position[6] 23 | return joint_position 24 | 25 | def move_arms(bot_list, target_pose_list, move_time=1): 26 | num_steps = int(move_time / DT) 27 | curr_pose_list = [get_arm_joint_positions(bot) for bot in bot_list] 28 | traj_list = [np.linspace(curr_pose, target_pose, num_steps) for curr_pose, target_pose in zip(curr_pose_list, target_pose_list)] 29 | for t in range(num_steps): 30 | for bot_id, bot in enumerate(bot_list): 31 | bot.arm.set_joint_positions(traj_list[bot_id][t], blocking=False) 32 | time.sleep(DT) 33 | 34 | def move_grippers(bot_list, target_pose_list, move_time): 35 | gripper_command = JointSingleCommand(name="gripper") 36 | num_steps = int(move_time / DT) 37 | curr_pose_list = [get_arm_gripper_positions(bot) for bot in bot_list] 38 | traj_list = [np.linspace(curr_pose, target_pose, num_steps) for curr_pose, target_pose in zip(curr_pose_list, target_pose_list)] 39 | for t in range(num_steps): 40 | for bot_id, bot in enumerate(bot_list): 41 | gripper_command.cmd = traj_list[bot_id][t] 42 | bot.gripper.core.pub_single.publish(gripper_command) 43 | time.sleep(DT) 44 | 45 | def setup_puppet_bot(bot): 46 | if hasattr(bot, 'gripper') is False: 47 | bot.dxl.robot_set_operating_modes("group", "arm", "position") 48 | else: 49 | bot.dxl.robot_reboot_motors("single", "gripper", True) 50 | bot.dxl.robot_set_operating_modes("group", "arm", "position") 51 | bot.dxl.robot_set_operating_modes("single", "gripper", "current_based_position") 52 | torque_on(bot) 53 | 54 | def setup_master_bot(bot): 55 | bot.dxl.robot_set_operating_modes("group", "arm", "pwm") 56 | bot.dxl.robot_set_operating_modes("single", "gripper", "current_based_position") 57 | torque_off(bot) 58 | 59 | def set_standard_pid_gains(bot): 60 | bot.dxl.robot_set_motor_registers("group", "arm", 'Position_P_Gain', 800) 61 | bot.dxl.robot_set_motor_registers("group", "arm", 'Position_I_Gain', 0) 62 | 63 | def set_low_pid_gains(bot): 64 | bot.dxl.robot_set_motor_registers("group", "arm", 'Position_P_Gain', 100) 65 | bot.dxl.robot_set_motor_registers("group", "arm", 'Position_I_Gain', 0) 66 | 67 | def torque_off(bot): 68 | bot.dxl.robot_torque_enable("group", "arm", False) 69 | if hasattr(bot, 'gripper') is True: 70 | bot.dxl.robot_torque_enable("single", "gripper", False) 71 | 72 | 73 | def torque_on(bot): 74 | bot.dxl.robot_torque_enable("group", "arm", True) 75 | if hasattr(bot, 'gripper') is True: 76 | bot.dxl.robot_torque_enable("single", "gripper", True) 77 | 78 | def sleep(puppet_bot_left, puppet_bot_right, puppet_bot_middle): 79 | all_bots = [puppet_bot_left, puppet_bot_right, puppet_bot_middle] 80 | for bot in all_bots: 81 | torque_on(bot) 82 | 83 | left_puppet_sleep_position = (0, -1.7, 1.55, 0, 0.65, 0) 84 | right_puppet_sleep_position = (0, -1.7, 1.55, 0, 0.65, 0) 85 | middle_puppet_sleep_position = (0, -1.7, 1.55, 0, 0.65, 0, 0) 86 | move_arms(all_bots, [left_puppet_sleep_position, right_puppet_sleep_position, middle_puppet_sleep_position], move_time=3) 87 | 88 | left_puppet_sleep_position = (0, -1.85, 1.6, 0, 0.65, 0) 89 | right_puppet_sleep_position = (0, -1.85, 1.6, 0, 0.65, 0) 90 | middle_puppet_sleep_position = (0, -1.85, 1.6, 0, 0.65, 0, 0) 91 | move_arms(all_bots, [left_puppet_sleep_position, right_puppet_sleep_position, middle_puppet_sleep_position], move_time=3) 92 | -------------------------------------------------------------------------------- /eval_scripts/save_policy.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import sys 3 | from pathlib import Path 4 | from lerobot.common.policies.act.modeling_act import ACTPolicy 5 | from huggingface_hub import upload_file 6 | 7 | def main(repo_id, checkpoint_dir): 8 | # Download the diffusion policy for pusht environment 9 | pretrained_policy_path = Path(checkpoint_dir) 10 | 11 | # Load the policy from the pretrained checkpoint 12 | policy = ACTPolicy.from_pretrained(pretrained_policy_path) 13 | 14 | # Push policy to the specified Hugging Face Hub repository 15 | policy.push_to_hub(repo_id) 16 | 17 | # Upload the YAML configuration file separately 18 | yaml_config_path = pretrained_policy_path / "config.yaml" 19 | upload_file( 20 | path_or_fileobj=yaml_config_path, 21 | path_in_repo="config.yaml", 22 | repo_id=repo_id, 23 | commit_message="Upload YAML configuration" 24 | ) 25 | 26 | if __name__ == "__main__": 27 | # Create argument parser 28 | parser = argparse.ArgumentParser( 29 | description="Push a pretrained ACT policy and YAML config to the Hugging Face Hub.", 30 | epilog="Example usage:\n" 31 | " python save_policy.py --repo_id iantc104/sim_slot_insertion_3arms_zed_wrist_act " 32 | "--checkpoint_dir outputs/train/sim_slot_insertion_3arms_zed_wrist_act/checkpoints/014000/pretrained_model", 33 | formatter_class=argparse.RawTextHelpFormatter 34 | ) 35 | 36 | # Add arguments 37 | parser.add_argument('--repo_id', type=str, required=True, help="Repository ID on Hugging Face Hub.") 38 | parser.add_argument('--checkpoint_dir', type=str, required=True, help="Directory containing the pretrained model checkpoints.") 39 | 40 | # If no arguments are provided, show example command 41 | if len(sys.argv) == 1: 42 | parser.print_help() 43 | print("\nExample usage:") 44 | print(" python save_policy.py --repo_id iantc104/sim_slot_insertion_3arms_zed_wrist_act " 45 | "--checkpoint_dir outputs/train/sim_slot_insertion_3arms_zed_wrist_act/checkpoints/014000/pretrained_model") 46 | sys.exit(1) 47 | 48 | # Parse arguments 49 | args = parser.parse_args() 50 | 51 | # Call the main function with parsed arguments 52 | main(args.repo_id, args.checkpoint_dir) 53 | -------------------------------------------------------------------------------- /eval_scripts/sleep.py: -------------------------------------------------------------------------------- 1 | from interbotix_xs_modules.arm import InterbotixManipulatorXS 2 | from robot_utils import sleep 3 | 4 | def main(): 5 | puppet_bot_left = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper", robot_name=f'puppet_left', init_node=True) 6 | puppet_bot_right = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper", robot_name=f'puppet_right', init_node=False) 7 | puppet_bot_middle = InterbotixManipulatorXS(robot_model="vx300s_7dof", group_name="arm", gripper_name=None, robot_name=f"puppet_middle", init_node=False) 8 | 9 | sleep(puppet_bot_left, puppet_bot_right, puppet_bot_middle) 10 | 11 | if __name__ == "__main__": 12 | import rospy 13 | import os 14 | 15 | def shutdown(): 16 | print("Shutting down...") 17 | os._exit(42) 18 | rospy.on_shutdown(shutdown) 19 | 20 | try: 21 | main() 22 | except KeyboardInterrupt: 23 | print("Shutting down...") 24 | os._exit(42) 25 | -------------------------------------------------------------------------------- /guided_vision_ros/guided_vision/config/master_modes_left.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/ttyDXL_master_left 2 | 3 | groups: 4 | arm: 5 | torque_enable: false 6 | 7 | singles: 8 | gripper: 9 | torque_enable: false 10 | -------------------------------------------------------------------------------- /guided_vision_ros/guided_vision/config/master_modes_right.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/ttyDXL_master_right 2 | 3 | groups: 4 | arm: 5 | torque_enable: false 6 | 7 | singles: 8 | gripper: 9 | torque_enable: false 10 | -------------------------------------------------------------------------------- /guided_vision_ros/guided_vision/config/puppet_modes_left.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/ttyDXL_puppet_left 2 | 3 | groups: 4 | arm: 5 | operating_mode: position 6 | profile_type: velocity 7 | profile_velocity: 0 8 | profile_acceleration: 0 9 | torque_enable: true 10 | 11 | singles: 12 | gripper: 13 | operating_mode: linear_position 14 | profile_type: velocity 15 | profile_velocity: 0 16 | profile_acceleration: 0 17 | torque_enable: true 18 | -------------------------------------------------------------------------------- /guided_vision_ros/guided_vision/config/puppet_modes_middle.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/ttyDXL_puppet_middle 2 | 3 | groups: 4 | arm: 5 | torque_enable: false 6 | -------------------------------------------------------------------------------- /guided_vision_ros/guided_vision/config/puppet_modes_right.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/ttyDXL_puppet_right 2 | 3 | groups: 4 | arm: 5 | operating_mode: position 6 | profile_type: velocity 7 | profile_velocity: 0 8 | profile_acceleration: 0 9 | torque_enable: true 10 | 11 | singles: 12 | gripper: 13 | operating_mode: linear_position 14 | profile_type: velocity 15 | profile_velocity: 0 16 | profile_acceleration: 0 17 | torque_enable: true 18 | -------------------------------------------------------------------------------- /guided_vision_ros/guided_vision/launch/3arms_teleop.launch: -------------------------------------------------------------------------------- 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 | 56 | 57 | 62 | 63 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /guided_vision_ros/guided_vision/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | guided_vision 4 | 0.0.0 5 | The guided_vision package 6 | 7 | 8 | 9 | 10 | ian 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/3D_printed_parts/wrist_yaw_motor_holder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/guided_vision_ros/vx300s_7dof_description/3D_printed_parts/wrist_yaw_motor_holder.stl -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/3D_printed_parts/zed_camera_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/guided_vision_ros/vx300s_7dof_description/3D_printed_parts/zed_camera_mount.stl -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vx300s_7dof_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | joint_state_publisher 9 | joint_state_publisher_gui 10 | robot_state_publisher 11 | roslaunch 12 | rviz 13 | xacro 14 | ) 15 | 16 | ################################### 17 | ## catkin specific configuration ## 18 | ################################### 19 | ## The catkin_package macro generates cmake config files for your package 20 | ## Declare things to be passed to dependent projects 21 | catkin_package( 22 | CATKIN_DEPENDS joint_state_publisher joint_state_publisher_gui robot_state_publisher roslaunch rviz xacro 23 | ) 24 | 25 | ########### 26 | ## Build ## 27 | ########### 28 | 29 | ## Specify additional locations of header files 30 | ## Your package locations should be listed before other locations 31 | include_directories( 32 | ${catkin_INCLUDE_DIRS} 33 | ) 34 | 35 | ############# 36 | ## Install ## 37 | ############# 38 | 39 | ## Mark other files for installation (e.g. launch and bag files, etc.) 40 | install( 41 | DIRECTORY 42 | launch 43 | meshes 44 | rviz 45 | urdf 46 | DESTINATION 47 | ${CATKIN_PACKAGE_SHARE_DESTINATION} 48 | ) 49 | -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/config/modes.yaml: -------------------------------------------------------------------------------- 1 | groups: 2 | arm: 3 | operating_mode: position 4 | profile_type: time 5 | profile_velocity: 200 6 | profile_acceleration: 100 7 | torque_enable: true 8 | 9 | # singles: 10 | # gripper: 11 | # operating_mode: pwm 12 | # profile_type: velocity 13 | # profile_velocity: 0 14 | # profile_acceleration: 0 15 | # torque_enable: true 16 | -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/config/vx300s_7dof.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/ttyDXL_puppet_middle 2 | 3 | joint_order: [waist, shoulder, elbow, forearm_roll, wrist_1_joint, wrist_2_joint, wrist_3_joint] 4 | sleep_positions: [0, -1.85, 1.55, 0, 0.3, 0, 0] 5 | 6 | joint_state_publisher: 7 | update_rate: 100 8 | publish_states: true 9 | topic_name: joint_states 10 | 11 | groups: 12 | arm: [waist, shoulder, elbow, forearm_roll, wrist_1_joint, wrist_2_joint, wrist_3_joint] 13 | 14 | # grippers: 15 | # gripper: 16 | # horn_radius: 0.022 17 | # arm_length: 0.036 18 | # left_finger: left_finger 19 | # right_finger: right_finger 20 | 21 | shadows: 22 | shoulder: 23 | shadow_list: [shoulder_shadow] 24 | calibrate: true 25 | elbow: 26 | shadow_list: [elbow_shadow] 27 | calibrate: true 28 | 29 | sisters: 30 | 31 | motors: 32 | waist: 33 | ID: 1 34 | Baud_Rate: 3 35 | Return_Delay_Time: 0 36 | Drive_Mode: 0 37 | Velocity_Limit: 131 38 | Min_Position_Limit: 0 39 | Max_Position_Limit: 4095 40 | Secondary_ID: 255 41 | 42 | shoulder: 43 | ID: 2 44 | Baud_Rate: 3 45 | Return_Delay_Time: 0 46 | Drive_Mode: 1 47 | Velocity_Limit: 131 48 | Min_Position_Limit: 841 49 | Max_Position_Limit: 2867 50 | Secondary_ID: 255 51 | 52 | shoulder_shadow: 53 | ID: 3 54 | Baud_Rate: 3 55 | Return_Delay_Time: 0 56 | Drive_Mode: 0 57 | Velocity_Limit: 131 58 | Min_Position_Limit: 841 59 | Max_Position_Limit: 2867 60 | Secondary_ID: 2 61 | 62 | elbow: 63 | ID: 4 64 | Baud_Rate: 3 65 | Return_Delay_Time: 0 66 | Drive_Mode: 1 67 | Velocity_Limit: 131 68 | Min_Position_Limit: 898 69 | Max_Position_Limit: 3094 70 | Secondary_ID: 255 71 | 72 | elbow_shadow: 73 | ID: 5 74 | Baud_Rate: 3 75 | Return_Delay_Time: 0 76 | Drive_Mode: 0 77 | Velocity_Limit: 131 78 | Min_Position_Limit: 898 79 | Max_Position_Limit: 3094 80 | Secondary_ID: 4 81 | 82 | forearm_roll: 83 | ID: 6 84 | Baud_Rate: 3 85 | Return_Delay_Time: 0 86 | Drive_Mode: 0 87 | Velocity_Limit: 131 88 | Min_Position_Limit: 0 89 | Max_Position_Limit: 4095 90 | Secondary_ID: 255 91 | 92 | wrist_1_joint: 93 | ID: 7 94 | Baud_Rate: 3 95 | Return_Delay_Time: 0 96 | Drive_Mode: 1 97 | Velocity_Limit: 131 98 | Min_Position_Limit: 830 99 | Max_Position_Limit: 3504 100 | Secondary_ID: 255 101 | 102 | wrist_2_joint: 103 | ID: 8 104 | Baud_Rate: 3 105 | Return_Delay_Time: 0 106 | Drive_Mode: 0 107 | Velocity_Limit: 131 108 | Min_Position_Limit: 0 109 | Max_Position_Limit: 4095 110 | Secondary_ID: 255 111 | 112 | wrist_3_joint: 113 | ID: 9 114 | Baud_Rate: 3 115 | Return_Delay_Time: 0 116 | Drive_Mode: 0 117 | Velocity_Limit: 131 118 | Min_Position_Limit: 0 119 | Max_Position_Limit: 4095 120 | Secondary_ID: 255 121 | -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/launch/xsarm_control.launch: -------------------------------------------------------------------------------- 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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/launch/xsarm_description.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 19 | 20 | 21 | 22 | 27 | 28 | $(arg source_list) 29 | 30 | 31 | 36 | 37 | 38 | 43 | 44 | 45 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/meshes/interbotix_black.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/guided_vision_ros/vx300s_7dof_description/meshes/interbotix_black.png -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/meshes/vx300s_1_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/guided_vision_ros/vx300s_7dof_description/meshes/vx300s_1_base.stl -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/meshes/vx300s_2_shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/guided_vision_ros/vx300s_7dof_description/meshes/vx300s_2_shoulder.stl -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/meshes/vx300s_3_upper_arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/guided_vision_ros/vx300s_7dof_description/meshes/vx300s_3_upper_arm.stl -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/meshes/vx300s_4_upper_forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/guided_vision_ros/vx300s_7dof_description/meshes/vx300s_4_upper_forearm.stl -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/meshes/vx300s_5_lower_forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/guided_vision_ros/vx300s_7dof_description/meshes/vx300s_5_lower_forearm.stl -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/meshes/vx300s_7dof_wrist_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/guided_vision_ros/vx300s_7dof_description/meshes/vx300s_7dof_wrist_1.stl -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/meshes/vx300s_7dof_wrist_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/guided_vision_ros/vx300s_7dof_description/meshes/vx300s_7dof_wrist_2.stl -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/meshes/vx300s_7dof_wrist_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/guided_vision_ros/vx300s_7dof_description/meshes/vx300s_7dof_wrist_3.stl -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/meshes/zedm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/guided_vision_ros/vx300s_7dof_description/meshes/zedm.stl -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vx300s_7dof_description 4 | 0.0.0 5 | The interbotix_xsarm_descriptions package 6 | Solomon Wiznitzer 7 | BSD 8 | 9 | Solomon Wiznitzer 10 | 11 | 12 | 13 | catkin 14 | joint_state_publisher 15 | joint_state_publisher_gui 16 | robot_state_publisher 17 | roslaunch 18 | rviz 19 | xacro 20 | joint_state_publisher 21 | joint_state_publisher_gui 22 | robot_state_publisher 23 | roslaunch 24 | rviz 25 | xacro 26 | joint_state_publisher 27 | joint_state_publisher_gui 28 | robot_state_publisher 29 | roslaunch 30 | rviz 31 | xacro 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /guided_vision_ros/vx300s_7dof_description/urdf/zedm_macro.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /gym_guided_vision/.gitignore: -------------------------------------------------------------------------------- 1 | data 2 | 3 | # Byte-compiled / optimized / DLL files 4 | __pycache__/ 5 | *.py[cod] 6 | *$py.class 7 | 8 | # C extensions 9 | *.so 10 | 11 | # Distribution / packaging 12 | .Python 13 | build/ 14 | develop-eggs/ 15 | dist/ 16 | downloads/ 17 | eggs/ 18 | .eggs/ 19 | lib/ 20 | lib64/ 21 | parts/ 22 | sdist/ 23 | var/ 24 | wheels/ 25 | share/python-wheels/ 26 | *.egg-info/ 27 | .installed.cfg 28 | *.egg 29 | MANIFEST 30 | 31 | # PyInstaller 32 | # Usually these files are written by a python script from a template 33 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 34 | *.manifest 35 | *.spec 36 | 37 | # Installer logs 38 | pip-log.txt 39 | pip-delete-this-directory.txt 40 | 41 | # Unit test / coverage reports 42 | htmlcov/ 43 | .tox/ 44 | .nox/ 45 | .coverage 46 | .coverage.* 47 | .cache 48 | nosetests.xml 49 | coverage.xml 50 | *.cover 51 | *.py,cover 52 | .hypothesis/ 53 | .pytest_cache/ 54 | cover/ 55 | 56 | # Translations 57 | *.mo 58 | *.pot 59 | 60 | # Django stuff: 61 | *.log 62 | local_settings.py 63 | db.sqlite3 64 | db.sqlite3-journal 65 | 66 | # Flask stuff: 67 | instance/ 68 | .webassets-cache 69 | 70 | # Scrapy stuff: 71 | .scrapy 72 | 73 | # Sphinx documentation 74 | docs/_build/ 75 | 76 | # PyBuilder 77 | .pybuilder/ 78 | target/ 79 | 80 | # Jupyter Notebook 81 | .ipynb_checkpoints 82 | 83 | # IPython 84 | profile_default/ 85 | ipython_config.py 86 | 87 | # pyenv 88 | # For a library or package, you might want to ignore these files since the code is 89 | # intended to run in multiple environments; otherwise, check them in: 90 | # .python-version 91 | 92 | # pipenv 93 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 94 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 95 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 96 | # install all needed dependencies. 97 | #Pipfile.lock 98 | 99 | # poetry 100 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 101 | # This is especially recommended for binary packages to ensure reproducibility, and is more 102 | # commonly ignored for libraries. 103 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 104 | #poetry.lock 105 | 106 | # pdm 107 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 108 | #pdm.lock 109 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 110 | # in version control. 111 | # https://pdm.fming.dev/latest/usage/project/#working-with-version-control 112 | .pdm.toml 113 | .pdm-python 114 | .pdm-build/ 115 | 116 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 117 | __pypackages__/ 118 | 119 | # Celery stuff 120 | celerybeat-schedule 121 | celerybeat.pid 122 | 123 | # SageMath parsed files 124 | *.sage.py 125 | 126 | # Environments 127 | .env 128 | .venv 129 | env/ 130 | venv/ 131 | ENV/ 132 | env.bak/ 133 | venv.bak/ 134 | 135 | # Spyder project settings 136 | .spyderproject 137 | .spyproject 138 | 139 | # Rope project settings 140 | .ropeproject 141 | 142 | # mkdocs documentation 143 | /site 144 | 145 | # mypy 146 | .mypy_cache/ 147 | .dmypy.json 148 | dmypy.json 149 | 150 | # Pyre type checker 151 | .pyre/ 152 | 153 | # pytype static type analyzer 154 | .pytype/ 155 | 156 | # Cython debug symbols 157 | cython_debug/ 158 | 159 | # PyCharm 160 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 161 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 162 | # and can be added to the global gitignore or merged into this file. For a more nuclear 163 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 164 | #.idea/ -------------------------------------------------------------------------------- /gym_guided_vision/README.md: -------------------------------------------------------------------------------- 1 | ```python 2 | ENVS = { 3 | # InsertPeg 4 | "gym_guided_vision/InsertPeg-3Arms-v0": { 5 | "env": "InsertPegEnv", 6 | "num_arms": 3, 7 | "cameras": ["zed_cam_left", "zed_cam_right", "wrist_cam_left", "wrist_cam_right", "overhead_cam", "worms_eye_cam"], 8 | "observation_height": 480, 9 | "observation_width": 640, 10 | }, 11 | "gym_guided_vision/InsertPeg-2Arms-v0": { 12 | "env": "InsertPegEnv", 13 | "num_arms": 2, 14 | "cameras": ["overhead_cam", "worms_eye_cam", "wrist_cam_left", "wrist_cam_right"], 15 | "observation_height": 480, 16 | "observation_width": 640, 17 | }, 18 | 19 | # SlotInsertion 20 | "gym_guided_vision/SlotInsertion-3Arms-v0": { 21 | "env": "SlotInsertion", 22 | "num_arms": 3, 23 | "cameras": ["zed_cam_left", "zed_cam_right", "wrist_cam_left", "wrist_cam_right", "overhead_cam", "worms_eye_cam"], 24 | "observation_height": 480, 25 | "observation_width": 640, 26 | }, 27 | "gym_guided_vision/SlotInsertion-2Arms-v0": { 28 | "env": "SlotInsertion", 29 | "num_arms": 2, 30 | "cameras": ["overhead_cam", "worms_eye_cam", "wrist_cam_left", "wrist_cam_right"], 31 | "observation_height": 480, 32 | "observation_width": 640, 33 | }, 34 | 35 | # SewNeedle 36 | "gym_guided_vision/SewNeedle-3Arms-v0": { 37 | "env": "SewNeedle", 38 | "num_arms": 3, 39 | "cameras": ["zed_cam_left", "zed_cam_right", "wrist_cam_left", "wrist_cam_right", "overhead_cam", "worms_eye_cam"], 40 | "observation_height": 480, 41 | "observation_width": 640, 42 | }, 43 | "gym_guided_vision/SewNeedle-2Arms-v0": { 44 | "env": "SewNeedle", 45 | "num_arms": 2, 46 | "cameras": ["overhead_cam", "worms_eye_cam", "wrist_cam_left", "wrist_cam_right"], 47 | "observation_height": 480, 48 | "observation_width": 640, 49 | }, 50 | 51 | # TubeTransfer 52 | "gym_guided_vision/TubeTransfer-3Arms-v0": { 53 | "env": "TubeTransfer", 54 | "num_arms": 3, 55 | "cameras": ["zed_cam_left", "zed_cam_right", "wrist_cam_left", "wrist_cam_right", "overhead_cam", "worms_eye_cam"], 56 | "observation_height": 480, 57 | "observation_width": 640, 58 | }, 59 | "gym_guided_vision/TubeTransfer-2Arms-v0": { 60 | "env": "TubeTransfer", 61 | "num_arms": 2, 62 | "cameras": ["overhead_cam", "worms_eye_cam", "wrist_cam_left", "wrist_cam_right"], 63 | "observation_height": 480, 64 | "observation_width": 640, 65 | }, 66 | 67 | 68 | # HookPackage 69 | "gym_guided_vision/HookPackage-3Arms-v0": { 70 | "env": "HookPackage", 71 | "num_arms": 3, 72 | "cameras": ["zed_cam_left", "zed_cam_right", "wrist_cam_left", "wrist_cam_right", "overhead_cam", "worms_eye_cam"], 73 | "observation_height": 480, 74 | "observation_width": 640, 75 | }, 76 | "gym_guided_vision/HookPackage-2Arms-v0": { 77 | "env": "HookPackage", 78 | "num_arms": 2, 79 | "cameras": ["overhead_cam", "worms_eye_cam", "wrist_cam_left", "wrist_cam_right"], 80 | "observation_height": 480, 81 | "observation_width": 640, 82 | }, 83 | 84 | } 85 | ``` -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/__init__.py: -------------------------------------------------------------------------------- 1 | from gymnasium.envs.registration import register 2 | 3 | 4 | ENVS = { 5 | # InsertPeg 6 | "gym_guided_vision/InsertPeg-3Arms-v0": { 7 | "env": "InsertPegEnv", 8 | "num_arms": 3, 9 | "cameras": ["zed_cam_left", "zed_cam_right", "wrist_cam_left", "wrist_cam_right", "overhead_cam", "worms_eye_cam"], 10 | "observation_height": 480, 11 | "observation_width": 640, 12 | }, 13 | "gym_guided_vision/InsertPeg-2Arms-v0": { 14 | "env": "InsertPegEnv", 15 | "num_arms": 2, 16 | "cameras": ["overhead_cam", "worms_eye_cam", "wrist_cam_left", "wrist_cam_right"], 17 | "observation_height": 480, 18 | "observation_width": 640, 19 | }, 20 | 21 | # SlotInsertion 22 | "gym_guided_vision/SlotInsertion-3Arms-v0": { 23 | "env": "SlotInsertionEnv", 24 | "num_arms": 3, 25 | "cameras": ["zed_cam_left", "zed_cam_right", "wrist_cam_left", "wrist_cam_right", "overhead_cam", "worms_eye_cam"], 26 | "observation_height": 480, 27 | "observation_width": 640, 28 | }, 29 | "gym_guided_vision/SlotInsertion-2Arms-v0": { 30 | "env": "SlotInsertionEnv", 31 | "num_arms": 2, 32 | "cameras": ["overhead_cam", "worms_eye_cam", "wrist_cam_left", "wrist_cam_right"], 33 | "observation_height": 480, 34 | "observation_width": 640, 35 | }, 36 | 37 | # SewNeedle 38 | "gym_guided_vision/SewNeedle-3Arms-v0": { 39 | "env": "SewNeedleEnv", 40 | "num_arms": 3, 41 | "cameras": ["zed_cam_left", "zed_cam_right", "wrist_cam_left", "wrist_cam_right", "overhead_cam", "worms_eye_cam"], 42 | "observation_height": 480, 43 | "observation_width": 640, 44 | }, 45 | "gym_guided_vision/SewNeedle-2Arms-v0": { 46 | "env": "SewNeedleEnv", 47 | "num_arms": 2, 48 | "cameras": ["overhead_cam", "worms_eye_cam", "wrist_cam_left", "wrist_cam_right"], 49 | "observation_height": 480, 50 | "observation_width": 640, 51 | }, 52 | 53 | # TubeTransfer 54 | "gym_guided_vision/TubeTransfer-3Arms-v0": { 55 | "env": "TubeTransferEnv", 56 | "num_arms": 3, 57 | "cameras": ["zed_cam_left", "zed_cam_right", "wrist_cam_left", "wrist_cam_right", "overhead_cam", "worms_eye_cam"], 58 | "observation_height": 480, 59 | "observation_width": 640, 60 | }, 61 | "gym_guided_vision/TubeTransfer-2Arms-v0": { 62 | "env": "TubeTransferEnv", 63 | "num_arms": 2, 64 | "cameras": ["overhead_cam", "worms_eye_cam", "wrist_cam_left", "wrist_cam_right"], 65 | "observation_height": 480, 66 | "observation_width": 640, 67 | }, 68 | 69 | 70 | # HookPackage 71 | "gym_guided_vision/HookPackage-3Arms-v0": { 72 | "env": "HookPackageEnv", 73 | "num_arms": 3, 74 | "cameras": ["zed_cam_left", "zed_cam_right", "wrist_cam_left", "wrist_cam_right", "overhead_cam", "worms_eye_cam"], 75 | "observation_height": 480, 76 | "observation_width": 640, 77 | }, 78 | "gym_guided_vision/HookPackage-2Arms-v0": { 79 | "env": "HookPackageEnv", 80 | "num_arms": 2, 81 | "cameras": ["overhead_cam", "worms_eye_cam", "wrist_cam_left", "wrist_cam_right"], 82 | "observation_height": 480, 83 | "observation_width": 640, 84 | }, 85 | 86 | } 87 | 88 | for env_id, env_kwargs in ENVS.items(): 89 | register( 90 | id=env_id, 91 | entry_point=f"gym_guided_vision.env:{env_kwargs['env']}", 92 | # Even after seeding, the rendered observations are slightly different, 93 | # so we set `nondeterministic=True` to pass `check_env` tests 94 | nondeterministic=True, 95 | kwargs={ 96 | "num_arms": env_kwargs["num_arms"], 97 | "cameras": env_kwargs["cameras"], 98 | "observation_height": env_kwargs["observation_height"], 99 | "observation_width": env_kwargs["observation_width"], 100 | } 101 | ) 102 | -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2023, Trossen Robotics 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | 3. Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/joint_position_actuators.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 40 | 41 | -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/angled_extrusion.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/angled_extrusion.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/corner_bracket.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/corner_bracket.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/d405_solid.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/d405_solid.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/extrusion_1000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/extrusion_1000.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/extrusion_1220.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/extrusion_1220.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/extrusion_150.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/extrusion_150.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/extrusion_2040_1000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/extrusion_2040_1000.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/extrusion_2040_880.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/extrusion_2040_880.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/extrusion_600.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/extrusion_600.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/overhead_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/overhead_mount.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/small_meta_table_diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/small_meta_table_diffuse.png -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/tabletop.obj: -------------------------------------------------------------------------------- 1 | mtllib material_0.mtl 2 | usemtl material_0 3 | v -0.37953900 -0.60982800 0.73674500 4 | v -0.37953900 -0.60982800 0.74887200 5 | v -0.38100000 -0.60839100 0.73674500 6 | v -0.38100000 -0.60839100 0.74887200 7 | v -0.18977000 -0.60982800 0.74887200 8 | v -0.18977000 -0.60839100 0.75078500 9 | v -0.37953900 -0.60839100 0.75078700 10 | v -0.38100000 0.60793400 0.74887200 11 | v -0.37953900 0.60793400 0.75078700 12 | v 0.38100000 -0.60839100 0.73674500 13 | v 0.38100000 -0.60839100 0.74887200 14 | v 0.37953900 -0.60982800 0.73674500 15 | v 0.37953900 -0.60982800 0.74887200 16 | v 0.37953900 0.60793400 0.75078700 17 | v 0.37953900 -0.60839100 0.75078700 18 | v 0.38100000 0.60793400 0.74887200 19 | v -0.38100000 0.60793400 0.73674500 20 | v -0.37953900 0.60937200 0.73674500 21 | v -0.37953900 0.60937200 0.74887200 22 | v -0.18977000 0.60793400 0.75078700 23 | v -0.18977000 0.60937200 0.74887200 24 | v 0.37953900 0.60937200 0.73674500 25 | v 0.37953900 0.60937200 0.74887200 26 | v 0.38100000 0.60793400 0.73674500 27 | v -0.37953900 -0.60839100 0.73483100 28 | v -0.37953900 0.60793400 0.73483100 29 | v -0.18977000 0.60937200 0.73674500 30 | v -0.18977000 0.60793400 0.73483100 31 | v 0.37953900 -0.60839100 0.73483100 32 | v 0.37953900 0.60793400 0.73483100 33 | v -0.18977000 -0.60839100 0.73483100 34 | v -0.18977000 -0.60982800 0.73674500 35 | v 0.18977000 0.60793400 0.75078700 36 | v 0.18977000 -0.60839100 0.75078700 37 | v 0.18977000 0.60937200 0.74887200 38 | v 0.18977000 0.60937200 0.73674500 39 | v 0.18977000 0.60793400 0.73483100 40 | v 0.18977000 -0.60839100 0.73483100 41 | v 0.18977000 -0.60982800 0.73674500 42 | v 0.18977000 -0.60982800 0.74887200 43 | v 0.00000000 0.60793400 0.75078700 44 | v 0.00000000 -0.60839100 0.75078500 45 | v 0.00000000 0.60937200 0.74887200 46 | v 0.00000000 0.60937200 0.73674500 47 | v 0.00000000 0.60793400 0.73483100 48 | v 0.00000000 -0.60839100 0.73483100 49 | v 0.00000000 -0.60982800 0.73674500 50 | v 0.00000000 -0.60982800 0.74887200 51 | vt 0.75019800 0.57261000 52 | vt 0.74519900 0.57261000 53 | vt 0.74382100 0.56603500 54 | vt 0.74382100 0.57103100 55 | vt 0.74519900 0.67519800 56 | vt 0.74362100 0.67519800 57 | vt 0.74362100 0.57261000 58 | vt 0.12297800 0.57103200 59 | vt 0.12297900 0.57261000 60 | vt 0.74362100 0.15331600 61 | vt 0.74382100 0.98454000 62 | vt 0.75019800 0.98296000 63 | vt 0.74519900 0.98296000 64 | vt 0.12381100 0.98296000 65 | vt 0.74362100 0.98296000 66 | vt 0.12381700 0.98453800 67 | vt 0.11445600 0.57182100 68 | vt 0.11448400 0.57261000 69 | vt 0.12160500 0.57261000 70 | vt 0.12318700 0.67519800 71 | vt 0.12181300 0.67519800 72 | vt 0.11531600 0.98296000 73 | vt 0.12243700 0.98296000 74 | vt 0.11524200 0.98375000 75 | vt 0.74362100 0.56445500 76 | vt 0.12296000 0.56445500 77 | vt 0.11469200 0.67519800 78 | vt 0.11331800 0.67519800 79 | vt 0.74362100 0.15410500 80 | vt 0.12256600 0.15410500 81 | vt 0.74362100 0.46186700 82 | vt 0.74441000 0.46186700 83 | vt 0.12360300 0.88037300 84 | vt 0.74362100 0.88037300 85 | vt 0.12222900 0.88037300 86 | vt 0.11510800 0.88037300 87 | vt 0.11373500 0.88037300 88 | vt 0.74362100 0.25669200 89 | vt 0.74441000 0.25669200 90 | vt 0.74519900 0.88037300 91 | vt 0.12339500 0.77778500 92 | vt 0.74362100 0.77778500 93 | vt 0.12202100 0.77778500 94 | vt 0.11490000 0.77778500 95 | vt 0.11352700 0.77778500 96 | vt 0.74362100 0.35928000 97 | vt 0.74441000 0.35928000 98 | vt 0.74519900 0.77778500 99 | f 1/1 2/2 3/3 100 | f 3/3 2/2 4/4 101 | f 5/5 6/6 2/2 102 | f 2/2 6/6 7/7 103 | f 8/8 4/4 9/9 104 | f 9/9 4/4 7/7 105 | f 10/10 11/11 12/12 106 | f 12/12 11/11 13/13 107 | f 14/14 15/15 16/16 108 | f 16/16 15/15 11/11 109 | f 17/17 8/8 18/18 110 | f 18/18 8/8 19/19 111 | f 20/20 21/21 9/9 112 | f 9/9 21/21 19/19 113 | f 22/22 23/23 24/24 114 | f 24/24 23/23 16/16 115 | f 3/3 17/17 25/25 116 | f 25/25 17/17 26/26 117 | f 27/27 28/28 18/18 118 | f 18/18 28/28 26/26 119 | f 29/29 30/30 10/10 120 | f 10/10 30/30 24/24 121 | f 31/31 32/32 25/25 122 | f 25/25 32/32 1/1 123 | f 9/9 7/7 20/20 124 | f 20/20 7/7 6/6 125 | f 18/18 19/19 27/27 126 | f 27/27 19/19 21/21 127 | f 25/25 26/26 31/31 128 | f 31/31 26/26 28/28 129 | f 2/2 1/1 5/5 130 | f 5/5 1/1 32/32 131 | f 16/16 11/11 24/24 132 | f 24/24 11/11 10/10 133 | f 17/17 3/3 8/8 134 | f 8/8 3/3 4/4 135 | f 2/2 7/7 4/4 136 | f 11/11 15/15 13/13 137 | f 8/8 9/9 19/19 138 | f 23/23 14/14 16/16 139 | f 17/17 18/18 26/26 140 | f 30/30 22/22 24/24 141 | f 3/3 25/25 1/1 142 | f 12/12 29/29 10/10 143 | f 33/33 34/34 14/14 144 | f 14/14 34/34 15/15 145 | f 23/23 35/35 14/14 146 | f 14/14 35/35 33/33 147 | f 36/36 35/35 22/22 148 | f 22/22 35/35 23/23 149 | f 30/30 37/37 22/22 150 | f 22/22 37/37 36/36 151 | f 38/38 37/37 29/29 152 | f 29/29 37/37 30/30 153 | f 12/12 39/39 29/29 154 | f 29/29 39/39 38/38 155 | f 40/40 39/39 13/13 156 | f 13/13 39/39 12/12 157 | f 15/15 34/34 13/13 158 | f 13/13 34/34 40/40 159 | f 41/41 42/42 33/33 160 | f 33/33 42/42 34/34 161 | f 33/33 35/35 41/41 162 | f 41/41 35/35 43/43 163 | f 44/44 43/43 36/36 164 | f 36/36 43/43 35/35 165 | f 36/36 37/37 44/44 166 | f 44/44 37/37 45/45 167 | f 46/46 45/45 38/38 168 | f 38/38 45/45 37/37 169 | f 38/38 39/39 46/46 170 | f 46/46 39/39 47/47 171 | f 48/48 47/47 40/40 172 | f 40/40 47/47 39/39 173 | f 40/40 34/34 48/48 174 | f 48/48 34/34 42/42 175 | f 20/20 6/6 41/41 176 | f 41/41 6/6 42/42 177 | f 41/41 43/43 20/20 178 | f 20/20 43/43 21/21 179 | f 27/27 21/21 44/44 180 | f 44/44 21/21 43/43 181 | f 44/44 45/45 27/27 182 | f 27/27 45/45 28/28 183 | f 31/31 28/28 46/46 184 | f 46/46 28/28 45/45 185 | f 46/46 47/47 31/31 186 | f 31/31 47/47 32/32 187 | f 5/5 32/32 48/48 188 | f 48/48 32/32 47/47 189 | f 48/48 42/42 5/5 190 | f 5/5 42/42 6/6 191 | -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_1_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_1_base.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_2_shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_2_shoulder.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_3_upper_arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_3_upper_arm.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_4_upper_forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_4_upper_forearm.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_5_lower_forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_5_lower_forearm.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_6_wrist.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_6_wrist.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7_gripper.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7_gripper.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7_gripper_bar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7_gripper_bar.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7_gripper_camera.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7_gripper_camera.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7_gripper_prop.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7_gripper_prop.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7_gripper_prop_bar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7_gripper_prop_bar.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7_gripper_wrist_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7_gripper_wrist_mount.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7dof_wrist_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7dof_wrist_1.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7dof_wrist_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7dof_wrist_2.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7dof_wrist_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_7dof_wrist_3.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_8_custom_finger_left.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_8_custom_finger_left.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_8_custom_finger_right.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/vx300s_8_custom_finger_right.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/wormseye_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/wormseye_mount.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/meshes/zedm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Soltanilara/av-aloha/65da5200dda9200ced58bc3dadb5345f4703d017/gym_guided_vision/gym_guided_vision/assets/meshes/zedm.stl -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/task_hook_package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/task_insert_peg.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/task_sew_needle.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/task_slot_insertion.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/assets/task_tube_transfer.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 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 | -------------------------------------------------------------------------------- /gym_guided_vision/gym_guided_vision/constants.py: -------------------------------------------------------------------------------- 1 | import pathlib 2 | import os 3 | # task parameters 4 | XML_PATH = os.path.join(pathlib.Path(__file__).parent.resolve(), 'assets', 'aloha.xml') # note: absolute paths 5 | XML_DIR = str(pathlib.Path(__file__).parent.resolve()) + '/assets' 6 | DATA_DIR = str(pathlib.Path(__file__).parent.resolve()) + '/data/recordings' 7 | 8 | CAMERAS = [ 9 | "zed_cam_left", 10 | "zed_cam_right", 11 | "wrist_cam_left", 12 | "wrist_cam_right", 13 | "overhead_cam", 14 | "worms_eye_cam", 15 | ] 16 | 17 | RENDER_CAMERA = "overhead_cam" 18 | 19 | # physics parameters 20 | SIM_PHYSICS_DT=0.002 21 | SIM_DT = 0.04 22 | SIM_PHYSICS_ENV_STEP_RATIO = int(SIM_DT/SIM_PHYSICS_DT) 23 | SIM_DT = SIM_PHYSICS_DT * SIM_PHYSICS_ENV_STEP_RATIO 24 | 25 | # robot parameters 26 | LEFT_ARM_POSE = [0, -0.082, 1.06, 0, -0.953, 0, 0.02239] 27 | RIGHT_ARM_POSE = [0, -0.082, 1.06, 0, -0.953, 0, 0.02239] 28 | MIDDLE_ARM_POSE = [0, -0.8, 0.8, 0, 0.5, 0, 0] 29 | LEFT_JOINT_NAMES = [ 30 | "left_waist", 31 | "left_shoulder", 32 | "left_elbow", 33 | "left_forearm_roll", 34 | "left_wrist_angle", 35 | "left_wrist_rotate", 36 | "left_left_finger", 37 | ] 38 | RIGHT_JOINT_NAMES = [ 39 | "right_waist", 40 | "right_shoulder", 41 | "right_elbow", 42 | "right_forearm_roll", 43 | "right_wrist_angle", 44 | "right_wrist_rotate", 45 | "right_right_finger", 46 | ] 47 | MIDDLE_JOINT_NAMES = [ 48 | "middle_waist", 49 | "middle_shoulder", 50 | "middle_elbow", 51 | "middle_forearm_roll", 52 | "middle_wrist_1_joint", 53 | "middle_wrist_2_joint", 54 | "middle_wrist_3_joint", 55 | ] 56 | LEFT_ACTUATOR_NAMES = [ 57 | "left_waist", 58 | "left_shoulder", 59 | "left_elbow", 60 | "left_forearm_roll", 61 | "left_wrist_angle", 62 | "left_wrist_rotate", 63 | "left_gripper", 64 | ] 65 | RIGHT_ACTUATOR_NAMES = [ 66 | "right_waist", 67 | "right_shoulder", 68 | "right_elbow", 69 | "right_forearm_roll", 70 | "right_wrist_angle", 71 | "right_wrist_rotate", 72 | "right_gripper", 73 | ] 74 | MIDDLE_ACTUATOR_NAMES = [ 75 | "middle_waist", 76 | "middle_shoulder", 77 | "middle_elbow", 78 | "middle_forearm_roll", 79 | "middle_wrist_1_joint", 80 | "middle_wrist_2_joint", 81 | "middle_wrist_3_joint", 82 | ] 83 | LEFT_EEF_SITE = "left_gripper_control" 84 | RIGHT_EEF_SITE = "right_gripper_control" 85 | MIDDLE_EEF_SITE = "middle_zed_camera_center" 86 | MIDDLE_BASE_LINK = "middle_base_link" 87 | LEFT_GRIPPER_JOINT_NAMES = ["left_left_finger", "left_right_finger"] 88 | RIGHT_GRIPPER_JOINT_NAMES = ["right_left_finger", "right_right_finger"] -------------------------------------------------------------------------------- /gym_guided_vision/pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.poetry] 2 | name = "gym_guided_vision" 3 | version = "0.1.0" 4 | description = "gym env for guided vision project" 5 | authors = ["Your Name "] 6 | readme = "README.md" 7 | 8 | [tool.poetry.dependencies] 9 | python = "^3.10" 10 | mujoco = "^3.2.2" 11 | dm-control = "^1.0.22" 12 | 13 | [tool.poetry.group.dev.dependencies] 14 | numba = "^0.60.0" 15 | h5py = "^3.11.0" 16 | 17 | [build-system] 18 | requires = ["poetry-core"] 19 | build-backend = "poetry.core.masonry.api" 20 | -------------------------------------------------------------------------------- /gym_guided_vision/scripts/check_dataset_reward.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('../') 3 | 4 | import gym_guided_vision 5 | import gymnasium as gym 6 | 7 | import os 8 | import h5py 9 | import argparse 10 | import time 11 | import numpy as np 12 | import glob 13 | from tqdm import tqdm 14 | 15 | def main(args): 16 | dataset_dir = args["dataset_dir"] 17 | render = args["render"] 18 | 19 | env = gym.make(args["env"]) 20 | 21 | not_max_reward_episodes = [] 22 | 23 | # get files in the dataset directory 24 | dataset_paths = glob.glob(os.path.join(dataset_dir, f'episode_*.hdf5')) 25 | 26 | # Using tqdm to track progress over episodes 27 | episode_idx = 0 28 | for dataset_path in tqdm(dataset_paths, desc="Processing Episodes"): 29 | with h5py.File(dataset_path, 'r') as root: 30 | all_qpos = root['/observations/all_qpos'][()] 31 | actions = root['/action'][()] 32 | 33 | ts, info = env.reset(options={}) 34 | 35 | env.unwrapped.set_qpos(all_qpos[0]) 36 | 37 | reward = [] 38 | 39 | for action in actions: 40 | step_start = time.time() 41 | 42 | # Apply the action 43 | env.unwrapped.step_action(action) 44 | if render: env.unwrapped.render_viewer() 45 | 46 | reward.append(env.unwrapped.get_reward()) 47 | 48 | # Rudimentary time keeping, will drift relative to wall clock. 49 | if render: 50 | time_until_next_step = 0.001 - (time.time() - step_start) 51 | time.sleep(max(0, time_until_next_step)) 52 | 53 | # Check if the episode did not reach the maximum reward 54 | if max(reward) != env.unwrapped.max_reward: 55 | not_max_reward_episodes.append(episode_idx) 56 | 57 | episode_idx += 1 58 | 59 | # Print all episodes that did not reach max reward 60 | if not_max_reward_episodes: 61 | print(f"Episodes that did not reach max reward: {not_max_reward_episodes}") 62 | else: 63 | print("All episodes reached max reward!") 64 | 65 | 66 | 67 | if __name__ == '__main__': 68 | parser = argparse.ArgumentParser() 69 | parser.add_argument('--dataset_dir', action='store', type=str, help='Path to the dataset directory.', required=True) 70 | parser.add_argument('--env', action='store', type=str, help='Task name.', required=True) 71 | parser.add_argument('--render', action='store_true', help='Render the environment.') 72 | 73 | main(vars(parser.parse_args())) 74 | 75 | 76 | -------------------------------------------------------------------------------- /gym_guided_vision/scripts/replay_sim_episode.py: -------------------------------------------------------------------------------- 1 | import os 2 | import h5py 3 | import argparse 4 | import time 5 | import glob 6 | from tqdm import tqdm 7 | 8 | import gymnasium as gym 9 | import gym_guided_vision 10 | 11 | def save_episode(data_dict, save_path): 12 | 13 | camera_names = [key.split('/')[-1] for key in data_dict.keys() if 'images' in key] 14 | max_timesteps = len(data_dict['/observations/qpos']) 15 | 16 | # HDF5 17 | try: 18 | if len(camera_names) > 0: 19 | image_shapes = {cam_name: data_dict[f'/observations/images/{cam_name}'][0].shape for cam_name in camera_names} 20 | qpos_len = len(data_dict['/observations/qpos'][0]) 21 | qvel_len = len(data_dict['/observations/qvel'][0]) 22 | action_len = len(data_dict['/action'][0]) 23 | except IndexError: 24 | print('Empty episode, skipping...') 25 | return False 26 | t0 = time.time() 27 | with h5py.File(save_path, 'w', rdcc_nbytes=1024 ** 2 * 2) as root: 28 | root.attrs['sim'] = True 29 | obs = root.create_group('observations') 30 | if len(camera_names) > 0: 31 | image = obs.create_group('images') 32 | for cam_name in camera_names: 33 | _ = image.create_dataset(cam_name, (max_timesteps, *image_shapes[cam_name]), dtype='uint8', 34 | chunks=(1, *image_shapes[cam_name]), ) 35 | # compression='gzip',compression_opts=2,) 36 | # compression=32001, compression_opts=(0, 0, 0, 0, 9, 1, 1), shuffle=False) 37 | data_qpos = obs.create_dataset('qpos', (max_timesteps, qpos_len)) 38 | data_qvel = obs.create_dataset('qvel', (max_timesteps, qvel_len)) 39 | data_action = root.create_dataset('action', (max_timesteps, action_len)) 40 | 41 | for name, array in data_dict.items(): 42 | root[name][...] = array 43 | # print(f'Saving: {time.time() - t0:.1f} secs\n') 44 | return True 45 | 46 | 47 | def replay_episode(env, dataset_path, save_path): 48 | if not os.path.isfile(dataset_path): 49 | print(f'Dataset does not exist at \n{dataset_path}\n') 50 | exit() 51 | 52 | 53 | ts, info = env.reset() 54 | 55 | with h5py.File(dataset_path, 'r') as root: 56 | qpos = root['/observations/qpos'][()] 57 | qvel = root['/observations/qvel'][()] 58 | all_qpos = root['/observations/all_qpos'][()] 59 | action = root['/action'][()] 60 | 61 | if env.unwrapped.num_arms == 2: 62 | data_dict = { 63 | '/observations/qpos': qpos[:,:14], 64 | '/observations/qvel': qvel[:,:14], 65 | '/action': action[:,:14], 66 | } 67 | else: 68 | data_dict = { 69 | '/observations/qpos': qpos, 70 | '/observations/qvel': qvel, 71 | '/action': action, 72 | } 73 | for cam_name in ts['pixels']: 74 | data_dict[f'/observations/images/{cam_name}'] = [] 75 | 76 | for qpos in all_qpos: 77 | step_start = time.time() 78 | 79 | # Apply the action 80 | env.unwrapped.set_qpos(qpos) 81 | ts = env.unwrapped.get_obs() 82 | 83 | # Save the images 84 | for cam_name in ts['pixels']: 85 | data_dict[f'/observations/images/{cam_name}'].append(ts['pixels'][cam_name]) 86 | 87 | # print(f'Step time: {time.time() - step_start:.2f} secs') 88 | 89 | save_episode(data_dict, save_path) 90 | 91 | 92 | def main(args): 93 | dataset_dir = args["dataset_dir"] 94 | episode_idx = args["episode_idx"] 95 | env_name = args["env"] 96 | 97 | env = gym.make(env_name) 98 | 99 | if episode_idx is None: 100 | dataset_paths = glob.glob(os.path.join(dataset_dir, f'episode_*.hdf5')) 101 | dataset_paths.sort() 102 | else: 103 | dataset_paths = glob.glob(os.path.join(dataset_dir, f'episode_{episode_idx}.hdf5')) 104 | 105 | 106 | for dataset_path in tqdm(dataset_paths): 107 | 108 | dataset_name = dataset_path.split('/')[-1] 109 | save_path = os.path.join( 110 | dataset_dir, env_name.split('/')[-1], dataset_name) 111 | os.makedirs(os.path.dirname(save_path), exist_ok=True) 112 | 113 | replay_episode(env, dataset_path, save_path) 114 | 115 | if __name__ == '__main__': 116 | parser = argparse.ArgumentParser() 117 | parser.add_argument('--env', action='store', type=str, help='Task name.', required=True) 118 | parser.add_argument('--dataset_dir', action='store', type=str, help='Path to the dataset directory.', required=True) 119 | parser.add_argument('--episode_idx', action='store', type=int, help='Episode index.', required=False) 120 | 121 | main(vars(parser.parse_args())) 122 | 123 | 124 | -------------------------------------------------------------------------------- /gym_guided_vision/scripts/test_env.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('../') 3 | 4 | import gym_guided_vision 5 | from gym_guided_vision.constants import SIM_DT 6 | import gymnasium as gym 7 | 8 | import os 9 | import h5py 10 | import argparse 11 | import time 12 | import numpy as np 13 | import glob 14 | from tqdm import tqdm 15 | 16 | 17 | 18 | def main(args): 19 | episode_path = args["episode_path"] 20 | env = gym.make(args["env"]) 21 | 22 | # Using tqdm to track progress over episodes 23 | with h5py.File(episode_path, 'r') as root: 24 | all_qpos = root['/observations/all_qpos'][()] 25 | 26 | 27 | paused = False 28 | 29 | def key_callback(keycode): 30 | if chr(keycode) == ' ': 31 | nonlocal paused 32 | paused = not paused 33 | 34 | env.unwrapped.create_viewer(key_callback=key_callback) 35 | 36 | i = 0 37 | while True: 38 | step_start = time.time() 39 | 40 | if not paused: 41 | # Apply the action 42 | env.unwrapped.set_qpos(all_qpos[i%len(all_qpos)]) 43 | env.unwrapped.render_viewer() 44 | i+=1 45 | 46 | 47 | # Rudimentary time keeping, will drift relative to wall clock. 48 | time_until_next_step = SIM_DT - (time.time() - step_start) 49 | time.sleep(max(0, time_until_next_step)) 50 | 51 | 52 | 53 | 54 | if __name__ == '__main__': 55 | parser = argparse.ArgumentParser() 56 | parser.add_argument('--episode_path', action='store', type=str, help='Path to the dataset directory.', required=True) 57 | parser.add_argument('--env', action='store', type=str, help='Task name.', required=True) 58 | 59 | main(vars(parser.parse_args())) 60 | 61 | 62 | -------------------------------------------------------------------------------- /gym_guided_vision/scripts/visualize_all_episodes.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('../') 3 | 4 | import os 5 | import numpy as np 6 | import cv2 7 | import h5py 8 | import argparse 9 | import glob 10 | import matplotlib.pyplot as plt 11 | from constants import REAL_DT, SIM_DT, LEFT_JOINT_NAMES, RIGHT_JOINT_NAMES, MIDDLE_JOINT_NAMES 12 | import re 13 | 14 | def load_hdf5(dataset_path): 15 | if not os.path.isfile(dataset_path): 16 | print(f'Dataset does not exist at \n{dataset_path}\n') 17 | exit() 18 | 19 | with h5py.File(dataset_path, 'r') as root: 20 | is_sim = root.attrs['sim'] 21 | qpos = root['/observations/qpos'][()] 22 | qvel = root['/observations/qvel'][()] 23 | action = root['/action'][()] 24 | image_dict = dict() 25 | for cam_name in root[f'/observations/images/'].keys(): 26 | image_dict[cam_name] = root[f'/observations/images/{cam_name}'][()] 27 | 28 | return qpos, qvel, action, image_dict 29 | 30 | def main(args): 31 | 32 | glob_path = args['glob_path'] 33 | 34 | episode_files = glob.glob(glob_path) 35 | 36 | if len(episode_files) == 0: 37 | print(f'No episodes found in {glob_path}') 38 | exit() 39 | 40 | # make sure files all in same directory 41 | dataset_dir = os.path.dirname(episode_files[0]) 42 | for episode_file in episode_files: 43 | if os.path.dirname(episode_file) != dataset_dir: 44 | print('All episodes must be in the same directory') 45 | exit() 46 | 47 | # make sure all files are .hdf5 48 | for episode_file in episode_files: 49 | if not episode_file.endswith('.hdf5'): 50 | print('All episodes must be .hdf5 files') 51 | exit() 52 | 53 | # sort episode files by episode number "episode_i.hdf5" 54 | episode_files = sorted(episode_files, key=lambda x: int(re.search(r'episode_(\d+)', x).group(1))) 55 | 56 | # make sure all episodes are present (continuously numbered) 57 | for i, episode_file in enumerate(episode_files): 58 | # check for string i in episode_file 59 | if f'episode_{i}' not in episode_file: 60 | print(f'Missing episode_{i} in {dataset_dir}') 61 | exit() 62 | 63 | # if dataset_dir starts with sim_, use SIM_DT, else use REAL_DT 64 | DT = SIM_DT if dataset_dir.startswith('sim_') else REAL_DT 65 | 66 | out = None 67 | 68 | video_path = 'all_episodes.mp4' 69 | 70 | for i in range(len(episode_files)): 71 | qpos, qvel, action, image_dict = load_hdf5(episode_files[i]) 72 | 73 | video = image_dict 74 | cam_names = list(video.keys()) 75 | all_cam_videos = [] 76 | 77 | min_h = np.inf 78 | total_w = 0 79 | for cam_name in cam_names: 80 | _, h, w, _ = video[cam_name].shape 81 | min_h = min(min_h, h) 82 | total_w += w 83 | 84 | for cam_name in cam_names: 85 | _, h, w, _ = video[cam_name].shape 86 | 87 | new_w = int(min_h * w / h) 88 | 89 | cam_video = video[cam_name] 90 | buffer = np.zeros((len(cam_video), min_h, new_w, 3), dtype=np.uint8) 91 | for ts in range(len(cam_video)): 92 | buffer[ts, :, :, :] = cv2.resize(cam_video[ts], (new_w, min_h)) 93 | all_cam_videos.append(buffer) 94 | all_cam_videos = np.concatenate(all_cam_videos, axis=2) # width dimension 95 | 96 | n_frames, h, w, _ = all_cam_videos.shape 97 | 98 | if out is None: 99 | fps = int(1 / DT / 10) 100 | out = cv2.VideoWriter(video_path, cv2.VideoWriter_fourcc(*'mp4v'), fps, (w, h)) 101 | 102 | for t in range(0, n_frames, 20): 103 | image = all_cam_videos[t] 104 | image = image[:, :, [2, 1, 0]] # swap B and R channel 105 | 106 | # write episode number on image 107 | font = cv2.FONT_HERSHEY_SIMPLEX 108 | bottomLeftCornerOfText = (10, 30) 109 | fontScale = 1 110 | fontColor = (255, 255, 255) 111 | lineType = 2 112 | image = cv2.putText(image.copy(), f'Episode {i}', bottomLeftCornerOfText, font, 113 | fontScale, fontColor, 2, cv2.LINE_AA) 114 | 115 | out.write(image) 116 | 117 | print(f'Processed episode_{i}') 118 | 119 | if out is not None: 120 | out.release() 121 | print(f'Saved video to: {video_path}') 122 | else: 123 | print('No episodes found to visualize') 124 | 125 | 126 | 127 | if __name__ == '__main__': 128 | parser = argparse.ArgumentParser() 129 | parser.add_argument('--glob_path', type=str, help='Dataset dir.', required=True) 130 | main(vars(parser.parse_args())) -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | google-cloud-firestore 2 | aiortc 3 | numba 4 | ipython 5 | scipy 6 | opencv-python 7 | h5py 8 | gymnasium 9 | mujoco 10 | dm_control 11 | --------------------------------------------------------------------------------