├── .github └── workflows │ └── python-package.yml ├── .gitignore ├── LICENSE.txt ├── README.md ├── img ├── admit_test.gif ├── force_test.gif ├── gain_test.gif ├── insertion_task.gif └── ps_move_demo.gif ├── irl_control ├── __init__.py ├── action_sequence_configs │ ├── insertion_task.yaml │ └── iros2022_task.yaml ├── device.py ├── examples │ ├── __init__.py │ ├── admit_test.py │ ├── force_test.py │ ├── gain_test.py │ ├── insertion_task.py │ ├── ps_move_example.py │ └── space_mouse_example.py ├── input_devices │ ├── ps_move.py │ └── space_mouse.py ├── meshes │ ├── Female_NIST │ │ ├── Female_NIST.xml │ │ ├── Female_NIST_001.stl │ │ ├── Female_NIST_002r.001000.stl │ │ ├── Female_NIST_002r.001001.stl │ │ ├── Female_NIST_002r.001002.stl │ │ ├── Female_NIST_002r.001003.stl │ │ ├── Female_NIST_002r.001004.stl │ │ ├── Female_NIST_002r.001005.stl │ │ ├── Female_NIST_002r.001006.stl │ │ ├── Female_NIST_002r.002000.stl │ │ ├── Female_NIST_002r.002001.stl │ │ ├── Female_NIST_002r.002002.stl │ │ ├── Female_NIST_002r.002003.stl │ │ ├── Female_NIST_002r.002004.stl │ │ ├── Female_NIST_002r.002005.stl │ │ ├── Female_NIST_002r.003000.stl │ │ ├── Female_NIST_002r.003001.stl │ │ ├── Female_NIST_002r.003002.stl │ │ ├── Female_NIST_002r.003003.stl │ │ ├── Female_NIST_002r.003004.stl │ │ ├── Female_NIST_002r.003005.stl │ │ ├── Female_NIST_002r.003006.stl │ │ ├── Female_NIST_002r.003007.stl │ │ ├── Female_NIST_002r.003008.stl │ │ ├── Female_NIST_002r.003009.stl │ │ ├── Female_NIST_002r.004000.stl │ │ ├── Female_NIST_002r.004001.stl │ │ ├── Female_NIST_002r.004002.stl │ │ ├── Female_NIST_002r.004003.stl │ │ ├── Female_NIST_002r.004004.stl │ │ ├── Female_NIST_002r.004005.stl │ │ ├── Female_NIST_002r.004006.stl │ │ ├── Female_NIST_002r.004007.stl │ │ ├── Female_NIST_002r.004008.stl │ │ ├── Female_NIST_002r.005000.stl │ │ ├── Female_NIST_002r.005001.stl │ │ ├── Female_NIST_002r.005002.stl │ │ ├── Female_NIST_002r.005003.stl │ │ ├── Female_NIST_002r.005004.stl │ │ ├── Female_NIST_002r.005005.stl │ │ ├── Female_NIST_002r.005006.stl │ │ ├── Female_NIST_002r.005007.stl │ │ ├── Female_NIST_002r.006000.stl │ │ ├── Female_NIST_002r.006001.stl │ │ ├── Female_NIST_002r.006002.stl │ │ ├── Female_NIST_002r.006003.stl │ │ ├── Female_NIST_002r.006004.stl │ │ ├── Female_NIST_002r.006005.stl │ │ ├── Female_NIST_002r.006006.stl │ │ ├── Female_NIST_002r.006007.stl │ │ ├── Female_NIST_002r.006008.stl │ │ ├── Female_NIST_002r.006009.stl │ │ ├── Female_NIST_003.stl │ │ ├── Female_NIST_004.stl │ │ └── collection.info │ ├── Male_NIST │ │ ├── Male_NIST.xml │ │ ├── Male_NIST_001.stl │ │ ├── Male_NIST_002.stl │ │ ├── Male_NIST_003.stl │ │ ├── Male_NIST_003C.stl │ │ ├── Male_NIST_004.stl │ │ ├── Male_NIST_005.stl │ │ ├── Male_NIST_005C.stl │ │ ├── Male_NIST_006.stl │ │ ├── Male_NIST_006C.stl │ │ ├── Male_NIST_007.stl │ │ ├── Male_NIST_007C.stl │ │ └── collection.info │ ├── basket │ │ ├── basket.xml │ │ ├── basket000.stl │ │ ├── basket001.stl │ │ ├── basket002.stl │ │ ├── basket003.stl │ │ └── basket004.stl │ ├── compose_xml.py │ ├── dual_pegs │ │ ├── base.stl │ │ ├── dual_pegs.xml │ │ ├── grommet_11mm.xml │ │ ├── grommet_16mm.xml │ │ ├── handle.stl │ │ ├── hole_11_01.stl │ │ ├── hole_11_02.stl │ │ ├── hole_11_03.stl │ │ ├── hole_11_04.stl │ │ ├── hole_11_05.stl │ │ ├── hole_11_06.stl │ │ ├── hole_11_07.stl │ │ ├── hole_11_08.stl │ │ ├── hole_11_09.stl │ │ ├── hole_11_10.stl │ │ ├── hole_11_11.stl │ │ ├── hole_11_12.stl │ │ ├── hole_11_13.stl │ │ ├── hole_11_14.stl │ │ ├── hole_11_15.stl │ │ ├── hole_11_16.stl │ │ ├── hole_11_17.stl │ │ ├── hole_11_18.stl │ │ ├── hole_11_19.stl │ │ ├── hole_11_20.stl │ │ ├── hole_11_21.stl │ │ ├── hole_11_22.stl │ │ ├── hole_11_23.stl │ │ ├── hole_11_24.stl │ │ ├── hole_11_25.stl │ │ ├── hole_11_26.stl │ │ ├── hole_11_27.stl │ │ ├── hole_11_28.stl │ │ ├── hole_11_29.stl │ │ ├── hole_11_30.stl │ │ ├── hole_11_31.stl │ │ ├── hole_11_32.stl │ │ ├── hole_11_33.stl │ │ ├── hole_16_01.stl │ │ ├── hole_16_02.stl │ │ ├── hole_16_03.stl │ │ ├── hole_16_04.stl │ │ ├── hole_16_05.stl │ │ ├── hole_16_06.stl │ │ ├── hole_16_07.stl │ │ ├── hole_16_08.stl │ │ ├── hole_16_09.stl │ │ ├── hole_16_10.stl │ │ ├── hole_16_11.stl │ │ ├── hole_16_12.stl │ │ ├── hole_16_13.stl │ │ ├── hole_16_14.stl │ │ ├── hole_16_15.stl │ │ ├── hole_16_16.stl │ │ ├── hole_16_17.stl │ │ ├── hole_16_18.stl │ │ ├── hole_16_19.stl │ │ ├── hole_16_20.stl │ │ ├── hole_16_21.stl │ │ ├── hole_16_22.stl │ │ ├── hole_16_23.stl │ │ ├── hole_16_24.stl │ │ ├── hole_16_25.stl │ │ ├── hole_16_26.stl │ │ ├── hole_16_27.stl │ │ ├── hole_16_28.stl │ │ ├── hole_16_29.stl │ │ ├── hole_16_30.stl │ │ ├── hole_16_31.stl │ │ ├── hole_16_32.stl │ │ ├── hole_16_33.stl │ │ ├── pin_a.stl │ │ ├── pin_b.stl │ │ ├── stand_a.stl │ │ └── stand_b.stl │ ├── dual_ur_stand │ │ ├── dual_ur_stand.blend │ │ ├── dual_ur_stand.blend1 │ │ ├── dual_ur_stand.stl │ │ ├── dual_ur_stand.xml │ │ ├── dual_ur_stand2.stl │ │ └── dual_ur_stand_ring.stl │ ├── pegs │ │ ├── Waterproof_Female.stl │ │ ├── Waterproof_Female_part_1.stl │ │ ├── Waterproof_Female_part_2.stl │ │ ├── Waterproof_Male.stl │ │ ├── back.stl │ │ ├── bracket_11.stl │ │ ├── bracket_16.stl │ │ ├── bracket_21.stl │ │ ├── dual_pegs.stl │ │ ├── front.stl │ │ ├── left.stl │ │ ├── nist.xml │ │ └── right.stl │ ├── quad_peg │ │ ├── BaseAndHandles.blend │ │ ├── BaseAndHandles.blend1 │ │ ├── handle.stl │ │ ├── quad_base.stl │ │ ├── quad_bracket.xml │ │ ├── quad_peg_base.stl │ │ └── quad_pegs.xml │ ├── robotiq_85_gripper │ │ ├── robotiq_arg2f_85_base_link.stl │ │ ├── robotiq_arg2f_85_inner_finger.stl │ │ ├── robotiq_arg2f_85_inner_finger_vis.stl │ │ ├── robotiq_arg2f_85_inner_knuckle.stl │ │ ├── robotiq_arg2f_85_inner_knuckle_vis.stl │ │ ├── robotiq_arg2f_85_outer_finger.stl │ │ ├── robotiq_arg2f_85_outer_finger_vis.stl │ │ ├── robotiq_arg2f_85_outer_knuckle.stl │ │ ├── robotiq_arg2f_85_outer_knuckle_vis.stl │ │ ├── robotiq_arg2f_85_pad_vis.stl │ │ ├── robotiq_arg2f_base_link.stl │ │ ├── robotiq_gripper_85.xml │ │ └── robotiq_gripper_coupling_vis.stl │ ├── ur5 │ │ ├── floor_tile.png │ │ ├── link0.stl │ │ ├── link1.stl │ │ ├── link1_cap.stl │ │ ├── link1_connector.stl │ │ ├── link2.stl │ │ ├── link2_cap.stl │ │ ├── link2_connector.stl │ │ ├── link2_tube.stl │ │ ├── link3_cap.stl │ │ ├── link3_tube.stl │ │ ├── link3a.stl │ │ ├── link3a_connector.stl │ │ ├── link3b.stl │ │ ├── link3b_connector.stl │ │ ├── link3b_connector2.stl │ │ ├── link4.stl │ │ ├── link4_cap.stl │ │ ├── link4_connector.stl │ │ ├── link5.stl │ │ ├── link5_cap.stl │ │ ├── link5_connector.stl │ │ ├── link6.stl │ │ ├── link6_connector.stl │ │ └── ur5.skp │ └── wall │ │ ├── .DS_Store │ │ ├── wall.xml │ │ └── wallv3.stl ├── mujoco_app.py ├── osc.py ├── robot.py ├── robot_configs │ ├── default_xyz.yaml │ ├── default_xyz_abg.yaml │ └── iros2022.yaml ├── scenes │ ├── admit_test_scene.xml │ ├── dual_ur5.xml │ ├── dual_ur5_grip_pos_ctrl.xml │ ├── force_test_scene.xml │ ├── gain_test_scene.xml │ ├── insertion_task_scene.xml │ ├── iros2022.xml │ ├── ps_move_scene.xml │ ├── space_mouse_scene.xml │ ├── ur5.xml │ └── world.xml ├── tests │ └── run_tests.py ├── utils.py └── version.py ├── requirements.in └── setup.py /.github/workflows/python-package.yml: -------------------------------------------------------------------------------- 1 | # This workflow will install Python dependencies, run tests and lint with a variety of Python versions 2 | # For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python 3 | 4 | name: Python package 5 | 6 | on: 7 | push: 8 | branches: [ "master" ] 9 | pull_request: 10 | branches: [ "master" ] 11 | 12 | jobs: 13 | build: 14 | 15 | runs-on: ubuntu-latest 16 | strategy: 17 | fail-fast: false 18 | matrix: 19 | python-version: ["3.8"] 20 | 21 | steps: 22 | - uses: actions/checkout@v3 23 | - name: Set up Python ${{ matrix.python-version }} 24 | uses: actions/setup-python@v3 25 | with: 26 | python-version: ${{ matrix.python-version }} 27 | - name: Install dependencies 28 | run: | 29 | python -m pip install --upgrade pip 30 | # python -m pip install pip-tools 31 | # pip-compile -o requirements.txt requirements.in 32 | # if [ -f requirements.txt ]; then pip install -r requirements.txt; fi 33 | - name: Run unit tests 34 | run: | 35 | python irl_control/tests/run_tests.py 36 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Data files 10 | *.csv 11 | 12 | # Distribution / packaging 13 | .Python 14 | build/ 15 | develop-eggs/ 16 | dist/ 17 | downloads/ 18 | eggs/ 19 | .eggs/ 20 | lib/ 21 | lib64/ 22 | parts/ 23 | sdist/ 24 | var/ 25 | wheels/ 26 | share/python-wheels/ 27 | *.egg-info/ 28 | .installed.cfg 29 | *.egg 30 | MANIFEST 31 | 32 | # PyInstaller 33 | # Usually these files are written by a python script from a template 34 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 35 | *.manifest 36 | *.spec 37 | 38 | # Installer logs 39 | pip-log.txt 40 | pip-delete-this-directory.txt 41 | 42 | # Unit test / coverage reports 43 | htmlcov/ 44 | .tox/ 45 | .nox/ 46 | .coverage 47 | .coverage.* 48 | .cache 49 | nosetests.xml 50 | coverage.xml 51 | *.cover 52 | *.py,cover 53 | .hypothesis/ 54 | .pytest_cache/ 55 | cover/ 56 | 57 | # Translations 58 | *.mo 59 | *.pot 60 | 61 | # Django stuff: 62 | *.log 63 | local_settings.py 64 | db.sqlite3 65 | db.sqlite3-journal 66 | 67 | # Flask stuff: 68 | instance/ 69 | .webassets-cache 70 | 71 | # Scrapy stuff: 72 | .scrapy 73 | 74 | # Sphinx documentation 75 | docs/_build/ 76 | 77 | # PyBuilder 78 | .pybuilder/ 79 | target/ 80 | 81 | # Jupyter Notebook 82 | .ipynb_checkpoints 83 | 84 | # IPython 85 | profile_default/ 86 | ipython_config.py 87 | 88 | # pyenv 89 | # For a library or package, you might want to ignore these files since the code is 90 | # intended to run in multiple environments; otherwise, check them in: 91 | # .python-version 92 | 93 | # pipenv 94 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 95 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 96 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 97 | # install all needed dependencies. 98 | #Pipfile.lock 99 | 100 | # poetry 101 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 102 | # This is especially recommended for binary packages to ensure reproducibility, and is more 103 | # commonly ignored for libraries. 104 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 105 | #poetry.lock 106 | 107 | # pdm 108 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 109 | #pdm.lock 110 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 111 | # in version control. 112 | # https://pdm.fming.dev/#use-with-ide 113 | .pdm.toml 114 | 115 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 116 | __pypackages__/ 117 | 118 | # Celery stuff 119 | celerybeat-schedule 120 | celerybeat.pid 121 | 122 | # SageMath parsed files 123 | *.sage.py 124 | 125 | # Environments 126 | .env 127 | .venv 128 | env/ 129 | venv/ 130 | ENV/ 131 | env.bak/ 132 | venv.bak/ 133 | 134 | # Spyder project settings 135 | .spyderproject 136 | .spyproject 137 | 138 | # Rope project settings 139 | .ropeproject 140 | 141 | # mkdocs documentation 142 | /site 143 | 144 | # mypy 145 | .mypy_cache/ 146 | .dmypy.json 147 | dmypy.json 148 | 149 | # Pyre type checker 150 | .pyre/ 151 | 152 | # pytype static type analyzer 153 | .pytype/ 154 | 155 | # Cython debug symbols 156 | cython_debug/ 157 | 158 | # PyCharm 159 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 160 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 161 | # and can be added to the global gitignore or merged into this file. For a more nuclear 162 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 163 | #.idea/ 164 | # 165 | .DS_Store 166 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Interactive Robotics Lab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## IRL Control 2 | 3 | IRL Control is a simulation and control framework for bimanual robot manipulation developed at the Interactive Robotics Lab at Arizona State University. The framework provides a variety of features and algorithms for implementing and learning of bimanual control policies. In particular, it provides easy access to functions for collecting training data from input devices, generating low-level robot control signals, for visualization and debugging, as well as the evaluation of generated robot controllers. IRL Control uses MuJoCo as a simulation backbone to generate physically-correct simulations of the interactions between the robot, its environment, and manipulated objects. 4 | 5 | ***Note***: This repo is no longer actively maintained. For a newer version of irl_control (which uses the official mujoco API instead of mujoco-py), check out https://github.com/ir-lab/bimanual-imitation. 6 | 7 | ## Capabilities 8 | - Operational Space Control 9 | - Admittance Control for handling external forces to the robot 10 | - Rapid development of demos with a dual-arm UR5 robot through inheritance 11 | - Support for PS Move controllers and 3D Connexion Space Mouse to teleoperate the robot arms 12 | - Supports easy addition of controlled devices to the scene (handles Jacobians, forces, joint states, etc) 13 | - YAML configuration files for setting the PID Gains, Min/Max velocities, and kinematic descriptions of the robot devices 14 | - NIST Male/Female adapters with low-tolerance for performing insertion tasks 15 | 16 | ## Examples 17 | 18 | ### Insertion Task 19 | 22 | In [this example](irl_control/examples/insertion_task.py), the Dual UR5 Robot inserts adapters into their corresponding slot, with each component spawned at a randomly generated angle and position. The robot must precisely align and insert the components under small tolerance contraints. Here, the robot executes a sequence of actions defined by the [insertion action sequence](irl_control/action_sequence_configs/insertion_task.yaml). This action sequence defines a generic algorithm that can be executed on either arm, as well as: how much force the gripper should apply at each stage of the task, the velocity constraints, and the objects involved in the action (containing information on the proper location and orientation for picking/inserting). 23 | 24 | drawing 25 | 26 | ### PS Move Teleoperation 27 | In [this example](irl_control/examples/ps_move_example.py), the user teleoperates the robot for picking up objects in the scene. The PS Move controllers allow for opening and closing the gripper and controlling the desired position of the end effector, which the robot moves to when the trigger is pressed. The controllers have a one-to-one correspondance with each robot arm, so that it is possible to extend this example to more complex (and jointly controlled) bi-manual manipulation tasks. 28 | 29 | drawing 30 | 31 | ### PID Gain Test 32 | The objective for [this example](irl_control/examples/gain_test.py) is to evaluate PID gains to ensure that the base stand and a desired arm (the arm on the left in this case) are stable under high torques. It can be seen that the arm on the right is vigorously moving back-and-forth in order to apply a high torque to the rest of the robot's body. Notably, the base joint and left arm are stable under these forces, reinforcing the notion that the chosen gains are adequate for tasks which require an arm to be steady under high external forces. 33 | 34 | drawing 35 | 36 | ### Admittance Test 37 | [This example](irl_control/examples/admit_test.py) demonstrates the Admittance Controller on the DualUR5 robot. In this example, both of the arms are given a fixed position and an external force is applied to the left arm end-effector. The arm acts as a spring-damper system whose stifness and damping can be changed by modifying the Kp and Kd parameters in the controller config file. 38 | 39 | drawing 40 | 41 | ### Force Test 42 | In [this example](irl_control/examples/force_test.py) used to demonstrate how the admittance control react to the external environment, here the target position of the left arm is given such that the hit the wall, we can see that the arm bounces of the wall insteading of pushing against the wall with large force. 43 | 44 | drawing 45 | 46 | We'd like to thank/reference ABR control (https://github.com/abr/abr_control) for the operational space controller design and helpful guide on interacting with mujoco-py! 47 | -------------------------------------------------------------------------------- /img/admit_test.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/img/admit_test.gif -------------------------------------------------------------------------------- /img/force_test.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/img/force_test.gif -------------------------------------------------------------------------------- /img/gain_test.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/img/gain_test.gif -------------------------------------------------------------------------------- /img/insertion_task.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/img/insertion_task.gif -------------------------------------------------------------------------------- /img/ps_move_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/img/ps_move_demo.gif -------------------------------------------------------------------------------- /irl_control/__init__.py: -------------------------------------------------------------------------------- 1 | from .version import version as __version__ 2 | from irl_control.device import Device 3 | from irl_control.robot import Robot 4 | from irl_control.osc import OSC 5 | from irl_control.mujoco_app import MujocoApp -------------------------------------------------------------------------------- /irl_control/action_sequence_configs/insertion_task.yaml: -------------------------------------------------------------------------------- 1 | grommet_action_objects: 2 | male_object: 3 | joint_name: "free_joint_grommet_11mm" 4 | hover_offset: [0, 0, 0.2] 5 | grip_offset: [0, 0, 0.158] 6 | initial_pos_xyz: [-0.2, 0.5, 0.005] 7 | initial_pos_abg: [0, 0, 20] 8 | grip_yaw: 90 9 | female_object: 10 | joint_name: "free_joint_dual_peg" 11 | hover_offset: [0, 0, 0.23] 12 | hover_offset_lift: [0, 0, 0.25] 13 | insert_offset: [0, 0, 0.18] 14 | initial_pos_xyz: [-0.4, 0.7, 0.0] 15 | initial_pos_abg: [0, 0, 30] 16 | grip_yaw: 90 17 | 18 | nist_action_objects: 19 | male_object: 20 | joint_name: "free_joint_male" 21 | hover_offset: [0, 0, 0.32] 22 | grip_offset: [0, 0, 0.24] 23 | initial_pos_xyz: [0.4, 0.6, -0.0515] 24 | initial_pos_abg: [0, 0, 30] 25 | grip_yaw: 90 26 | female_object: 27 | joint_name: "free_joint_female" 28 | hover_offset: [0, 0, 0.27] 29 | hover_offset_lift: [0, 0, 0.3] 30 | insert_offset: [0, 0, 0.22] 31 | initial_pos_xyz: [0.7, 0.4, -0.00002] 32 | initial_pos_abg: [0, 0, -30] 33 | grip_yaw: 90 34 | 35 | insertion_action_sequence: 36 | # Hover above the male object 37 | - action: WP 38 | target_xyz: male_object 39 | target_abg: male_object 40 | offset: hover_offset 41 | # Open gripper for 1 second 42 | - action: GRIP 43 | gripper_force: -0.08 44 | gripper_duration: 1.0 45 | # Position gripper around the male object 46 | - action: WP 47 | target_xyz: male_object 48 | target_abg: male_object 49 | offset: grip_offset 50 | gripper_force: -0.08 51 | # Close the gripper (Grab the object) 52 | - action: GRIP 53 | gripper_force: 0.2 54 | gripper_duration: 2.0 55 | # Lift the arm vertically 56 | - action: WP 57 | target_xyz: male_object 58 | target_abg: male_object 59 | gripper_force: 0.2 60 | offset: hover_offset 61 | # Move the object to the female (Hover) 62 | - action: WP 63 | max_speed_xyz: 0.3 64 | target_xyz: female_object 65 | target_abg: male_object 66 | gripper_force: 0.2 67 | offset: hover_offset 68 | # Move the object to the female (Hover) 69 | - action: WP 70 | max_speed_xyz: 0.1 71 | target_xyz: female_object 72 | target_abg: female_object 73 | gripper_force: 0.2 74 | offset: hover_offset 75 | # Wait a moment before inserting the male 76 | - action: GRIP 77 | gripper_force: 0.2 78 | gripper_duration: 1.0 79 | # Lower the object into the female 80 | - action: WP 81 | target_xyz: female_object 82 | target_abg: female_object 83 | max_speed_xyz: 1.0 84 | gripper_force: 0.1 85 | max_error: 0.01 86 | offset: insert_offset 87 | # Release the gripper 88 | - action: GRIP 89 | gripper_force: -0.1 90 | gripper_duration: 2.0 91 | # Lift the arm vertically 92 | - action: WP 93 | target_xyz: female_object 94 | target_abg: female_object 95 | gripper_force: -0.1 96 | max_speed_xyz: 0.1 97 | offset: hover_offset_lift 98 | # Return to start_pos 99 | - action: WP 100 | target_xyz: start_pos 101 | target_abg: female_object 102 | max_speed_xyz: 2.0 103 | gripper_force: 0 104 | max_error: 0.05 -------------------------------------------------------------------------------- /irl_control/action_sequence_configs/iros2022_task.yaml: -------------------------------------------------------------------------------- 1 | device_config: 2 | devices: ["base", "ur5left", "ur5right"] 3 | controllers: ["osc0", "osc2", "osc2"] 4 | control_type: ["joint", "task", "task"] 5 | 6 | iros2022_action_objects: 7 | male_object: 8 | joint_name: "free_joint_quad_grommet" 9 | hover_offset: [[0,0,0], [0.33, 0.26, 0.3], [0.59, -0.16, 0.3]] 10 | grip_offset: [[0,0,0], [0.33, 0.26, 0.19], [0.59, -0.16, 0.19]] 11 | initial_pos_xyz: [0.5, 0.0, 0.0] 12 | initial_pos_quat: [1.0, 0, 0, 0.3] 13 | grip_yaw: 90 14 | female_object: 15 | joint_name: "free_joint_quad_peg" 16 | hover_offset: [[0,0,0], [-0.2, 0.5, 0.3], [0.3, 0.5, 0.3]] 17 | initial_pos_xyz: [0.1, 0.6, 0.0] 18 | initial_pos_quat: [1.0, 0, 0, 1.0] 19 | grip_yaw: 90 20 | 21 | iros2022_pickup_sequence: 22 | # Hover above the male object 23 | - action: WP 24 | target_xyz: [[0,0,0], [0.375, 0.3, 0.35], [0.625, -0.3, 0.35]] 25 | target_quat: [[0.707, 0, 0, -0.707], [0.707, 0.2, -0.707, 0.2], [0.707, 0.2, -0.707, 0.2]] 26 | name: Initialization 27 | max_error: 0.08 28 | # Open gripper for 1 second 29 | - action: GRIP 30 | gripper_force: -0.1 31 | gripper_duration: 0.1 32 | name: Open-Grippers 33 | # Go above the handles 34 | - action: WP 35 | target_xyz: male_object.hover_offset 36 | target_quat: [[0.8660254, 0, 0, -0.5], [0.707, 0.2, -0.707, 0.2], [0.707, 0.2, -0.707, 0.2]] 37 | name: Pre-Grasp-Hover 38 | max_error: 0.08 39 | # Go above the handles 40 | - action: WP 41 | target_xyz: male_object.grip_offset 42 | target_quat: [[0.8660254, 0, 0, -0.5], [0.707, 0.2, -0.707, 0.2], [0.707, 0.2, -0.707, 0.2]] 43 | name: Pre-Grasp 44 | max_error: 0.04 45 | # noise: [0.0, 0.0025] 46 | # Open gripper for 1 second 47 | - action: GRIP 48 | gripper_force: 0.05 49 | gripper_duration: 2.0 50 | name: Close-Grippers 51 | # Open gripper for 1 second 52 | - action: GRIP 53 | gripper_force: 0.2 54 | gripper_duration: 0.1 55 | name: Force-Grippers 56 | # Go above the handles 57 | - action: WP 58 | target_xyz: male_object.hover_offset 59 | target_quat: [[0.8660254, 0, 0, -0.5], [0.707, 0.2, -0.707, 0.2], [0.707, 0.2, -0.707, 0.2]] 60 | name: Lift-Object 61 | max_error: 0.08 62 | noise: [0.0, 0.005] 63 | 64 | iros2022_demo_sequence: 65 | # Hover above the female object 66 | - action: INTERP 67 | target_xyz: [[0,0,0], [-0.225, 0.59, 0.3], [0.275, 0.59, 0.3]] 68 | target_quat: [[1.0, 0, 0, 0], [0.707, 0.707, -0.707, 0.707], [0.707, 0.707, -0.707, 0.707]] 69 | name: Transfer 70 | max_error: 0.2 71 | steps: 100 72 | noise: [0.0, 0.001] 73 | # Make sure the last waypoint is reacht closely 74 | - action: WP 75 | target_xyz: [[0,0,0], [-0.225, 0.59, 0.3], [0.275, 0.59, 0.3]] 76 | target_quat: [[1.0, 0, 0, 0], [0.707, 0.707, -0.707, 0.707], [0.707, 0.707, -0.707, 0.707]] 77 | name: Pre-Insert-Hover 78 | max_error: 0.04 79 | noise: [0.0, 0.001] 80 | # Insert 81 | - action: INTERP 82 | target_xyz: [[0,0,0], [-0.225, 0.61, 0.23], [0.275, 0.59, 0.23]] 83 | target_quat: [[1.0, 0, 0, 0], [0.707, 0.707, -0.707, 0.707], [0.707, 0.707, -0.707, 0.707]] 84 | name: Insertion 85 | max_error: 0.05 86 | steps: 20 87 | noise: [0.0, 0.001] 88 | 89 | iros2022_release_sequence: 90 | # Open gripper for 1 second 91 | - action: GRIP 92 | gripper_force: -0.1 93 | gripper_duration: 2.0 94 | name: Open-Grippers 95 | # Hover Over 96 | - action: WP 97 | target_xyz: [[0,0,0], [-0.2, 0.6, 0.3], [0.3, 0.6, 0.3]] 98 | target_quat: [[1.0, 0, 0, 0], [0.707, 0.707, -0.707, 0.707], [0.707, 0.707, -0.707, 0.707]] 99 | name: Dummy 100 | max_error: 0.05 -------------------------------------------------------------------------------- /irl_control/device.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from threading import Lock 3 | from typing import Dict, Any 4 | from enum import Enum 5 | import copy 6 | 7 | class DeviceState(Enum): 8 | Q = 'Q' 9 | Q_ACTUATED = 'Q_ACTUATED' 10 | DQ = 'DQ' 11 | DQ_ACTUATED = 'DQ_ACTUATED' 12 | DDQ = 'DDQ' 13 | EE_XYZ = 'EE_XYZ' 14 | EE_XYZ_VEL = 'EE_XYZ_VEL' 15 | EE_QUAT = 'EE_QUAT' 16 | FORCE = 'FORCE' 17 | TORQUE = 'TORQUE' 18 | J = 'JACOBIAN' 19 | 20 | class Device(): 21 | """ 22 | The Device class encapsulates the device parameters specified in the yaml file 23 | that is passed to MujocoApp. It collects data from the simulator, obtaining the 24 | desired device states. 25 | """ 26 | def __init__(self, device_yml: Dict, model, sim, use_sim: bool): 27 | self.sim = sim 28 | self.__use_sim = use_sim 29 | # Assign all of the yaml parameters 30 | self.name = device_yml['name'] 31 | self.max_vel = device_yml.get('max_vel') 32 | #self.max_vel = device_yml['max_vel'] 33 | self.EE = device_yml['EE'] 34 | self.ctrlr_dof_xyz = device_yml['ctrlr_dof_xyz'] 35 | self.ctrlr_dof_abg = device_yml['ctrlr_dof_abg'] 36 | self.ctrlr_dof = np.hstack([self.ctrlr_dof_xyz, self.ctrlr_dof_abg]) 37 | self.start_angles = np.array(device_yml['start_angles']) 38 | self.num_gripper_joints = device_yml['num_gripper_joints'] 39 | 40 | # Check if the user specifies a start body for the while loop to terminte at 41 | try: 42 | start_body = model.body_name2id(device_yml['start_body']) 43 | except: 44 | start_body = 0 45 | 46 | # Reference: ABR Control 47 | # Get the joint ids, using the specified EE / start body 48 | # start with the end-effector (EE) and work back to the world body 49 | body_id = model.body_name2id(self.EE) 50 | joint_ids = [] 51 | joint_names = [] 52 | while model.body_parentid[body_id] != 0 and model.body_parentid[body_id] != start_body: 53 | jntadrs_start = model.body_jntadr[body_id] 54 | tmp_ids = [] 55 | tmp_names = [] 56 | for ii in range(model.body_jntnum[body_id]): 57 | tmp_ids.append(jntadrs_start + ii) 58 | tmp_names.append(model.joint_id2name(tmp_ids[-1])) 59 | joint_ids += tmp_ids[::-1] 60 | joint_names += tmp_names[::-1] 61 | body_id = model.body_parentid[body_id] 62 | # flip the list so it starts with the base of the arm / first joint 63 | self.joint_names = joint_names[::-1] 64 | self.joint_ids = np.array(joint_ids[::-1]) 65 | 66 | gripper_start_idx = self.joint_ids[-1] + 1 67 | self.gripper_ids = np.arange(gripper_start_idx, 68 | gripper_start_idx + self.num_gripper_joints) 69 | self.joint_ids_all = np.hstack([self.joint_ids, self.gripper_ids]) 70 | 71 | # Find the actuator and control indices 72 | actuator_trnids = model.actuator_trnid[:,0] 73 | self.ctrl_idxs = np.intersect1d(actuator_trnids, self.joint_ids_all, return_indices=True)[1] 74 | self.actuator_trnids = actuator_trnids[self.ctrl_idxs] 75 | 76 | if self.name == "ur5right" or self.name == "ur5left": 77 | self.sim.data.qpos[self.joint_ids] = np.copy(self.start_angles) 78 | elif self.name == "base": 79 | self.sim.data.qpos[self.joint_ids] = np.copy(self.start_angles) 80 | self.sim.forward() 81 | 82 | # Check that the 83 | if np.sum(np.hstack([self.ctrlr_dof_xyz, self.ctrlr_dof_abg])) > len(self.joint_ids): 84 | print("Fewer DOF than specified") 85 | 86 | # Initialize dicts to keep track of the state variables and locks 87 | self.__state_var_map: Dict[DeviceState, function] = { 88 | DeviceState.Q : lambda : self.sim.data.qpos[self.joint_ids_all], 89 | DeviceState.Q_ACTUATED : lambda : self.sim.data.qpos[self.joint_ids], 90 | DeviceState.DQ : lambda : self.sim.data.qvel[self.joint_ids_all], 91 | DeviceState.DQ_ACTUATED : lambda : self.sim.data.qvel[self.joint_ids], 92 | DeviceState.DDQ : lambda : self.sim.data.qacc[self.joint_ids_all], 93 | DeviceState.EE_XYZ : lambda : self.sim.data.get_body_xpos(self.EE), 94 | DeviceState.EE_XYZ_VEL : lambda : self.sim.data.get_body_xvelp(self.EE), 95 | DeviceState.EE_QUAT : lambda : self.sim.data.get_body_xquat(self.EE), 96 | DeviceState.FORCE : lambda : self.__get_force(), 97 | DeviceState.TORQUE : lambda : self.__get_torque(), 98 | DeviceState.J : lambda : self.__get_jacobian() 99 | } 100 | 101 | self.__state: Dict[DeviceState, Any] = dict() 102 | self.__state_locks: Dict[DeviceState, Lock] = dict([(key, Lock()) for key in DeviceState]) 103 | 104 | # These are the that keys we should use when returning data from get_all_states() 105 | self.concise_state_vars = [ 106 | DeviceState.Q_ACTUATED, 107 | DeviceState.DQ_ACTUATED, 108 | DeviceState.EE_XYZ, 109 | DeviceState.EE_XYZ_VEL, 110 | DeviceState.EE_QUAT, 111 | DeviceState.FORCE, 112 | DeviceState.TORQUE 113 | ] 114 | 115 | def __get_jacobian(self, full=False): 116 | """ 117 | NOTE: Returns either: 118 | 1) The full jacobian (of the Device, using its EE), if full==True 119 | 2) The full jacobian evaluated at the controlled DoF, if full==False 120 | The parameter, full=False, is added in case we decide for the get methods 121 | to take in arguments (currently not supported). 122 | """ 123 | J = np.array([]) 124 | # Get the jacobian for the x,y,z components 125 | J = self.sim.data.get_body_jacp(self.EE) 126 | J = J.reshape(3, -1) 127 | # Get the jacobian for the a,b,g components 128 | Jr = self.sim.data.get_body_jacr(self.EE) 129 | Jr = Jr.reshape(3, -1) 130 | J = np.vstack([J, Jr]) if J.size else Jr 131 | if full == False: 132 | J = J[self.ctrlr_dof] 133 | return J 134 | 135 | def __get_R(self): 136 | """ 137 | Get rotation matrix for device's ft_frame 138 | """ 139 | if self.name == "ur5right": 140 | return self.sim.data.get_site_xmat("ft_frame_ur5right") 141 | if self.name == "ur5left": 142 | return self.sim.data.get_site_xmat("ft_frame_ur5left") 143 | 144 | def __get_force(self): 145 | """ 146 | Get the external forces, used (for admittance control) acting upon 147 | the gripper sensors 148 | """ 149 | if self.name == "ur5right": 150 | force = np.matmul(self.__get_R(), self.sim.data.sensordata[0:3]) 151 | return force 152 | if self.name == "ur5left": 153 | force = np.matmul(self.__get_R(), self.sim.data.sensordata[6:9]) 154 | return force 155 | else: 156 | return np.zeros(3) 157 | 158 | def __get_torque(self): 159 | """ 160 | Get the external torques, used (for admittance control) acting upon 161 | the gripper sensors 162 | """ 163 | if self.name == "ur5right": 164 | force = np.matmul(self.__get_R(), self.sim.data.sensordata[3:6]) 165 | return force 166 | if self.name == "ur5left": 167 | force = np.matmul(self.__get_R(), self.sim.data.sensordata[9:12]) 168 | return force 169 | else: 170 | return np.zeros(3) 171 | 172 | def __set_state(self, state_var: DeviceState): 173 | """ 174 | Set the state of the device corresponding to the key value (if exists) 175 | """ 176 | assert self.__use_sim is False 177 | self.__state_locks[state_var].acquire() 178 | var_func = self.__state_var_map[state_var] 179 | var_value = var_func() 180 | self.__state[state_var] = copy.copy(var_value) # Make sure to copy (or else reference will stick to Dict value) 181 | self.__state_locks[state_var].release() 182 | 183 | def get_state(self, state_var: DeviceState): 184 | """ 185 | Get the state of the device corresponding to the key value (if exists) 186 | """ 187 | if self.__use_sim: 188 | func = self.__state_var_map[state_var] 189 | state = copy.copy(func()) 190 | else: 191 | self.__state_locks[state_var].acquire() 192 | state = copy.copy(self.__state[state_var]) 193 | self.__state_locks[state_var].release() 194 | return state 195 | 196 | def get_all_states(self): 197 | return dict([(key, self.get_state(key)) for key in self.concise_state_vars]) 198 | 199 | def update_state(self): 200 | """ 201 | This should running in a thread: Robot.start() 202 | """ 203 | assert self.__use_sim is False 204 | for var in DeviceState: 205 | self.__set_state(var) 206 | 207 | def get_all_joint_ids(self): 208 | return self.joint_ids_all 209 | 210 | def get_actuator_joint_ids(self): 211 | return self.joint_ids 212 | 213 | def get_gripper_joint_ids(self): 214 | return self.gripper_ids -------------------------------------------------------------------------------- /irl_control/examples/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/examples/__init__.py -------------------------------------------------------------------------------- /irl_control/examples/admit_test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from mujoco_py import MjViewer, functions 3 | import threading 4 | from typing import Dict, Tuple 5 | from irl_control import OSC, MujocoApp 6 | from irl_control.utils import Target 7 | 8 | class AdmitTest(MujocoApp): 9 | """ 10 | This class implements the Admittance Controller on Dual UR5 robot 11 | """ 12 | def __init__(self, robot_config_file: str = None, scene_file: str = None, active_arm: str = 'right'): 13 | # Initialize the Parent class with the config file 14 | super().__init__(robot_config_file, scene_file) 15 | # Specify the robot in the scene that we'd like to use 16 | self.robot = self.get_robot(robot_name="DualUR5") 17 | # Specify the controller configuations that should be used for 18 | # the corresponding devices 19 | admit_device_configs = [ 20 | ('ur5right', self.get_controller_config('osc2')), 21 | ('ur5left', self.get_controller_config('osc2')) 22 | ] 23 | # Get the configuration for the nullspace controller 24 | nullspace_config = self.get_controller_config('nullspace') 25 | self.controller = OSC(self.robot, self.sim, admit_device_configs, nullspace_config,admittance = True) 26 | 27 | # self.robot_data_thread = threading.Thread(target=self.robot.start) 28 | # self.robot_data_thread.start() 29 | 30 | # Keep track of device target errors 31 | self.errors = dict() 32 | self.errors['ur5right'] = 0 33 | self.viewer = MjViewer(self.sim) 34 | 35 | def gen_target(self) -> Tuple[np.ndarray, np.ndarray]: #Generates the target position for both arms 36 | """ 37 | Generates the target position for both arms 38 | """ 39 | right_wp = np.array([0.3, 0.46432, 0.5]) 40 | left_wp = np.array([-0.3, 0.46432, 0.5]) 41 | return (right_wp, left_wp) 42 | 43 | def run(self): 44 | """ 45 | This is the main function that gets called. Uses the 46 | Admittance Controller to control the DualUR5 47 | and adjust to the external forces acting on the end effector. 48 | """ 49 | #start the count and timer 50 | count = 0 51 | time_thread = threading.Thread(target=self.sleep_for, args=(50,)) 52 | time_thread.start() 53 | body_id = self.sim.model.body_name2id("left_outer_knuckle_ur5left") 54 | #Define targets for both arms 55 | targets: Dict[str, Target] = { 56 | 'ur5right' : Target(), 57 | 'ur5left' : Target(), 58 | } 59 | #Get the targets for both arms 60 | right_wp, left_wp = self.gen_target() 61 | while self.timer_running: 62 | #set the targets position and orientation of both arms 63 | count += 1 64 | targets['ur5right'].set_xyz(right_wp) 65 | targets['ur5left'].set_xyz(left_wp) 66 | targets['ur5left'].set_abg(np.array([0,-1*np.pi/2,0])) 67 | #set the mocap position to target position 68 | self.sim.data.set_mocap_pos('target_red', right_wp) 69 | self.sim.data.set_mocap_pos('target_blue', left_wp) 70 | #Genetrate the admittance control output 71 | ctrlr_output = self.controller.generate(targets) 72 | for force_idx, force in zip(*ctrlr_output): 73 | self.sim.data.ctrl[force_idx] = force 74 | #Apply external force on left end effector 75 | self.sim.data.xfrc_applied[body_id] = [0,0,0,0,0,0] 76 | if count > 3000 and count < 5000: 77 | self.sim.data.xfrc_applied[body_id] = [20,0,0,0,0,0] 78 | self.sim.step() 79 | self.viewer.render() 80 | time_thread.join() 81 | # self.robot_data_thread.join() 82 | glfw.destroy_window(self.viewer.window) 83 | 84 | if __name__ == "__main__": 85 | ur5 = AdmitTest(robot_config_file="default_xyz_abg.yaml", scene_file="admit_test_scene.xml") 86 | ur5.run() -------------------------------------------------------------------------------- /irl_control/examples/force_test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from mujoco_py import MjViewer, functions 3 | import threading 4 | from typing import Dict, Tuple 5 | from irl_control import OSC, MujocoApp 6 | from irl_control.utils import Target 7 | from irl_control.device import DeviceState 8 | import csv 9 | 10 | class ForceTest(MujocoApp): 11 | def __init__(self, robot_config_file: str = None, scene_file: str = None, active_arm: str = 'right'): 12 | # Initialize the Parent class with the config file 13 | super().__init__(robot_config_file, scene_file) 14 | # Specify the robot in the scene that we'd like to use 15 | self.robot = self.get_robot(robot_name="DualUR5") 16 | # Specify the controller configuations that should be used for 17 | # the corresponding devices 18 | admit_device_configs = [ 19 | ('ur5right', self.get_controller_config('osc1')), 20 | ('ur5left', self.get_controller_config('osc1')) 21 | ] 22 | # Get the configuration for the nullspace controller 23 | nullspace_config = self.get_controller_config('nullspace') 24 | self.controller = OSC(self.robot, self.sim, admit_device_configs, nullspace_config,admittance = True) 25 | 26 | # self.robot_data_thread = threading.Thread(target=self.robot.start) 27 | # self.robot_data_thread.start() 28 | 29 | # Keep track of device target errors 30 | self.errors = dict() 31 | self.errors['ur5right'] = 0 32 | self.viewer = MjViewer(self.sim) 33 | 34 | def gen_target(self) -> Tuple[np.ndarray, np.ndarray]: 35 | """ 36 | Generates the target position for both arms 37 | """ 38 | right_wp = np.array([ 39 | [0.3, 0.46432, 0.36243] 40 | ]) 41 | 42 | left_wp = np.array([ 43 | [-0.3, 0.46432, 0.5], 44 | [-0.3, 0.5, 0.5], 45 | [-0.3, 0.55, 0.5], 46 | [-0.3, 0.575, 0.5], 47 | [-0.3, 0.6, 0.5], 48 | [-0.3, 0.625, 0.5], 49 | [-0.3, 0.65, 0.5], 50 | [-0.3, 0.675, 0.5], 51 | [-0.3, 0.7, 0.5], 52 | [-0.3, 0.75, 0.5], 53 | ]) 54 | 55 | return (right_wp, left_wp) 56 | 57 | def run(self): 58 | """ 59 | This is the main function that gets called. Uses the 60 | Admittance Controller on the DualUR5 to apply a force on the external 61 | object. 62 | """ 63 | #start the thread timmer 64 | time_thread = threading.Thread(target=self.sleep_for, args=(200,)) 65 | time_thread.start() 66 | threshold_ee = 0.01 67 | #Define targets for both arms 68 | targets: Dict[str, Target] = { 69 | 'ur5right' : Target(), 70 | 'ur5left' : Target(), 71 | } 72 | #Get the targets for both arms 73 | right_wps, left_wps = self.gen_target() 74 | ur5right = self.robot.sub_devices_dict['ur5right'] 75 | ur5left = self.robot.sub_devices_dict['ur5left'] 76 | #Define target indices 77 | right_wp_index = 0 78 | left_wp_index = 0 79 | #Initialize force variables 80 | x = 0 81 | #Define field names tp write to CSV 82 | fieldnames = ["x", "force_x", "force_y", "force_z"] 83 | #Write the fieldnames to CSV 84 | with open('data.csv', 'w') as csv_file: 85 | csv_writer = csv.DictWriter(csv_file, fieldnames=fieldnames) 86 | csv_writer.writeheader() 87 | 88 | while self.timer_running: 89 | #set the targets position and orientation of both arms 90 | targets['ur5right'].set_xyz(right_wps[right_wp_index]) 91 | targets['ur5left'].set_xyz(left_wps[left_wp_index]) 92 | targets['ur5left'].set_abg(np.array([0,0,-1*np.pi/2])) 93 | #set the mocap position to target position 94 | self.sim.data.set_mocap_pos('target_red', right_wps[right_wp_index]) 95 | self.sim.data.set_mocap_pos('target_blue', left_wps[left_wp_index]) 96 | #Genetrate the admittance control output 97 | ctrlr_output = self.controller.generate(targets) 98 | for force_idx, force in zip(*ctrlr_output): 99 | self.sim.data.ctrl[force_idx] = force 100 | #Measure the errors 101 | self.errors['ur5left'] = np.linalg.norm(ur5left.get_state(DeviceState.EE_XYZ) - targets['ur5left'].get_xyz()) 102 | #Move to next target if error is less then threshold 103 | if self.errors['ur5left'] < threshold_ee: 104 | if left_wp_index < left_wps.shape[0] - 1 : 105 | left_wp_index += 1 106 | else: 107 | left_wp_index = 0 108 | self.sim.step() 109 | self.viewer.render() 110 | functions.mj_inverse(self.model,self.sim.data) 111 | #Measure force on left end-effector 112 | force = ur5left.get_state(DeviceState.FORCE) 113 | x += 1 114 | #write force values to CSV 115 | with open('data.csv', 'a') as csv_file: 116 | csv_writer = csv.DictWriter(csv_file, fieldnames=fieldnames) 117 | 118 | info = { 119 | "x": x, 120 | "force_x": force[0], 121 | "force_y": force[1], 122 | "force_z": force[2], 123 | } 124 | csv_writer.writerow(info) 125 | time_thread.join() 126 | # self.robot_data_thread.join() 127 | glfw.destroy_window(self.viewer.window) 128 | 129 | if __name__ == "__main__": 130 | ur5 = ForceTest(robot_config_file="default_xyz_abg.yaml", scene_file="force_test_scene.xml") 131 | ur5.run() 132 | -------------------------------------------------------------------------------- /irl_control/examples/gain_test.py: -------------------------------------------------------------------------------- 1 | from mujoco_py import GlfwContext 2 | from mujoco_py.mjviewer import MjViewer 3 | import numpy as np 4 | from typing import Tuple 5 | import threading 6 | from irl_control import MujocoApp, OSC 7 | from irl_control.utils import Target 8 | from irl_control.device import DeviceState 9 | 10 | """ 11 | The purpose of this example is to test out the robot configuration 12 | to see how well the gains perform on stabilizing the base and the 13 | arm that does move rapidly. One of the arms in this demo will move 14 | wildly to test out this stabilization capability. 15 | """ 16 | class GainTest(MujocoApp): 17 | """ 18 | Implements the OSC and Dual UR5 robot 19 | """ 20 | def __init__(self, robot_config_file : str =None, scene_file : str = None): 21 | # Initialize the Parent class with the config file 22 | super().__init__(robot_config_file, scene_file) 23 | # Specify the robot in the scene that we'd like to use 24 | self.robot = self.get_robot(robot_name="DualUR5") 25 | 26 | # Specify the controller configuations that should be used for 27 | # the corresponding devices 28 | osc_device_configs = [ 29 | ('base', self.get_controller_config('osc0')), 30 | ('ur5right', self.get_controller_config('osc2')), 31 | ('ur5left', self.get_controller_config('osc2')) 32 | ] 33 | 34 | # Get the configuration for the nullspace controller 35 | nullspace_config = self.get_controller_config('nullspace') 36 | self.controller = OSC(self.robot, self.sim, osc_device_configs, nullspace_config) 37 | 38 | # Start collecting device states from simulator 39 | # NOTE: This is necessary when you are using OSC, as it assumes 40 | # that the robot.start() thread is running. 41 | self.robot_data_thread = threading.Thread(target=self.robot.start) 42 | self.robot_data_thread.start() 43 | 44 | # Keep track of device target errors 45 | self.errors = dict() 46 | self.errors['ur5right'] = 0 47 | self.viewer = MjViewer(self.sim) 48 | self.viewer.cam.azimuth = 90 49 | self.viewer.cam.elevation = -30 50 | self.viewer.cam.distance = self.model.stat.extent*1.5 51 | 52 | def gen_figure_eight_path(self) -> Tuple[np.ndarray, np.ndarray]: 53 | """ 54 | Generates a figure 8 path. 55 | """ 56 | left_wp = np.array([ 57 | [-0.3, -0.2, 0.2], 58 | [-0.4, -0.2, 0.3], 59 | [-0.7, -0.3, 0.3], 60 | [-0.8, -0.4, 0.5], 61 | [-0.9, -0.35, 0.7], 62 | [-0.9, -0.2, 0.5], 63 | [-0.9, -0.6, 0.2], 64 | [-0.7, -0.7, 0.2], 65 | [-0.4, -0.8, 0.3], 66 | ]) 67 | 68 | interp = np.linspace(left_wp[0], left_wp[1], 5) 69 | for i in range(1, left_wp.shape[0] - 1): 70 | arr = np.linspace(left_wp[i], left_wp[i+1], 5) 71 | interp = np.vstack((interp, arr)) 72 | 73 | arr = np.linspace(left_wp[left_wp.shape[0] - 1], left_wp[0], 5) 74 | left_wp = np.vstack((interp, arr)) 75 | right_wp = np.copy(left_wp) 76 | right_wp[:,[0,1]] *= -1 77 | 78 | return (right_wp, left_wp) 79 | 80 | 81 | def gen_waypoint_path(self) -> Tuple[np.ndarray, np.ndarray]: 82 | """ 83 | Generates a back-and-forth path. 84 | """ 85 | # Generate waypoints for the right arm 86 | right_wp = np.array([ 87 | [0.8, 0.6, 0.7], 88 | [0.8, -0.6, 0.7], 89 | ]) 90 | 91 | # Generate waypoints for the left arm 92 | left_wp = np.array([ 93 | [-0.5, -0.5, 0.5], 94 | ]) 95 | 96 | return (right_wp, left_wp) 97 | 98 | def run(self, demo_type: str, demo_duration: int): 99 | """ 100 | This is the main function that gets called. Uses the 101 | Operational Space Controller to control the DualUR5 102 | and guide the arms and base to specified target waypoints. 103 | 104 | Parameters 105 | ---------- 106 | demo_type: str 107 | The name of the demo that should be run 108 | """ 109 | # Choose demo type 110 | if demo_type == 'gain_test': 111 | right_wps, left_wps = self.gen_waypoint_path() 112 | elif demo_type == 'figure8': 113 | right_wps, left_wps = self.gen_figure_eight_path() 114 | else: 115 | print("Demo not available!") 116 | return 117 | 118 | # Start a timer for the demo 119 | time_thread = threading.Thread(target=self.sleep_for, args=(demo_duration,)) 120 | time_thread.start() 121 | threshold_ee = 0.1 122 | 123 | # Initialize the targets to be filled in later 124 | targets: Dict[str, Target] = { 125 | 'ur5right' : Target(), 126 | 'ur5left' : Target(), 127 | 'base' : Target() 128 | } 129 | # Get the device instances from the robot 130 | ur5right = self.robot.get_device('ur5right') 131 | ur5left = self.robot.get_device('ur5left') 132 | 133 | # counters/indexers used to keep track of waypoints 134 | right_wp_idx = 0 135 | left_wp_idx = 0 136 | while self.timer_running: 137 | # Set the target values for the robot's devices 138 | targets['ur5right'].set_xyz(right_wps[right_wp_idx]) 139 | targets['ur5left'].set_xyz(left_wps[left_wp_idx]) 140 | # targets['base'].abg[2] = 0.0 141 | 142 | # Generate an OSC signal to steer robot toward the targets 143 | ctrlr_output = self.controller.generate(targets) 144 | 145 | # Generate an OSC signal to steer robot toward the targets 146 | for force_idx, force in zip(*ctrlr_output): 147 | self.sim.data.ctrl[force_idx] = force 148 | 149 | # Collect errors for the arms in order to determine whether to update 150 | # waypoint indexes 151 | self.errors['ur5right'] = np.linalg.norm(ur5right.get_state(DeviceState.EE_XYZ) - targets['ur5right'].get_xyz()) 152 | self.errors['ur5left'] = np.linalg.norm(ur5left.get_state(DeviceState.EE_XYZ) - targets['ur5left'].get_xyz()) 153 | if self.errors['ur5right'] < threshold_ee: 154 | if right_wp_idx < right_wps.shape[0] - 1: 155 | right_wp_idx += 1 156 | else: 157 | right_wp_idx = 0 158 | if self.errors['ur5left'] < threshold_ee: 159 | if left_wp_idx < left_wps.shape[0] - 1: 160 | left_wp_idx += 1 161 | else: 162 | left_wp_idx = 0 163 | 164 | # Move the target objects to the new waypoints 165 | self.sim.data.set_mocap_pos('target_red', right_wps[right_wp_idx]) 166 | self.sim.data.set_mocap_pos('target_blue', left_wps[left_wp_idx]) 167 | 168 | # Step simulator / Render scene 169 | self.sim.step() 170 | self.viewer.render() 171 | 172 | # Join threads / Stop the simulator 173 | time_thread.join() 174 | self.robot.stop() 175 | self.robot_data_thread.join() 176 | 177 | # Main entrypoint 178 | if __name__ == "__main__": 179 | # Initialize the gain test demo 180 | demo = GainTest(robot_config_file="default_xyz.yaml", scene_file="gain_test_scene.xml") 181 | # Run the gain test 182 | demo_name1 = "gain_test" 183 | demo.run(demo_name1, 10) 184 | # demo_name2 = "figure8" 185 | # demo.run(demo_name2, 10) -------------------------------------------------------------------------------- /irl_control/examples/ps_move_example.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from transforms3d.euler import quat2euler, euler2quat, quat2mat, mat2euler, euler2mat 3 | from transforms3d.affines import compose 4 | import time 5 | import numpy as np 6 | from mujoco_py import load_model_from_path, MjSim 7 | from mujoco_py.mjviewer import MjViewer 8 | import numpy as np 9 | from typing import List, Tuple, Dict 10 | import threading 11 | from irl_control import MujocoApp, OSC 12 | from irl_control.utils import Target 13 | from irl_control.input_devices.ps_move import PSMoveInterface, MoveName 14 | from irl_control.device import DeviceState 15 | 16 | """ 17 | This demo uses the PS Move controllers to control both robot arms 18 | of the Dual UR5. In this demo, you can open the gripper by pressing 19 | the Circle button and close the gripper by pressing the Triangle button. 20 | The back trigger enables the robot, such that it will travel to the end 21 | effector that is displayed by the mocap object attached to the 22 | corresponding move controller. 23 | """ 24 | class PSMoveExample(MujocoApp): 25 | """ 26 | Implements the OSC and Dual UR5 Robot 27 | """ 28 | def __init__(self, robot_config_file : str =None, scene_file : str = None): 29 | # Initialize the Parent class with the config file 30 | super().__init__(robot_config_file, scene_file) 31 | self.set_free_joint_qpos('free_joint_female', euler2quat(0, 0, np.pi/2)) 32 | self.set_free_joint_qpos('free_joint_male', euler2quat(0, 0, np.pi/2)) 33 | 34 | # Specify the robot in the scene that we'd like to use 35 | self.robot = self.get_robot(robot_name="DualUR5") 36 | osc_device_configs = [ 37 | ('base', self.get_controller_config('osc2')), 38 | ('ur5right', self.get_controller_config('osc2')), 39 | ('ur5left', self.get_controller_config('osc2')) 40 | ] 41 | 42 | # Get the configuration for the nullspace controller 43 | nullspace_config = self.get_controller_config('nullspace') 44 | self.controller = OSC(self.robot, self.sim, osc_device_configs, nullspace_config) 45 | self.viewer = MjViewer(self.sim) 46 | 47 | # Set the scene camera distance and angle 48 | self.viewer.cam.azimuth = -120 49 | self.viewer.cam.elevation = -40 50 | self.viewer.cam.distance = self.model.stat.extent 51 | 52 | # Initialize the move interface (without multiprocessing) 53 | self.move_interface = PSMoveInterface(multiprocess=False) 54 | self.move_states = self.move_interface.move_states 55 | self.grip_pos = dict([(move_name, 0.0) for move_name in MoveName]) 56 | 57 | # Start collecting the robot states 58 | # self.robot_data_thread = threading.Thread(target=self.robot.start) 59 | # self.robot_data_thread.start() 60 | 61 | def update_move_button_states(self, sleep_time = 0.1): 62 | """ 63 | Get the move button states from the Move Interface 64 | at a rate specified by the sleep time so that the 65 | gripper (open and close) state can be updated 66 | """ 67 | # Specify the min/max positions of the gripper (found experimentally) 68 | max_pos = 0.9 69 | min_pos = 0.0 70 | # Keep track of the grip positions locally 71 | grip_pos = dict([(move_name, 0.0) for move_name in MoveName]) 72 | while True: 73 | for move_name in MoveName: 74 | close = self.move_states[move_name].get('circle') 75 | open = self.move_states[move_name].get('triangle') 76 | # Apply a fixed increment if the button is pressed 77 | if close: 78 | grip_pos[move_name] -= 0.05 79 | if open: 80 | grip_pos[move_name] += 0.05 81 | # Limit the grip positoin based on the min/max values specified above 82 | grip_pos[move_name] = min(max_pos, max(min_pos, grip_pos[move_name])) 83 | # Update the class grip positions with the local position 84 | self.grip_pos[move_name] = grip_pos[move_name] 85 | time.sleep(sleep_time) 86 | 87 | def run(self): 88 | # Start a timer for the demo 89 | targets: Dict[str, Target] = { 90 | 'ur5right' : Target(), 91 | 'ur5left' : Target(), 92 | 'base' : Target() 93 | } 94 | 95 | # Start a thread to read the move buttons from the interface 96 | move_button_thread = threading.Thread(target=self.update_move_button_states, args=()) 97 | move_button_thread.start() 98 | 99 | # Get the ur5 devices from the robot 100 | ur5right = self.robot.get_device('ur5right') 101 | ur5left = self.robot.get_device('ur5left') 102 | 103 | while True: 104 | # Get the xyz/abg states from the move interface for the left and right move controllers 105 | xyz_r = self.move_states[MoveName.RIGHT].get('pos') 106 | ang_r = self.move_states[MoveName.RIGHT].get('quat') 107 | xyz_l = self.move_states[MoveName.LEFT].get('pos') 108 | ang_l = self.move_states[MoveName.LEFT].get('quat') 109 | 110 | # Apply the given transformation to the end effector based on the orientation of the move, 111 | # since we wish to control the end effector orientation using the move orientations 112 | angle_mat_r = quat2mat(ang_r) 113 | tfmat1 = compose(xyz_r, angle_mat_r, [1,1,1]) 114 | tfmat_r = compose([0.0,0,0], np.eye(3), [1,1,1]) 115 | tfmat_r = np.matmul(tfmat1, tfmat_r) 116 | # The following transformation (below) is used to orient the end effector in a way that faces 117 | # down the y axis for easier gripping of objects 118 | tfmat_r = np.matmul(tfmat_r, compose([0.0,0,0], euler2mat(np.pi/2,0,np.pi/2), [1,1,1])) 119 | 120 | # Similar to above, apply the transformations to the end effector 121 | # of the left arm using the left move 122 | angle_mat_l = quat2mat(ang_l) 123 | tfmat2 = compose(xyz_l, angle_mat_l, [1,1,1]) 124 | tfmat_l = compose([0.05,0,0], np.eye(3), [1,1,1]) 125 | tfmat_l = np.matmul(tfmat2, tfmat_l) 126 | tfmat_l = np.matmul(tfmat_l, compose([-0.15,0,0], euler2mat(0,0,-np.pi/2), [1,1,1])) 127 | 128 | # Extract the final rotation from the rotation matrices for the 129 | # left and right arms / moves 130 | r_xyz = tfmat_r[0:3,-1].flatten() 131 | l_xyz = tfmat_l[0:3,-1].flatten() 132 | r_ang = np.array(mat2euler(tfmat_r[:3, :3])) 133 | l_ang = np.array(mat2euler(tfmat_l[:3, :3])) 134 | 135 | # Set the targets for operational space control if the trigger is pressed 136 | if self.move_states[MoveName.RIGHT].get('trigger'): 137 | ur5right.ctrlr_dof_abg = [True, True, True] 138 | targets['ur5right'].set_xyz(r_xyz) 139 | targets['ur5right'].set_abg(r_ang) 140 | else: 141 | ur5right.ctrlr_dof_abg = [False, False, False] 142 | targets['ur5right'].set_xyz(ur5right.get_state(DeviceState.EE_XYZ)) 143 | 144 | # Set the targets for operational space control if the trigger is pressed 145 | if self.move_states[MoveName.LEFT].get('trigger'): 146 | ur5left.ctrlr_dof_abg = [True, True, True] 147 | targets['ur5left'].set_xyz(l_xyz) 148 | targets['ur5left'].set_abg(l_ang) 149 | else: 150 | ur5left.ctrlr_dof_abg = [False, False, False] 151 | targets['ur5left'].set_xyz(ur5left.get_state(DeviceState.EE_XYZ)) 152 | 153 | # Get the control from the operational space control based on the targets 154 | ctrlr_output = self.controller.generate(targets) 155 | # Send the forces to the robot (including gripper values) 156 | for force_idx, force in zip(*ctrlr_output): 157 | self.sim.data.ctrl[force_idx] = force 158 | self.sim.data.ctrl[7] = self.grip_pos[MoveName.RIGHT] 159 | self.sim.data.ctrl[14] = self.grip_pos[MoveName.LEFT] 160 | 161 | # Set the position of the mocap corresponding the the right move 162 | self.sim.data.set_mocap_pos('hand_right', r_xyz) 163 | # self.sim.data.set_mocap_quat('hand_ur5right', euler2quat(r_ang[0], r_ang[1], r_ang[2])) 164 | 165 | # Set the position of the mocap corresponding the the left move 166 | self.sim.data.set_mocap_pos('hand_left', l_xyz) 167 | # self.sim.data.set_mocap_quat('hand_left', euler2quat(l_ang[0], l_ang[1], l_ang[2])) 168 | 169 | # Apply rumble to the right controller based on the sensed force from the end effector 170 | ur5right_gripper_force = self.sim.data.sensordata[13] 171 | self.move_states[MoveName.RIGHT].set('rumble', ur5right_gripper_force) 172 | 173 | # Apply rumble to the left controller based on the sensed force from the end effector 174 | ur5left_gripper_force = self.sim.data.sensordata[16] 175 | self.move_states[MoveName.LEFT].set('rumble', ur5left_gripper_force) 176 | 177 | # Step the simulator / Render scene 178 | self.sim.step() 179 | self.viewer.render() 180 | 181 | # Join threads / Stop the simulator 182 | # self.robot.stop() 183 | # self.robot_data_thread.join() 184 | 185 | # Main entrypoint 186 | if __name__ == "__main__": 187 | # Initialize the PS Move Demo 188 | demo = PSMoveExample(robot_config_file="default_xyz_abg.yaml", scene_file="ps_move_scene.xml") 189 | # Run the PS Move Demo until user terminates program 190 | demo.run() -------------------------------------------------------------------------------- /irl_control/examples/space_mouse_example.py: -------------------------------------------------------------------------------- 1 | from mujoco_py import GlfwContext 2 | from mujoco_py.mjviewer import MjViewer 3 | import numpy as np 4 | from typing import Dict 5 | import threading 6 | from irl_control import OSC, MujocoApp 7 | from transforms3d.euler import quat2euler, euler2quat, quat2mat, mat2euler, euler2mat 8 | from transforms3d.utils import normalized_vector 9 | from transforms3d.affines import compose 10 | from irl_control.utils import Target 11 | from irl_control.input_devices.space_mouse import SpaceMouse 12 | # from InverseKinematics import IK 13 | 14 | class SpaceMouseDemo(MujocoApp): 15 | """ 16 | This class implements the OSC and Dual UR5 robot 17 | """ 18 | def __init__(self, robot_config_file : str =None, scene_file : str = None): 19 | # Initialize the Parent class with the config file 20 | super().__init__(robot_config_file, scene_file) 21 | # Specify the robot in the scene that we'd like to use 22 | self.viewer = MjViewer(self.sim) 23 | self.robot = self.get_robot(robot_name="DualUR5") 24 | # self.ik = IK(self.robot, self.sim) 25 | 26 | osc_device_configs = [ 27 | ('base', self.get_controller_config('osc2')), 28 | ('ur5right', self.get_controller_config('osc2')), 29 | ('ur5left', self.get_controller_config('osc2')) 30 | ] 31 | 32 | # Get the configuration for the nullspace controller 33 | nullspace_config = self.get_controller_config('nullspace') 34 | self.controller = OSC(self.robot, self.sim, osc_device_configs, nullspace_config) 35 | 36 | # self.robot_data_thread = threading.Thread(target=self.robot.start) 37 | # self.robot_data_thread.start() 38 | 39 | self.viewer = MjViewer(self.sim) 40 | 41 | def run_ik_demo(self, demo_duration: int): 42 | # Start a timer for the demo 43 | time_thread = threading.Thread(target=self.sleep_for, args=(demo_duration,)) 44 | time_thread.start() 45 | 46 | targets: Dict[str, Target] = { 47 | 'ur5right' : Target(), 48 | # 'ur5left' : Target(), 49 | # 'base' : Target() 50 | } 51 | ur5right = self.robot.get_device('ur5right') 52 | # ur5left = self.robot.get_device('ur5left') 53 | sm = SpaceMouse(origin=[0.0, 0.5, 0.5, 0.0, 0.0, 0.0]) 54 | 55 | while self.timer_running: 56 | # Set the target values for the robot's devices 57 | x, y, z, roll, pitch, yaw = sm.update_state() 58 | 59 | angle = euler2quat(pitch, roll, yaw, axes='rxyz') 60 | angle_mat = quat2mat(angle) 61 | tfmat1 = compose([x,y,z], angle_mat, [1,1,1]) 62 | 63 | tfmat_r = compose([0.2,0,0], np.eye(3), [1,1,1]) 64 | tfmat_r = np.matmul(tfmat1, tfmat_r) 65 | tfmat_r = np.matmul(tfmat_r, compose([0,0,0], euler2mat(np.pi/2,0,0), [1,1,1])) 66 | 67 | tfmat_l = compose([-0.2,0,0], np.eye(3), [1,1,1]) 68 | tfmat_l = np.matmul(tfmat1, tfmat_l) 69 | tfmat_l = np.matmul(tfmat_l, compose([0,0,0], euler2mat(np.pi/2,0,0), [1,1,1])) 70 | 71 | r_xyz = tfmat_r[0:3,-1].flatten() 72 | l_xyz = tfmat_l[0:3,-1].flatten() 73 | r_ang = np.array(mat2euler(tfmat_r[:3, :3])) 74 | l_ang = np.array(mat2euler(tfmat_l[:3, :3])) 75 | 76 | self.sim.data.set_mocap_pos('plate', [x,y,z]) 77 | targets['ur5right'].set_xyz(r_xyz) 78 | targets['ur5right'].set_abg(r_ang) 79 | # targets['ur5left'].xyz = l_xyz 80 | # targets['ur5left'].abg = l_ang 81 | 82 | path = self.ik.generate(targets=targets) 83 | # self.sim.data.qpos[ur5left.ctrl_idxs] += path[ur5left.actuator_trnids] 84 | self.sim.data.qpos[ur5right.ctrl_idxs] += 3*path[ur5right.actuator_trnids] 85 | 86 | self.sim.data.set_mocap_quat('plate', angle) 87 | self.sim.data.set_mocap_pos('hand_ur5right', r_xyz) 88 | self.sim.data.set_mocap_quat('hand_ur5right', euler2quat(r_ang[0], r_ang[1], r_ang[2])) 89 | self.sim.data.set_mocap_pos('hand_ur5left', l_xyz) 90 | self.sim.data.set_mocap_quat('hand_ur5left', euler2quat(l_ang[0], l_ang[1], l_ang[2])) 91 | # Step simulator / Render scene 92 | self.sim.forward() 93 | # self.sim.step() 94 | self.viewer.render() 95 | 96 | # Join threads / Stop the simulator 97 | self.robot.stop() 98 | self.robot_data_thread.join() 99 | time_thread.join() 100 | 101 | def run_demo(self, demo_duration: int): 102 | # Start a timer for the demo 103 | time_thread = threading.Thread(target=self.sleep_for, args=(demo_duration,)) 104 | time_thread.start() 105 | 106 | targets: Dict[str, Target] = { 107 | 'ur5right' : Target(), 108 | 'ur5left' : Target(), 109 | 'base' : Target() 110 | } 111 | ur5left = self.robot.get_device('ur5left') 112 | ur5right = self.robot.get_device('ur5right') 113 | sm = SpaceMouse(origin=[0.0, 0.5, 0.5, 0.0, 0.0, 0.0]) 114 | while self.timer_running: 115 | # Set the target values for the robot's devices 116 | x, y, z, roll, pitch, yaw = sm.update_state() 117 | angle = euler2quat(pitch, roll, yaw, axes='rxyz') # pitch and roll are flipped from SM API 118 | angle_mat = quat2mat(angle) 119 | tfmat1 = compose([x,y,z], angle_mat, [1,1,1]) 120 | 121 | tfmat_r = compose([0.05,0,0], np.eye(3), [1,1,1]) 122 | tfmat_r = np.matmul(tfmat1, tfmat_r) 123 | tfmat_r = np.matmul(tfmat_r, compose([0.15,0,0], euler2mat(0,0,0), [1,1,1])) 124 | 125 | tfmat_l = compose([-0.05,0,0], np.eye(3), [1,1,1]) 126 | tfmat_l = np.matmul(tfmat1, tfmat_l) 127 | tfmat_l = np.matmul(tfmat_l, compose([-0.15,0,0], euler2mat(0,0,np.pi), [1,1,1])) 128 | 129 | r_xyz = tfmat_r[0:3,-1].flatten() 130 | l_xyz = tfmat_l[0:3,-1].flatten() 131 | r_ang = np.array(mat2euler(tfmat_r[:3, :3])) 132 | l_ang = np.array(mat2euler(tfmat_l[:3, :3])) 133 | 134 | self.sim.data.set_mocap_pos('plate', [x,y,z]) 135 | targets['ur5right'].set_xyz(r_xyz) 136 | targets['ur5right'].set_abg(r_ang) 137 | targets['ur5left'].set_xyz(l_xyz) 138 | targets['ur5left'].set_abg(l_ang) 139 | targets['base'].set_abg([0 , 0, np.arctan2(y, x) - np.pi/2]) 140 | 141 | ctrlr_output = self.controller.generate(targets) 142 | for force_idx, force in zip(*ctrlr_output): 143 | self.sim.data.ctrl[force_idx] = force 144 | 145 | self.sim.data.set_mocap_quat('plate', angle) 146 | self.sim.data.set_mocap_pos('hand_ur5right', r_xyz) 147 | self.sim.data.set_mocap_quat('hand_ur5right', euler2quat(r_ang[0], r_ang[1], r_ang[2])) 148 | self.sim.data.set_mocap_pos('hand_ur5left', l_xyz) 149 | self.sim.data.set_mocap_quat('hand_ur5left', euler2quat(l_ang[0], l_ang[1], l_ang[2])) 150 | # error_left = self.controller.calc_error(targets['ur5left'], ur5left) 151 | # error_right = self.controller.calc_error(targets['ur5right'], ur5right) 152 | # print("orien") 153 | # print(error_right[3:]) 154 | # print("xyz") 155 | # print(error_right[:3]) 156 | 157 | # Step simulator / Render scene 158 | self.sim.step() 159 | self.viewer.render() 160 | 161 | # Join threads / Stop the simulator 162 | # self.robot.stop() 163 | # self.robot_data_thread.join() 164 | time_thread.join() 165 | 166 | if __name__ == "__main__": 167 | ur5 = SpaceMouseDemo(robot_config_file="default_xyz_abg.yaml", scene_file="space_mouse_scene.xml") 168 | ur5.run_demo(400) -------------------------------------------------------------------------------- /irl_control/input_devices/ps_move.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from transforms3d.euler import quat2euler, euler2quat 3 | import time 4 | import numpy as np 5 | from typing import List, Tuple, Dict, Any 6 | from enum import IntEnum, Enum 7 | import threading 8 | from multiprocessing import Process 9 | from multiprocessing.managers import BaseManager 10 | 11 | sys.path.insert(0, "/root/irl_control/libraries/psmoveapi/build") 12 | import psmove 13 | 14 | class Dim(IntEnum): 15 | X = 0 16 | Y = 1 17 | Z = 2 18 | 19 | class MoveName(Enum): 20 | RIGHT = 0 21 | LEFT = 1 22 | 23 | class DimRange(): 24 | def __init__(self, min: float, max: float): 25 | self.min = min 26 | self.max = max 27 | 28 | class DimRanges(): 29 | def __init__(self, sim: DimRange, move: DimRange): 30 | self.sim = sim 31 | self.move = move 32 | 33 | class MoveState(): 34 | def __init__(self): 35 | self.values: Dict[str, Any] = dict() 36 | self.values['pos']: np.ndarray = np.zeros(3) 37 | self.values['quat']: np.ndarray = np.zeros(4) 38 | self.values['rumble']: int = 0 39 | self.values['trigger']: bool = False 40 | self.values['square']: bool = False 41 | self.values['triangle']: bool = False 42 | self.values['circle']: bool = False 43 | 44 | def get(self, key: str): 45 | return self.values[key] 46 | 47 | def set(self, key: str, value: Any): 48 | self.values[key] = value 49 | 50 | 51 | class PSMoveInterface(): 52 | def __init__(self, multiprocess: bool = False): 53 | if psmove.count_connected() < 1: 54 | print('No controller connected') 55 | sys.exit(1) 56 | 57 | move_count = psmove.count_connected() 58 | print('Connected controllers:', move_count) 59 | 60 | if multiprocess: 61 | BaseManager.register('MoveState', MoveState) 62 | manager = BaseManager() 63 | manager.start() 64 | 65 | self.tracker = psmove.PSMoveTracker() 66 | self.tracker.set_mirror(True) 67 | self.moves = dict() 68 | self.move_workers = [] 69 | self.move_states = dict() 70 | self.running = True 71 | for idx in range(move_count): 72 | move_name = self.serial2name(psmove.PSMove(idx).get_serial()) 73 | self.moves[move_name] = psmove.PSMove(idx) 74 | if self.moves[move_name].connection_type != psmove.Conn_Bluetooth: 75 | print('Please connect controller via Bluetooth') 76 | sys.exit(1) 77 | self.moves[move_name].enable_orientation(True) 78 | move_dim_ranges = self.get_dim_ranges(move_name) 79 | if multiprocess: 80 | self.move_states[move_name] = manager.MoveState() 81 | self.move_workers.append(Process(target=self.collect_move_state, 82 | args=(self.move_states[move_name], self.moves[move_name], move_dim_ranges, self.tracker, self.running))) 83 | else: 84 | self.move_states[move_name] = MoveState() 85 | self.move_workers.append(threading.Thread(target=self.collect_move_state, 86 | args=(self.move_states[move_name], self.moves[move_name], move_dim_ranges, self.tracker, self.running))) 87 | 88 | # Calibrate the controller with the tracker 89 | result = -1 90 | while result != psmove.Tracker_CALIBRATED: 91 | print('Trying to calibrate... move controller.') 92 | result = self.tracker.enable(self.moves[move_name]) 93 | 94 | for idx in range(move_count): 95 | self.move_workers[idx].start() 96 | 97 | def get_dim_ranges(self, move_name: MoveName): 98 | if move_name == MoveName.LEFT: 99 | dim_ranges_dict = { 100 | Dim.X : DimRanges(sim=DimRange(0.2, -0.7), move=DimRange(375, 600)), 101 | Dim.Y : DimRanges(sim=DimRange(0.9, 0.0), move=DimRange(12, 70)), 102 | Dim.Z : DimRanges(sim=DimRange(0.01, 0.5), move=DimRange(-400, -20)), 103 | } 104 | elif move_name == MoveName.RIGHT: 105 | dim_ranges_dict = { 106 | Dim.X : DimRanges(sim=DimRange(0.7, -0.2), move=DimRange(150, 375)), 107 | Dim.Y : DimRanges(sim=DimRange(0.9, 0.0), move=DimRange(12, 70)), 108 | Dim.Z : DimRanges(sim=DimRange(0.01, 0.5), move=DimRange(-400, -20)), 109 | } 110 | else: 111 | print("Move Name is not valid!") 112 | raise ValueError 113 | 114 | return dim_ranges_dict 115 | 116 | def serial2name(self, serial) -> MoveName: 117 | if serial == "00:13:8a:91:f9:7e": 118 | move_name = MoveName.RIGHT 119 | elif serial == "e0:ae:5e:3e:10:24": 120 | move_name = MoveName.LEFT 121 | else: 122 | print("Serial " + serial + " not defined!") 123 | sys.exit(1) 124 | return move_name 125 | 126 | def collect_move_state(self, move_state, move, move_dim_ranges, tracker, running): 127 | tracker_positions = { 128 | Dim.X : 0.0, 129 | Dim.Y : 0.0, 130 | Dim.Z : 0.0 131 | } 132 | 133 | while running: 134 | while move.poll(): pass 135 | tracker.update_image() 136 | tracker.update() 137 | status = tracker.get_status(move) 138 | 139 | trigger_value = move.get_trigger() 140 | if trigger_value > 10: 141 | move_state.set('trigger', True) 142 | else: 143 | move_state.set('trigger', False) 144 | 145 | if status == psmove.Tracker_TRACKING: 146 | x, y, radius = tracker.get_position(move) 147 | tracker_positions[Dim.X] = x 148 | # radius is the forward/backward axis for tracker, but y is forward/backward in sim 149 | tracker_positions[Dim.Y] = radius 150 | #y is the up/down axis for tracker, but z is up/down in sim 151 | tracker_positions[Dim.Z] = -1*y 152 | 153 | buttons = move.get_buttons() 154 | if buttons & psmove.Btn_SQUARE: 155 | move.reset_orientation() 156 | 157 | if buttons & psmove.Btn_TRIANGLE: 158 | move_state.set('triangle', True) 159 | else: 160 | move_state.set('triangle', False) 161 | 162 | if buttons & psmove.Btn_CIRCLE: 163 | move_state.set('circle', True) 164 | else: 165 | move_state.set('circle', False) 166 | 167 | move_quat = move.get_orientation() 168 | eul = quat2euler(move_quat) 169 | new_eul = [eul[0], 0, eul[1]] 170 | move_quat_new = euler2quat(*new_eul) 171 | move_state.set('quat', move_quat_new) 172 | 173 | if status == psmove.Tracker_TRACKING: 174 | pos_arr = np.zeros(3) 175 | for dim in Dim: 176 | pos = tracker_positions[dim] 177 | mr = move_dim_ranges[dim].move # move range 178 | sr = move_dim_ranges[dim].sim # sim range 179 | if pos < mr.min: 180 | pos = mr.min 181 | if pos > mr.max: 182 | pos = mr.max 183 | val = sr.min + ((pos - mr.min)/(mr.max - mr.min) * (sr.max - sr.min)) 184 | pos_arr[dim] = val 185 | move_state.set('pos', pos_arr) 186 | 187 | rv = -1*move_state.get('rumble') 188 | min_rumble_val = 0.1 189 | max_rumble_val = 0.7 190 | val = (rv - min_rumble_val)/(max_rumble_val - min_rumble_val)*130 191 | val = min(max(0, val), 130) 192 | move.set_rumble(int(val)) 193 | time.sleep(0.05) 194 | 195 | def stop(self): 196 | self.running = False -------------------------------------------------------------------------------- /irl_control/input_devices/space_mouse.py: -------------------------------------------------------------------------------- 1 | import pyspacemouse 2 | import numpy as np 3 | import types 4 | 5 | class SpaceMouse(): 6 | def __init__(self, origin, increment=0.0015): 7 | success = pyspacemouse.open() 8 | if not success: 9 | print("Space Mouse not found!") 10 | raise Exception 11 | self.increment = increment 12 | self.state = types.SimpleNamespace() 13 | self.state.x = origin[0] 14 | self.state.y = origin[1] 15 | self.state.z = origin[2] 16 | self.state.roll = origin[3] 17 | self.state.pitch = origin[4] 18 | self.state.yaw = origin[5] 19 | 20 | def constrain_angle(self, angle): 21 | return np.arctan2(np.sin(angle), np.cos(angle)) 22 | 23 | def update_state(self): 24 | inc = self.increment 25 | cur_state = pyspacemouse.read() 26 | self.state.x += inc*cur_state.x 27 | self.state.y += inc*cur_state.y 28 | self.state.z += inc*cur_state.z 29 | self.state.yaw -= inc*cur_state.yaw 30 | self.state.yaw = self.constrain_angle(self.state.yaw) 31 | self.state.pitch -= inc*cur_state.pitch 32 | self.state.pitch = self.constrain_angle(self.state.pitch) 33 | self.state.roll += inc*cur_state.roll 34 | self.state.roll = self.constrain_angle(self.state.roll) 35 | return (self.state.x, self.state.y, self.state.z, self.state.roll, self.state.pitch, self.state.yaw) 36 | -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST.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 | 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 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_001.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_001.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.001000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.001000.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.001001.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.001001.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.001002.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.001002.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.001003.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.001003.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.001004.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.001004.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.001005.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.001005.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.001006.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.001006.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.002000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.002000.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.002001.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.002001.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.002002.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.002002.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.002003.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.002003.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.002004.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.002004.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.002005.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.002005.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.003000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.003000.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.003001.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.003001.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.003002.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.003002.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.003003.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.003003.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.003004.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.003004.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.003005.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.003005.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.003006.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.003006.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.003007.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.003007.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.003008.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.003008.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.003009.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.003009.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.004000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.004000.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.004001.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.004001.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.004002.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.004002.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.004003.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.004003.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.004004.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.004004.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.004005.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.004005.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.004006.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.004006.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.004007.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.004007.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.004008.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.004008.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.005000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.005000.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.005001.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.005001.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.005002.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.005002.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.005003.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.005003.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.005004.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.005004.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.005005.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.005005.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.005006.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.005006.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.005007.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.005007.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.006000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.006000.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.006001.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.006001.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.006002.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.006002.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.006003.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.006003.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.006004.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.006004.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.006005.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.006005.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.006006.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.006006.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.006007.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.006007.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.006008.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.006008.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_002r.006009.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_002r.006009.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_003.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_003.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/Female_NIST_004.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Female_NIST/Female_NIST_004.stl -------------------------------------------------------------------------------- /irl_control/meshes/Female_NIST/collection.info: -------------------------------------------------------------------------------- 1 | Relocated collection: 2 | Relative to: female_nist 3 | Using Relative: False 4 | X,Y,Z Translation: [0.0, 0.0, 0.0] 5 | X,Y,Z Rotation: [0.0, 0.0, 0.0] 6 | X,Y,Z Scale: [2, 2, 2] 7 | -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/Male_NIST.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 | -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/Male_NIST_001.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Male_NIST/Male_NIST_001.stl -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/Male_NIST_002.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Male_NIST/Male_NIST_002.stl -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/Male_NIST_003.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Male_NIST/Male_NIST_003.stl -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/Male_NIST_003C.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Male_NIST/Male_NIST_003C.stl -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/Male_NIST_004.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Male_NIST/Male_NIST_004.stl -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/Male_NIST_005.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Male_NIST/Male_NIST_005.stl -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/Male_NIST_005C.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Male_NIST/Male_NIST_005C.stl -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/Male_NIST_006.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Male_NIST/Male_NIST_006.stl -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/Male_NIST_006C.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Male_NIST/Male_NIST_006C.stl -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/Male_NIST_007.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Male_NIST/Male_NIST_007.stl -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/Male_NIST_007C.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/Male_NIST/Male_NIST_007C.stl -------------------------------------------------------------------------------- /irl_control/meshes/Male_NIST/collection.info: -------------------------------------------------------------------------------- 1 | Relocated collection: 2 | Relative to: male_nist 3 | Using Relative: False 4 | X,Y,Z Translation: [0.0, 0.0, 0.0] 5 | X,Y,Z Rotation: [0.0, 0.0, 0.0] 6 | X,Y,Z Scale: [2, 2, 2] 7 | -------------------------------------------------------------------------------- /irl_control/meshes/basket/basket.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /irl_control/meshes/basket/basket000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/basket/basket000.stl -------------------------------------------------------------------------------- /irl_control/meshes/basket/basket001.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/basket/basket001.stl -------------------------------------------------------------------------------- /irl_control/meshes/basket/basket002.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/basket/basket002.stl -------------------------------------------------------------------------------- /irl_control/meshes/basket/basket003.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/basket/basket003.stl -------------------------------------------------------------------------------- /irl_control/meshes/basket/basket004.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/basket/basket004.stl -------------------------------------------------------------------------------- /irl_control/meshes/compose_xml.py: -------------------------------------------------------------------------------- 1 | from xml.etree.ElementTree import Element, SubElement, tostring 2 | import xml.etree.ElementTree as ET 3 | import os 4 | from xml.dom import minidom 5 | 6 | folder = 'basket' 7 | files = [x for x in os.listdir(folder) if x.endswith(".stl")] 8 | root = Element('mujoco') 9 | assets = SubElement(root, "asset") 10 | worldbody = SubElement(root, "worldbody") 11 | 12 | for i, file in enumerate(files): 13 | fname = "".join(file.split('.')[:-1]) 14 | attribs = { 15 | "name" : fname, 16 | "file" : folder + '/' + file 17 | } 18 | child = SubElement(assets, "mesh", attrib=attribs) 19 | 20 | starting_pos = "0.0 0 0" 21 | attribs = { 22 | "name" : "basket_whole", 23 | "pos" : starting_pos, 24 | "quat" : "1.0 0 0 0" 25 | } 26 | sub_piece = SubElement(worldbody, "body", attrib=attribs) 27 | 28 | for i, file in enumerate(files): 29 | fname = "".join(file.split('.')[:-1]) 30 | attribs = { 31 | "name" : fname, 32 | "type" : "mesh", 33 | "mesh" : fname, 34 | "pos" : "0 0 0", 35 | "conaffinity" : "1", 36 | "contype" : "1", 37 | # "mass" : "0.001" 38 | } 39 | geoms = SubElement(sub_piece, "geom", attrib=attribs) 40 | 41 | tree = ET.ElementTree(root) 42 | 43 | with open(folder + '/basket.xml', 'wb') as f: 44 | xmlstr = minidom.parseString(ET.tostring(root)).toprettyxml(indent=" ") 45 | f.write(xmlstr.encode('utf-8')) 46 | 47 | # with open('out2.xml', 'wb') as f: 48 | # x = etree.parse("out.xml") 49 | # f.write(etree.tostring(x, pretty_print=True)) 50 | #files = os.listdir('./good_out') 51 | 52 | -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/base.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/dual_pegs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/handle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/handle.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_01.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_01.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_02.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_02.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_03.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_03.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_04.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_04.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_05.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_05.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_06.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_06.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_07.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_07.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_08.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_08.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_09.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_09.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_10.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_10.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_11.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_11.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_12.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_12.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_13.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_13.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_14.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_14.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_15.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_15.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_16.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_16.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_17.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_17.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_18.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_18.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_19.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_19.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_20.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_20.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_21.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_21.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_22.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_22.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_23.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_23.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_24.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_24.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_25.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_25.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_26.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_26.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_27.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_27.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_28.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_28.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_29.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_29.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_30.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_30.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_31.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_31.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_32.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_32.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_11_33.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_11_33.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_01.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_01.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_02.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_02.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_03.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_03.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_04.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_04.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_05.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_05.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_06.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_06.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_07.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_07.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_08.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_08.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_09.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_09.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_10.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_10.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_11.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_11.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_12.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_12.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_13.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_13.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_14.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_14.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_15.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_15.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_16.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_16.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_17.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_17.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_18.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_18.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_19.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_19.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_20.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_20.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_21.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_21.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_22.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_22.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_23.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_23.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_24.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_24.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_25.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_25.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_26.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_26.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_27.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_27.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_28.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_28.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_29.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_29.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_30.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_30.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_31.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_31.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_32.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_32.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/hole_16_33.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/hole_16_33.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/pin_a.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/pin_a.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/pin_b.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/pin_b.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/stand_a.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/stand_a.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_pegs/stand_b.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_pegs/stand_b.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_ur_stand/dual_ur_stand.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_ur_stand/dual_ur_stand.blend -------------------------------------------------------------------------------- /irl_control/meshes/dual_ur_stand/dual_ur_stand.blend1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_ur_stand/dual_ur_stand.blend1 -------------------------------------------------------------------------------- /irl_control/meshes/dual_ur_stand/dual_ur_stand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_ur_stand/dual_ur_stand.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_ur_stand/dual_ur_stand.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /irl_control/meshes/dual_ur_stand/dual_ur_stand2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_ur_stand/dual_ur_stand2.stl -------------------------------------------------------------------------------- /irl_control/meshes/dual_ur_stand/dual_ur_stand_ring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/dual_ur_stand/dual_ur_stand_ring.stl -------------------------------------------------------------------------------- /irl_control/meshes/pegs/Waterproof_Female.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/pegs/Waterproof_Female.stl -------------------------------------------------------------------------------- /irl_control/meshes/pegs/Waterproof_Female_part_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/pegs/Waterproof_Female_part_1.stl -------------------------------------------------------------------------------- /irl_control/meshes/pegs/Waterproof_Female_part_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/pegs/Waterproof_Female_part_2.stl -------------------------------------------------------------------------------- /irl_control/meshes/pegs/Waterproof_Male.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/pegs/Waterproof_Male.stl -------------------------------------------------------------------------------- /irl_control/meshes/pegs/back.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/pegs/back.stl -------------------------------------------------------------------------------- /irl_control/meshes/pegs/bracket_11.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/pegs/bracket_11.stl -------------------------------------------------------------------------------- /irl_control/meshes/pegs/bracket_16.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/pegs/bracket_16.stl -------------------------------------------------------------------------------- /irl_control/meshes/pegs/bracket_21.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/pegs/bracket_21.stl -------------------------------------------------------------------------------- /irl_control/meshes/pegs/dual_pegs.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/pegs/dual_pegs.stl -------------------------------------------------------------------------------- /irl_control/meshes/pegs/front.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/pegs/front.stl -------------------------------------------------------------------------------- /irl_control/meshes/pegs/left.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/pegs/left.stl -------------------------------------------------------------------------------- /irl_control/meshes/pegs/nist.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /irl_control/meshes/pegs/right.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/pegs/right.stl -------------------------------------------------------------------------------- /irl_control/meshes/quad_peg/BaseAndHandles.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/quad_peg/BaseAndHandles.blend -------------------------------------------------------------------------------- /irl_control/meshes/quad_peg/BaseAndHandles.blend1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/quad_peg/BaseAndHandles.blend1 -------------------------------------------------------------------------------- /irl_control/meshes/quad_peg/handle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/quad_peg/handle.stl -------------------------------------------------------------------------------- /irl_control/meshes/quad_peg/quad_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/quad_peg/quad_base.stl -------------------------------------------------------------------------------- /irl_control/meshes/quad_peg/quad_peg_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/quad_peg/quad_peg_base.stl -------------------------------------------------------------------------------- /irl_control/meshes/quad_peg/quad_pegs.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 | -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_base_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_base_link.stl -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_inner_finger.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_inner_finger.stl -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_inner_finger_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_inner_finger_vis.stl -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_inner_knuckle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_inner_knuckle.stl -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_inner_knuckle_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_inner_knuckle_vis.stl -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_outer_finger.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_outer_finger.stl -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_outer_finger_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_outer_finger_vis.stl -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_outer_knuckle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_outer_knuckle.stl -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_outer_knuckle_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_outer_knuckle_vis.stl -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_pad_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_85_pad_vis.stl -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_base_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/robotiq_85_gripper/robotiq_arg2f_base_link.stl -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_gripper_85.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 | 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 | 104 | 105 | -------------------------------------------------------------------------------- /irl_control/meshes/robotiq_85_gripper/robotiq_gripper_coupling_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/robotiq_85_gripper/robotiq_gripper_coupling_vis.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/floor_tile.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/floor_tile.png -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link0.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link1.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link1_cap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link1_cap.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link1_connector.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link1_connector.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link2.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link2_cap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link2_cap.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link2_connector.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link2_connector.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link2_tube.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link2_tube.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link3_cap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link3_cap.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link3_tube.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link3_tube.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link3a.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link3a.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link3a_connector.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link3a_connector.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link3b.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link3b.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link3b_connector.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link3b_connector.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link3b_connector2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link3b_connector2.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link4.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link4_cap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link4_cap.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link4_connector.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link4_connector.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link5.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link5_cap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link5_cap.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link5_connector.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link5_connector.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link6.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/link6_connector.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/link6_connector.stl -------------------------------------------------------------------------------- /irl_control/meshes/ur5/ur5.skp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/ur5/ur5.skp -------------------------------------------------------------------------------- /irl_control/meshes/wall/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/wall/.DS_Store -------------------------------------------------------------------------------- /irl_control/meshes/wall/wall.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /irl_control/meshes/wall/wallv3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ir-lab/irl_control/5580a196eb9fe4352fa34533b9877b9f5a01968b/irl_control/meshes/wall/wallv3.stl -------------------------------------------------------------------------------- /irl_control/mujoco_app.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from mujoco_py import load_model_from_path, MjSim 3 | import irl_control 4 | from irl_control import Device, Robot 5 | from typing import Dict 6 | import time 7 | import os 8 | import yaml 9 | 10 | class MujocoApp(): 11 | def __init__(self, robot_config_file : str = None, scene_file : str = None, use_sim : bool = True): 12 | main_dir = os.path.dirname(irl_control.__file__) 13 | scene_file_path = os.path.join(main_dir, "scenes", scene_file) 14 | robot_config_path = os.path.join(main_dir, "robot_configs", robot_config_file) 15 | with open(robot_config_path, 'r') as file: 16 | self.config = yaml.safe_load(file) 17 | self.model = load_model_from_path(scene_file_path) 18 | self.sim = MjSim(self.model) 19 | self.devices = np.array([Device(dev, self.model, self.sim, use_sim) for dev in self.config['devices']]) 20 | self.create_robot_devices(self.config['robots'], use_sim) 21 | self.controller_configs = self.config['controller_configs'] 22 | self.timer_running = False 23 | 24 | def create_robot_devices(self, robot_yml: Dict, use_sim: bool): 25 | robots = np.array([]) 26 | all_robot_device_idxs = np.array([], dtype=np.int32) 27 | for rbt in robot_yml: 28 | robot_device_idxs = rbt['device_ids'] 29 | all_robot_device_idxs = np.hstack([all_robot_device_idxs, robot_device_idxs]) 30 | robot = Robot(self.devices[robot_device_idxs], rbt['name'], self.sim, use_sim) 31 | robots = np.append(robots, robot) 32 | 33 | all_idxs = np.arange(len(self.devices)) 34 | keep_idxs = np.setdiff1d(all_idxs, all_robot_device_idxs) 35 | self.devices = np.hstack([self.devices[keep_idxs], robots]) 36 | 37 | def sleep_for(self, sleep_time: float): 38 | assert self.timer_running == False 39 | self.timer_running = True 40 | time.sleep(sleep_time) 41 | self.timer_running = False 42 | 43 | def get_robot(self, robot_name: str) -> Robot: 44 | for device in self.devices: 45 | if type(device) == Robot: 46 | if device.name == robot_name: 47 | return device 48 | 49 | def get_controller_config(self, name: str) -> Dict: 50 | ctrlr_conf = self.config['controller_configs'] 51 | for entry in ctrlr_conf: 52 | if entry['name'] == name: 53 | return entry 54 | 55 | def set_free_joint_qpos(self, free_joint_name, quat=None, pos=None): 56 | jnt_id = self.sim.model.joint_name2id(free_joint_name) 57 | offset = self.sim.model.jnt_qposadr[jnt_id] 58 | if quat is not None: 59 | quat_idxs = np.arange(offset+3, offset+7) # Grab the quaternion idxs 60 | self.sim.data.qpos[quat_idxs] = quat 61 | if pos is not None: 62 | pos_idxs = np.arange(offset, offset+3) 63 | self.sim.data.qpos[pos_idxs] = pos -------------------------------------------------------------------------------- /irl_control/osc.py: -------------------------------------------------------------------------------- 1 | from irl_control.robot import RobotState 2 | import numpy as np 3 | import mujoco_py as mjp 4 | from transforms3d.derivations.quaternions import qmult 5 | from transforms3d.quaternions import qconjugate 6 | from transforms3d.euler import quat2euler, euler2quat 7 | from transforms3d.utils import normalized_vector 8 | from typing import Dict, Tuple 9 | from irl_control import Robot, Device 10 | from irl_control.utils import ControllerConfig, Target 11 | from irl_control.device import DeviceState 12 | 13 | class OSC(): 14 | """ 15 | OSC provides Operational Space Control for a given Robot. 16 | This controller accepts targets as a input, and generates a control signal 17 | for the devices that are linked to the targets. 18 | """ 19 | def __init__(self, robot: Robot, sim, input_device_configs: Tuple[str, Dict], nullspace_config : Dict = None, use_g=True, admittance=False): 20 | self.sim = sim 21 | self.robot = robot 22 | 23 | # Create a dict, device_configs, which maps a device name to a 24 | # ControllerConfig. ControllerConfig is a lightweight wrapper 25 | # around the dict class to add some desired methods 26 | self.device_configs = dict() 27 | for dcnf in input_device_configs: 28 | self.device_configs[dcnf[0]] = ControllerConfig(dcnf[1]) 29 | self.nullspace_config = nullspace_config 30 | self.use_g = use_g 31 | self.admittance = admittance 32 | 33 | # Obtain the controller configuration parameters 34 | # and calculate the task space gains 35 | for device_name in self.device_configs.keys(): 36 | kv, kp, ko = self.device_configs[device_name].get_params(['kv', 'kp', 'ko']) 37 | task_space_gains = np.array([kp] * 3 + [ko] * 3) 38 | self.device_configs[device_name]['task_space_gains'] = task_space_gains 39 | self.device_configs[device_name]['lamb'] = task_space_gains / kv 40 | 41 | def __Mx(self, J, M): 42 | """ 43 | Returns the inverse of the task space inertia matrix 44 | Parameters 45 | ---------- 46 | J: Jacobian matrix 47 | M: inertia matrix 48 | """ 49 | M_inv = self.__svd_solve(M) 50 | Mx_inv = np.dot(J, np.dot(M_inv, J.T)) 51 | threshold = 1e-4 52 | if abs(np.linalg.det(Mx_inv)) >= threshold: 53 | Mx = self.__svd_solve(Mx_inv) 54 | else: 55 | Mx = np.linalg.pinv(Mx_inv, rcond=threshold*0.1) 56 | return Mx, M_inv 57 | 58 | 59 | def __svd_solve(self, A): 60 | """ 61 | Use the SVD Method to calculate the inverse of a matrix 62 | Parameters 63 | ---------- 64 | A: Matrix 65 | """ 66 | u, s, v = np.linalg.svd(A) 67 | Ainv = np.dot(v.transpose(), np.dot(np.diag(s**-1), u.transpose())) 68 | return Ainv 69 | 70 | def __limit_vel(self, u_task: np.ndarray, device: Device): 71 | """ 72 | Limit the velocity of the task space control vector 73 | Parameters 74 | ---------- 75 | u_task: array of length 6 corresponding to the task space control 76 | """ 77 | if device.max_vel is not None: 78 | kv, kp, ko, lamb = self.device_configs[device.name].get_params(['kv', 'kp', 'ko', 'lamb']) 79 | scale = np.ones(6) 80 | 81 | # Apply the sat gains to the x,y,z components 82 | norm_xyz = np.linalg.norm(u_task[:3]) 83 | sat_gain_xyz = device.max_vel[0] / kp * kv 84 | scale_xyz = device.max_vel[0] / kp * kv 85 | if norm_xyz > sat_gain_xyz: 86 | scale[:3] *= scale_xyz / norm_xyz 87 | 88 | # Apply the sat gains to the a,b,g components 89 | norm_abg = np.linalg.norm(u_task[3:]) 90 | sat_gain_abg = device.max_vel[1] / ko * kv 91 | scale_abg = device.max_vel[1] / ko * kv 92 | if norm_abg > sat_gain_abg: 93 | scale[3:] *= scale_abg / norm_abg 94 | u_task = kv * scale * lamb * u_task 95 | else: 96 | print("Device max_vel must be set in the yaml file!") 97 | raise Exception 98 | 99 | return u_task 100 | 101 | def calc_error(self, target: Target, device: Device): 102 | """ 103 | Compute the difference between the target and device EE 104 | for the x,y,z and a,b,g components 105 | """ 106 | u_task = np.zeros(6) 107 | # Calculate x,y,z error 108 | if np.sum(device.ctrlr_dof_xyz) > 0: 109 | diff = device.get_state(DeviceState.EE_XYZ) - target.get_xyz() 110 | u_task[:3] = diff 111 | 112 | # Calculate a,b,g error 113 | if np.sum(device.ctrlr_dof_abg) > 0: 114 | t_rot = target.get_quat() 115 | q_d = normalized_vector(t_rot) 116 | q_r = np.array(qmult(q_d, qconjugate(device.get_state(DeviceState.EE_QUAT)))) 117 | u_task[3:] = quat2euler(qconjugate(q_r)) # -q_r[1:] * np.sign(q_r[0]) 118 | return u_task 119 | 120 | def generate(self, targets: Dict[str, Target]): 121 | """ 122 | Generate forces for the corresponding devices which are in the 123 | robot's sub-devices. Accepts a dictionary of device names (keys), 124 | which map to a Target. 125 | Parameters 126 | ---------- 127 | targets: dict of device names mapping to Target objects 128 | """ 129 | if self.robot.is_using_sim() is False: 130 | assert self.robot.is_running(), "Robot must be running!" 131 | 132 | robot_state = self.robot.get_all_states() 133 | # Get the Jacobian for the all of devices passed in 134 | Js, J_idxs = robot_state[RobotState.J] 135 | # J, J_idxs = self.robot.get_jacobian(targets.keys()) 136 | J = np.array([]) 137 | for device_name in targets.keys(): 138 | J = np.vstack([J, Js[device_name]]) if J.size else Js[device_name] 139 | # Get the inertia matrix for the robot 140 | # M = self.robot.get_M() 141 | M = robot_state[RobotState.M] 142 | 143 | # Compute the inverse matrices used for task space operations 144 | Mx, M_inv = self.__Mx(J, M) 145 | 146 | # Initialize the control vectors and sim data needed for control calculations 147 | # dq = self.robot.get_dq() 148 | dq = robot_state[RobotState.DQ] 149 | 150 | dx = np.dot(J, dq) 151 | uv_all = np.dot(M, dq) 152 | u_all = np.zeros(self.robot.num_joints_total) 153 | u_task_all = np.array([]) 154 | ext_f = np.array([]) 155 | 156 | for device_name, target in targets.items(): 157 | device = self.robot.get_device(device_name) 158 | # Calculate the error from the device EE to target 159 | u_task = self.calc_error(target, device) 160 | stiffness = np.array(self.device_configs[device_name]['k'] + [1]*3) 161 | damping = np.array(self.device_configs[device_name]['d'] + [1]*3) 162 | # Apply gains to the error terms 163 | if device.max_vel is not None: 164 | u_task = self.__limit_vel(u_task, device) 165 | u_task *= stiffness 166 | else: 167 | task_space_gains = self.device_configs[device.name]['task_space_gains'] 168 | u_task *= task_space_gains * stiffness 169 | 170 | # Apply kv gain 171 | kv = self.device_configs[device.name]['kv'] 172 | target_vel = np.hstack([target.get_xyz_vel(), target.get_abg_vel()]) 173 | if np.all(target_vel) == 0: 174 | u_all[device.joint_ids_all] = -1 * kv * uv_all[device.joint_ids_all] 175 | else: 176 | diff = dx[J_idxs[device_name]] - np.array(target_vel)[device.ctrlr_dof] 177 | u_task[device.ctrlr_dof] += kv * diff * damping[device.ctrlr_dof] 178 | 179 | force = np.append(robot_state[device_name][DeviceState.FORCE], robot_state[device_name][DeviceState.TORQUE]) 180 | ext_f = np.append(ext_f, force[device.ctrlr_dof]) 181 | u_task_all = np.append(u_task_all, u_task[device.ctrlr_dof]) 182 | 183 | # Transform task space signal to joint space 184 | if self.admittance is True: 185 | u_all -= np.dot(J.T, np.dot(Mx, u_task_all+ext_f)) 186 | else: 187 | u_all -= np.dot(J.T, np.dot(Mx, u_task_all)) 188 | 189 | # Apply gravity forces 190 | if self.use_g: 191 | u_all += self.sim.data.qfrc_bias[self.robot.joint_ids_all] 192 | 193 | # Apply the nullspace controller using the specified parameters 194 | # (if passed to constructor / initialized) 195 | if self.nullspace_config is not None: 196 | damp_kv = self.nullspace_config['kv'] 197 | u_null = np.dot(M, -damp_kv*dq) 198 | Jbar = np.dot(M_inv, np.dot(J.T, Mx)) 199 | null_filter = np.eye(self.robot.num_joints_total) - np.dot(J.T, Jbar.T) 200 | u_all += np.dot(null_filter, u_null) 201 | 202 | # Return the forces and indices to apply the forces 203 | forces = [] 204 | force_idxs = [] 205 | for dev_name in targets.keys(): 206 | dev = self.robot.sub_devices_dict[dev_name] 207 | forces.append(u_all[dev.actuator_trnids]) 208 | force_idxs.append(dev.ctrl_idxs) 209 | 210 | return force_idxs, forces -------------------------------------------------------------------------------- /irl_control/robot.py: -------------------------------------------------------------------------------- 1 | from irl_control.device import DeviceState 2 | import numpy as np 3 | import mujoco_py as mjp 4 | import time 5 | from irl_control.device import Device, DeviceState 6 | from enum import Enum 7 | from threading import Lock 8 | from typing import Dict, Any, List 9 | import copy 10 | 11 | class RobotState(Enum): 12 | M = 'INERTIA' 13 | DQ = 'DQ' 14 | J = 'JACOBIAN' 15 | 16 | class Robot(): 17 | def __init__(self, sub_devices: List[Device], robot_name, sim, use_sim, collect_hz=1000): 18 | self.sim = sim 19 | self.__use_sim = use_sim 20 | self.sub_devices = sub_devices 21 | self.sub_devices_dict: Dict[str, Device] = dict() 22 | for dev in self.sub_devices: 23 | self.sub_devices_dict[dev.name] = dev 24 | 25 | self.name = robot_name 26 | self.num_scene_joints = self.sim.model.nv 27 | self.M_vec = np.zeros(self.num_scene_joints**2) 28 | self.joint_ids_all = np.array([], dtype=np.int32) 29 | for dev in self.sub_devices: 30 | self.joint_ids_all = np.hstack([self.joint_ids_all, dev.joint_ids_all]) 31 | self.joint_ids_all = np.sort(np.unique(self.joint_ids_all)) 32 | self.num_joints_total = len(self.joint_ids_all) 33 | self.running = False 34 | self.__state_locks: Dict[RobotState, Lock] = dict([(key, Lock()) for key in RobotState]) 35 | self.__state_var_map: Dict[RobotState, function] = { 36 | RobotState.M : lambda : self.__get_M(), 37 | RobotState.DQ : lambda : self.__get_dq(), 38 | RobotState.J : lambda : self.__get_jacobian() 39 | } 40 | self.__state: Dict[RobotState, Any] = dict() 41 | self.data_collect_hz = collect_hz 42 | 43 | 44 | def __get_jacobian(self): 45 | """ 46 | Return the Jacobians for all of the devices, 47 | so that OSC can stack them according to provided the target entries 48 | """ 49 | Js = dict() 50 | J_idxs = dict() 51 | start_idx = 0 52 | for name, device in self.sub_devices_dict.items(): 53 | J_sub = device.get_state(DeviceState.J) 54 | J_idxs[name] = np.arange(start_idx, start_idx + J_sub.shape[0]) 55 | start_idx += J_sub.shape[0] 56 | J_sub = J_sub[:, self.joint_ids_all] 57 | Js[name] = J_sub 58 | return Js, J_idxs 59 | 60 | def __get_dq(self): 61 | # dq = self.sim.data.qvel[self.joint_ids_all] 62 | dq = np.zeros(self.joint_ids_all.shape) 63 | for dev in self.sub_devices: 64 | dq[dev.get_all_joint_ids()] = dev.get_state(DeviceState.DQ) 65 | return dq 66 | 67 | 68 | def __get_M(self): 69 | mjp.cymj._mj_fullM(self.sim.model, self.M_vec, self.sim.data.qM) 70 | M = self.M_vec.reshape(self.num_scene_joints, self.num_scene_joints) 71 | M = M[np.ix_(self.joint_ids_all, self.joint_ids_all)] 72 | return M 73 | 74 | def get_state(self, state_var: RobotState): 75 | if self.__use_sim: 76 | func = self.__state_var_map[state_var] 77 | state = copy.copy(func()) 78 | else: 79 | self.__state_locks[state_var].acquire() 80 | state = copy.copy(self.__state[state_var]) 81 | self.__state_locks[state_var].release() 82 | return state 83 | 84 | def __set_state(self, state_var: RobotState): 85 | assert self.__use_sim is False 86 | self.__state_locks[state_var].acquire() 87 | func = self.__state_var_map[state_var] 88 | value = func() 89 | self.__state[state_var] = copy.copy(value) # Make sure to copy (or else reference will stick to Dict value) 90 | self.__state_locks[state_var].release() 91 | 92 | def is_running(self): 93 | return self.running 94 | 95 | def is_using_sim(self): 96 | return self.__use_sim 97 | 98 | def __update_state(self): 99 | assert self.__use_sim is False 100 | for var in RobotState: 101 | self.__set_state(var) 102 | 103 | def start(self): 104 | assert self.running is False and self.__use_sim is False 105 | self.running = True 106 | interval = float(1.0/float(self.data_collect_hz)) 107 | prev_time = time.time() 108 | while self.running: 109 | for dev in self.sub_devices: 110 | dev.update_state() 111 | self.__update_state() 112 | curr_time = time.time() 113 | diff = curr_time - prev_time 114 | delay = max(interval - diff, 0) 115 | time.sleep(delay) 116 | prev_time = curr_time 117 | 118 | def stop(self): 119 | assert self.running is True and self.__use_sim is False 120 | self.running = False 121 | 122 | def get_device(self, device_name: str) -> Device: 123 | return self.sub_devices_dict[device_name] 124 | 125 | def get_all_states(self): 126 | """ 127 | Get's the state of all the devices connected plus the robot states 128 | """ 129 | state = {} 130 | for device_name, device in self.sub_devices_dict.items(): 131 | state[device_name] = device.get_all_states() 132 | 133 | for key in RobotState: 134 | state[key] = self.get_state(key) 135 | 136 | return state 137 | 138 | def get_device_states(self): 139 | """ 140 | Get's the state of all the devices connected 141 | """ 142 | state = {} 143 | for device_name, device in self.sub_devices_dict.items(): 144 | state[device_name] = device.get_all_states() 145 | return state -------------------------------------------------------------------------------- /irl_control/robot_configs/default_xyz.yaml: -------------------------------------------------------------------------------- 1 | devices: 2 | - id: 0 3 | name: "base" 4 | max_vel: [0, 20] 5 | EE: "ur_stand_dummy" 6 | ctrlr_dof_xyz: [False, False, False] 7 | ctrlr_dof_abg: [False, False, True] 8 | start_angles: [0.0] 9 | num_gripper_joints: 0 10 | - id: 1 11 | name: "ur5right" 12 | max_vel: [1, 5] 13 | # start_body: "dual_ur_stand" 14 | EE: "ur_EE_ur5right" 15 | ctrlr_dof_xyz: [True, True, True] 16 | ctrlr_dof_abg: [False, False, False] 17 | start_angles: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 18 | num_gripper_joints: 6 19 | - id: 2 20 | name: "ur5left" 21 | max_vel: [1, 5] 22 | # start_body: "dual_ur_stand" 23 | EE: "ur_EE_ur5left" 24 | ctrlr_dof_xyz: [True, True, True] 25 | ctrlr_dof_abg: [False, False, False] 26 | start_angles: [0.126, -0.942, -1.88, -4.15, -4.78, 0.0] 27 | num_gripper_joints: 6 28 | robots: 29 | - id: 0 30 | name: "DualUR5" 31 | device_ids: [0,1,2] 32 | controller_configs: 33 | - name: "osc0" 34 | kp: 2000 35 | kv: 20 36 | ki: 1 37 | ko: 2000 38 | k: [1,2,3] 39 | d: [0.5,1,1] 40 | - name: "osc1" 41 | kp: 200 42 | kv: 50 43 | ki: 1 44 | ko: 200 45 | k: [1,2,3] 46 | d: [0.5,1,1] 47 | - name: "osc2" 48 | kp: 200 49 | kv: 20 50 | ki: 1 51 | ko: 200 52 | k: [1,2,3] 53 | d: [0.5,1,1] 54 | - name: "nullspace" 55 | kv: 10 56 | -------------------------------------------------------------------------------- /irl_control/robot_configs/default_xyz_abg.yaml: -------------------------------------------------------------------------------- 1 | devices: 2 | - id: 0 3 | name: "base" 4 | max_vel: [0, 20] 5 | EE: "ur_stand_dummy" 6 | ctrlr_dof_xyz: [False, False, False] 7 | ctrlr_dof_abg: [False, False, True] 8 | start_angles: [0.0] 9 | num_gripper_joints: 0 10 | - id: 1 11 | name: "ur5right" 12 | max_vel: [1, 5] 13 | # start_body: "dual_ur_stand" 14 | EE: "ur_EE_ur5right" 15 | ctrlr_dof_xyz: [True, True, True] 16 | ctrlr_dof_abg: [True, True, True] 17 | start_angles: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 18 | num_gripper_joints: 6 19 | - id: 2 20 | name: "ur5left" 21 | max_vel: [1, 5] 22 | # start_body: "dual_ur_stand" 23 | EE: "ur_EE_ur5left" 24 | ctrlr_dof_xyz: [True, True, True] 25 | ctrlr_dof_abg: [True, True, True] 26 | start_angles: [0.126, -0.942, -1.88, -4.15, -4.78, 0.0] 27 | num_gripper_joints: 6 28 | robots: 29 | - id: 0 30 | name: "DualUR5" 31 | device_ids: [0,1,2] 32 | controller_configs: 33 | - name: "osc0" 34 | kp: 2000 35 | kv: 20 36 | ki: 1 37 | ko: 2000 38 | k: [1,2,3] 39 | d: [0.5,1,1] 40 | - name: "osc1" 41 | kp: 200 42 | kv: 50 43 | ki: 1 44 | ko: 200 45 | k: [1,2,3] 46 | d: [0.5,1,1] 47 | - name: "osc2" 48 | kp: 200 49 | kv: 50 50 | ki: 1 51 | ko: 200 52 | k: [1,2,3] 53 | d: [0.5,1,1] 54 | - name: "nullspace" 55 | kv: 10 56 | -------------------------------------------------------------------------------- /irl_control/robot_configs/iros2022.yaml: -------------------------------------------------------------------------------- 1 | devices: 2 | - id: 0 3 | name: "base" 4 | max_vel: [0, 2] 5 | EE: "ur_stand_dummy" 6 | ctrlr_dof_xyz: [False, False, False] 7 | ctrlr_dof_abg: [False, False, True] 8 | start_angles: [-1.56] 9 | num_gripper_joints: 0 10 | - id: 1 11 | name: "ur5right" 12 | max_vel: [2.0, 5] 13 | start_body: "dual_ur_stand" 14 | EE: "ur_EE_ur5right" 15 | ctrlr_dof_xyz: [True, True, True] 16 | ctrlr_dof_abg: [True, True, True] 17 | start_angles: [-0.03614821, -0.27430234, -0.47910152, -0.14136462, -0.01368577, -0.54591325] 18 | num_gripper_joints: 6 19 | - id: 2 20 | name: "ur5left" 21 | max_vel: [2.0, 5] 22 | start_body: "dual_ur_stand" 23 | EE: "ur_EE_ur5left" 24 | ctrlr_dof_xyz: [True, True, True] 25 | ctrlr_dof_abg: [True, True, True] 26 | start_angles: [0.05044541, 0.18777629, 0.30305106, -3.10725317, 1.58646237, 2.71767586] 27 | num_gripper_joints: 6 28 | robots: 29 | - id: 0 30 | name: "DualUR5" 31 | device_ids: [0,1,2] 32 | controller_configs: 33 | - name: "osc0" 34 | kp: 200 35 | kv: 20 36 | ki: 1 37 | ko: 75 38 | k: [1,2,3] 39 | d: [0.5,1,1] 40 | - name: "osc1" 41 | kp: 200 42 | kv: 50 43 | ki: 1 44 | ko: 200 45 | k: [1,2,3] 46 | d: [0.5,1,1] 47 | - name: "osc2" 48 | kp: 200 49 | kv: 20 50 | ki: 1 51 | ko: 75 52 | k: [1,2,3] 53 | d: [0.5,1,1] 54 | - name: "nullspace" 55 | kv: 10 56 | -------------------------------------------------------------------------------- /irl_control/scenes/admit_test_scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /irl_control/scenes/force_test_scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 19 | -------------------------------------------------------------------------------- /irl_control/scenes/gain_test_scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /irl_control/scenes/insertion_task_scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /irl_control/scenes/iros2022.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /irl_control/scenes/ps_move_scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /irl_control/scenes/space_mouse_scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /irl_control/scenes/ur5.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 107 | 108 | 109 | 110 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | -------------------------------------------------------------------------------- /irl_control/scenes/world.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 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /irl_control/tests/run_tests.py: -------------------------------------------------------------------------------- 1 | assert True 2 | -------------------------------------------------------------------------------- /irl_control/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from typing import Any, List 3 | from transforms3d.euler import quat2euler, euler2quat 4 | 5 | class Target(): 6 | """ 7 | The Target class holds a target vector for both orientation (quaternion) and position (xyz) 8 | NOTE: Quat is stored as w, x, y, z 9 | """ 10 | def __init__(self, xyz_abg : List = np.zeros(6), xyz_abg_vel : List = np.zeros(6)): 11 | assert len(xyz_abg) == 6 and len(xyz_abg_vel) == 6 12 | self.__xyz = np.array(xyz_abg)[:3] 13 | self.__xyz_vel = np.array(xyz_abg_vel)[:3] 14 | self.__quat = np.array(euler2quat(*xyz_abg[3:])) 15 | self.__quat_vel = np.array(euler2quat(*xyz_abg_vel[3:])) 16 | 17 | def get_xyz(self): 18 | return self.__xyz 19 | 20 | def get_xyz_vel(self): 21 | return self.__xyz_vel 22 | 23 | def get_quat(self): 24 | return self.__quat 25 | 26 | def get_quat_vel(self): 27 | return np.asarray(self.__quat_vel) 28 | 29 | def get_abg(self): 30 | return np.asarray(quat2euler(self.__quat)) 31 | 32 | def get_abg_vel(self): 33 | return np.asarray(quat2euler(self.__quat_vel)) 34 | 35 | def set_xyz(self, xyz): 36 | assert len(xyz) == 3 37 | self.__xyz = np.asarray(xyz) 38 | 39 | def set_xyz_vel(self, xyz_vel): 40 | assert len(xyz_vel) == 3 41 | self.__xyz_vel = np.asarray(xyz_vel) 42 | 43 | def set_quat(self, quat): 44 | assert len(quat) == 4 45 | self.__quat = np.asarray(quat) 46 | 47 | def set_quat_vel(self, quat_vel): 48 | assert len(quat_vel) == 4 49 | self.__quat_vel = np.asarray(quat_vel) 50 | 51 | def set_abg(self, abg): 52 | assert len(abg) == 3 53 | self.__quat = np.asarray(euler2quat(*abg)) 54 | 55 | def set_abg_vel(self, abg_vel): 56 | assert len(abg_vel) == 3 57 | self.__quat_vel = np.asarray(euler2quat(*abg_vel)) 58 | 59 | def set_all_quat(self, xyz, quat): 60 | assert len(xyz) == 3 and len(quat) == 4 61 | self.__xyz = np.asarray(xyz) 62 | self.__quat = np.asarray(quat) 63 | 64 | def set_all_abg(self, xyz, abg): 65 | assert len(xyz) == 3 and len(abg) == 3 66 | self.__xyz = np.asarray(xyz) 67 | self.__quat = np.asarray(euler2quat(*abg)) 68 | 69 | class ControllerConfig(): 70 | def __init__(self, ctrlr_dict): 71 | self.ctrlr_dict = ctrlr_dict 72 | 73 | def __getitem__(self, __name: str) -> Any: 74 | return self.ctrlr_dict[__name] 75 | 76 | def get_params(self, keys): 77 | return [self.ctrlr_dict[key] for key in keys] 78 | 79 | def __setitem__(self, __name: str, __value: Any) -> None: 80 | self.ctrlr_dict[__name] = __value -------------------------------------------------------------------------------- /irl_control/version.py: -------------------------------------------------------------------------------- 1 | name = "irl_control" 2 | version_info = (1, 0, 0) # (major, minor, patch) 3 | dev = False 4 | 5 | v = ".".join(str(v) for v in version_info) 6 | dev_v = ".dev" if dev else "" 7 | 8 | version = f"{v}{dev_v}" 9 | -------------------------------------------------------------------------------- /requirements.in: -------------------------------------------------------------------------------- 1 | numpy 2 | PyYAML 3 | transforms3d -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | import runpy 3 | import pathlib 4 | 5 | root = pathlib.Path(__file__).parent 6 | version = runpy.run_path(str(root / "irl_control" / "version.py"))["version"] 7 | 8 | setup( 9 | name='irl_control', 10 | version=version, 11 | description='Control Suite for Bi-Manual Manipulation tasks in Mujoco', 12 | url='https://github.com/ir-lab/irl_control', 13 | author='Michael Drolet, Ravi Swaroop', 14 | author_email='mdrolet@asu.edu', 15 | license='MIT', 16 | packages=find_packages() 17 | ) --------------------------------------------------------------------------------