├── .gitignore ├── .readthedocs.yaml ├── LICENSE.txt ├── README.md ├── config ├── abb_1200_5_90_motion_program_robot_default_config.yml ├── abb_6700_150_320_motion_program_robot_default_config.yml └── abb_multimove_motion_program_robot_default_config.yml ├── docs ├── Makefile ├── abb_motion_program_exec │ ├── api │ │ ├── abb_motion_program_exec.rst │ │ └── abb_motion_program_exec_client_aio.rst │ └── api_reference.rst ├── conf.py ├── egm.md ├── figures │ ├── arm_logo.jpg │ ├── multimove │ │ ├── robotstudio_irb1200_select2.png │ │ ├── robotstudio_multimove_add_irb1200.png │ │ ├── robotstudio_multimove_base_frame1.png │ │ ├── robotstudio_multimove_base_frame5.png │ │ ├── robotstudio_multimove_base_frame6.png │ │ ├── robotstudio_multimove_controller_name_location.png │ │ ├── robotstudio_multimove_controller_options1.png │ │ ├── robotstudio_multimove_controller_options2.png │ │ ├── robotstudio_multimove_controller_options3.png │ │ ├── robotstudio_multimove_controller_options_finish.png │ │ ├── robotstudio_multimove_controller_tasks.png │ │ ├── robotstudio_multimove_controller_tasks3.png │ │ ├── robotstudio_multimove_installed1.png │ │ ├── robotstudio_multimove_installed2.png │ │ ├── robotstudio_multimove_move_irb1200.png │ │ ├── robotstudio_multimove_new_controller.png │ │ ├── robotstudio_multimove_new_solution.png │ │ ├── robotstudio_multimove_open_home.png │ │ ├── robotstudio_multimove_rapid_copied.png │ │ └── robotstudio_multimove_two_robots.png │ ├── nys_logo.jpg │ ├── robotstudio_addin_install_package.png │ ├── robotstudio_addin_installed1.png │ ├── robotstudio_addin_installed2.png │ ├── robotstudio_addin_installed3.png │ ├── robotstudio_addin_installed4.png │ ├── robotstudio_addin_installed5.png │ ├── robotstudio_addin_installed6.png │ ├── robotstudio_addin_installed7.png │ ├── robotstudio_addin_mp_installed.png │ ├── robotstudio_addin_robotware_irc5.png │ ├── robotstudio_addin_tab.png │ ├── robotstudio_change_options.png │ ├── robotstudio_change_options2.png │ ├── robotstudio_change_options3.png │ ├── robotstudio_install_manager1.png │ ├── robotstudio_install_manager2.png │ ├── robotstudio_install_manager3.png │ ├── robotstudio_install_manager4.png │ ├── robotstudio_irb1200_select.png │ ├── robotstudio_load_parameters.png │ ├── robotstudio_new_solution.png │ ├── robotstudio_open_home.png │ ├── robotstudio_rapid_copied.png │ ├── robotstudio_restart.png │ └── robotstudio_start_install_manager.png ├── index.rst ├── make.bat ├── readme.md ├── requirements.txt ├── robot_multimove_setup_manual.md ├── robot_setup.rst └── robot_setup_manual.md ├── examples ├── egm │ ├── egm_joint_target_example.py │ ├── egm_path_correction_example.py │ └── egm_pose_target_example.py ├── example.py ├── multimove_example.py ├── multimove_preempt_example.py ├── multimove_relative_example.py ├── preempt_example.py └── robotraconteur │ ├── example_client.py │ ├── example_client_confdata.py │ ├── example_client_egm_realtime_feedback.py │ └── multimove_example_client.py ├── pyproject.toml ├── robot ├── HOME │ ├── error_reporter.mod │ ├── motion_program_exec.mod │ ├── motion_program_exec_egm.mod │ ├── motion_program_logger.mod │ └── motion_program_shared.sys ├── config_params │ ├── EIO.cfg │ └── SYS.cfg ├── config_params_egm │ ├── EIO.cfg │ ├── MOC.cfg │ └── SYS.cfg └── config_params_multimove │ ├── EIO.cfg │ └── SYS.cfg └── src └── abb_motion_program_exec ├── __init__.py ├── abb_motion_program_exec_client.py ├── abb_motion_program_exec_client_aio.py ├── commands ├── __init__.py ├── command_base.py ├── commands.py ├── egm_commands.py ├── rapid_types.py └── util.py └── robotraconteur ├── __init__.py ├── __main__.py ├── _motion_program_conv.py ├── abb_motion_program_exec_robotraconteur.py ├── experimental.abb_robot.motion_program.robdef └── experimental.robotics.motion_program.robdef /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | 155 | # PyCharm 156 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 157 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 158 | # and can be added to the global gitignore or merged into this file. For a more nuclear 159 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 160 | #.idea/ 161 | 162 | docs/readme.md -------------------------------------------------------------------------------- /.readthedocs.yaml: -------------------------------------------------------------------------------- 1 | version: 2 2 | 3 | sphinx: 4 | configuration: docs/conf.py 5 | 6 | build: 7 | os: ubuntu-22.04 8 | tools: 9 | python: "3.10" 10 | 11 | python: 12 | install: 13 | - requirements: docs/requirements.txt -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright 2022 Wason Technology, LLC 190 | Rensselaer Polytechnic Institute 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. -------------------------------------------------------------------------------- /config/abb_1200_5_90_motion_program_robot_default_config.yml: -------------------------------------------------------------------------------- 1 | robot_info: 2 | device_info: 3 | device: 4 | name: abb_robot 5 | manufacturer: 6 | name: ABB 7 | uuid: ee260e78-d731-415a-84ca-4b02c487410c 8 | model: 9 | name: ABB_1200_5_90 10 | uuid: 9ad3c0ee-ec5e-4057-a297-d340e1484f01 11 | user_description: ABB Robot 12 | serial_number: 123456789 13 | device_classes: 14 | - class_identifier: 15 | name: robot 16 | uuid: 39b513e7-21b9-4b49-8654-7537473030eb 17 | subclasses: 18 | - serial 19 | - serial_six_axis 20 | implemented_types: 21 | - experimental.robotics.motion_program.MotionProgramRobot 22 | robot_type: serial 23 | robot_capabilities: 24 | - jog_command 25 | - trajectory_command 26 | - position_command 27 | chains: 28 | - kin_chain_identifier: robot_arm 29 | H: 30 | - x: 0.0 31 | y: 0.0 32 | z: 1.0 33 | - x: 0.0 34 | y: 1.0 35 | z: 0.0 36 | - x: 0.0 37 | y: 1.0 38 | z: 0.0 39 | - x: 1.0 40 | y: 0.0 41 | z: 0.0 42 | - x: 0.0 43 | y: 1.0 44 | z: 0.0 45 | - x: 1.0 46 | y: 0.0 47 | z: 0.0 48 | P: 49 | - x: 0.0 50 | y: 0.0 51 | z: 0.3991 52 | - x: 0.0 53 | y: 0.0 54 | z: 0.0 55 | - x: 0.0 56 | y: 0.0 57 | z: 0.448 58 | - x: 0.0 59 | y: 0.0 60 | z: 0.042 61 | - x: 0.451 62 | y: 0.0 63 | z: 0.0 64 | - x: 0.082 65 | y: 0.0 66 | z: 0.0 67 | - x: 0.0 68 | y: 0.0 69 | z: 0.0 70 | flange_identifier: tool0 71 | flange_pose: 72 | orientation: 73 | w: 0.7071067811882787 74 | x: 0.0 75 | y: 0.7071067811848163 76 | z: 0.0 77 | position: 78 | x: 0.0 79 | y: 0.0 80 | z: 0.0 81 | joint_numbers: 82 | - 0 83 | - 1 84 | - 2 85 | - 3 86 | - 4 87 | - 5 88 | joint_info: 89 | - default_effort_units: newton_meter 90 | default_units: radian 91 | joint_identifier: joint_1 92 | joint_limits: 93 | effort: 1000.0 94 | lower: -2.967 95 | upper: 2.967 96 | velocity: 5.027 97 | acceleration: 10 98 | joint_type: revolute 99 | - default_effort_units: newton_meter 100 | default_units: radian 101 | joint_identifier: joint_2 102 | joint_limits: 103 | effort: 1000.0 104 | lower: -1.745 105 | upper: 2.269 106 | velocity: 4.189 107 | acceleration: 15 108 | joint_type: revolute 109 | - default_effort_units: newton_meter 110 | default_units: radian 111 | joint_identifier: joint_3 112 | joint_limits: 113 | effort: 1000.0 114 | lower: -3.491 115 | upper: 1.222 116 | velocity: 5.236 117 | acceleration: 15 118 | joint_type: revolute 119 | - default_effort_units: newton_meter 120 | default_units: radian 121 | joint_identifier: joint_4 122 | joint_limits: 123 | effort: 1000.0 124 | lower: -4.712 125 | upper: 4.712 126 | velocity: 6.981 127 | acceleration: 20 128 | joint_type: revolute 129 | - default_effort_units: newton_meter 130 | default_units: radian 131 | joint_identifier: joint_5 132 | joint_limits: 133 | effort: 1000.0 134 | lower: -2.269 135 | upper: 2.269 136 | velocity: 7.069 137 | acceleration: 20 138 | joint_type: revolute 139 | - default_effort_units: newton_meter 140 | default_units: radian 141 | joint_identifier: joint_6 142 | joint_limits: 143 | effort: 1000.0 144 | lower: -6.283 145 | upper: 6.283 146 | velocity: 10.472 147 | acceleration: 20 148 | joint_type: revolute 149 | supported_commands: 150 | - MoveAbsJ 151 | - MoveJ 152 | - MoveL 153 | - MoveC 154 | - WaitTime 155 | - SetTool 156 | - SetPayload 157 | -------------------------------------------------------------------------------- /config/abb_6700_150_320_motion_program_robot_default_config.yml: -------------------------------------------------------------------------------- 1 | robot_info: 2 | device_info: 3 | device: 4 | name: abb_robot 5 | manufacturer: 6 | name: ABB 7 | uuid: ee260e78-d731-415a-84ca-4b02c487410c 8 | model: 9 | name: ABB_IRB6700_150_320 10 | uuid: 9d43d745-dbea-4a88-ac18-43a64bca67eb 11 | user_description: ABB Robot 12 | serial_number: 123456789 13 | device_classes: 14 | - class_identifier: 15 | name: robot 16 | uuid: 39b513e7-21b9-4b49-8654-7537473030eb 17 | subclasses: 18 | - serial 19 | - serial_six_axis 20 | implemented_types: 21 | - com.robotraconteur.robotics.robot.Robot 22 | robot_type: serial 23 | robot_capabilities: 24 | - jog_command 25 | - trajectory_command 26 | - position_command 27 | chains: 28 | - H: 29 | - x: 0.0 30 | y: 0.0 31 | z: 1.0 32 | - x: 0.0 33 | y: 1.0 34 | z: 0.0 35 | - x: 0.0 36 | y: 1.0 37 | z: 0.0 38 | - x: 1.0 39 | y: 0.0 40 | z: 0.0 41 | - x: 0.0 42 | y: 1.0 43 | z: 0.0 44 | - x: 1.0 45 | y: 0.0 46 | z: 0.0 47 | P: 48 | - x: 0.0 49 | y: 0.0 50 | z: 0.78 51 | - x: 0.32 52 | y: 0.0 53 | z: 0.0 54 | - x: 0.0 55 | y: 0.0 56 | z: 1.280 57 | - x: 0.0 58 | y: 0.0 59 | z: 0.2 60 | - x: 1.5925 61 | y: 0.0 62 | z: 0.0 63 | - x: 0.2 64 | y: 0.0 65 | z: 0.0 66 | - x: 0.0 67 | y: 0.0 68 | z: 0.0 69 | flange_identifier: tool0 70 | flange_pose: 71 | orientation: 72 | w: 0.707107 73 | x: 0.0 74 | y: 0.707107 75 | z: 0.0 76 | position: 77 | x: 0.0 78 | y: 0.0 79 | z: 0.0 80 | joint_numbers: 81 | - 0 82 | - 1 83 | - 2 84 | - 3 85 | - 4 86 | - 5 87 | kin_chain_identifier: robot_arm 88 | joint_info: 89 | - default_effort_units: newton_meter 90 | default_units: radian 91 | joint_identifier: joint_1 92 | joint_limits: 93 | effort: 0.0 94 | lower: -2.96706 95 | upper: 2.96706 96 | velocity: 1.745329 97 | acceleration: 10 98 | joint_type: revolute 99 | passive: false 100 | - default_effort_units: newton_meter 101 | default_units: radian 102 | joint_identifier: joint_2 103 | joint_limits: 104 | effort: 0.0 105 | lower: -1.134464 106 | upper: 1.48353 107 | velocity: 1.570796 108 | acceleration: 15 109 | joint_type: revolute 110 | passive: false 111 | - default_effort_units: newton_meter 112 | default_units: radian 113 | joint_identifier: joint_3 114 | joint_limits: 115 | effort: 0.0 116 | lower: -3.141593 117 | upper: 1.22173 118 | velocity: 1.570796 119 | acceleration: 15 120 | joint_type: revolute 121 | passive: false 122 | - default_effort_units: newton_meter 123 | default_units: radian 124 | joint_identifier: joint_4 125 | joint_limits: 126 | effort: 0.0 127 | lower: -5.235988 128 | upper: 5.235988 129 | velocity: 2.96706 130 | acceleration: 20 131 | joint_type: revolute 132 | passive: false 133 | - default_effort_units: newton_meter 134 | default_units: radian 135 | joint_identifier: joint_5 136 | joint_limits: 137 | effort: 0.0 138 | lower: -2.268928 139 | upper: 2.268928 140 | velocity: 2.094395 141 | acceleration: 20 142 | joint_type: revolute 143 | passive: false 144 | - default_effort_units: newton_meter 145 | default_units: radian 146 | joint_identifier: joint_6 147 | joint_limits: 148 | effort: 0.0 149 | lower: -6.283185 150 | upper: 6.283185 151 | velocity: 3.316126 152 | acceleration: 20 153 | joint_type: revolute 154 | passive: false 155 | supported_commands: 156 | - MoveAbsJ 157 | - MoveJ 158 | - MoveL 159 | - MoveC 160 | - WaitTime 161 | - SetTool 162 | - SetPayload -------------------------------------------------------------------------------- /config/abb_multimove_motion_program_robot_default_config.yml: -------------------------------------------------------------------------------- 1 | robot_info: 2 | device_info: 3 | device: 4 | name: abb_robot_multimove 5 | manufacturer: 6 | name: ABB 7 | uuid: ee260e78-d731-415a-84ca-4b02c487410c 8 | model: 9 | name: multimove_robot 10 | uuid: a3c3a2e3-d4b4-4d2b-a51c-56516fbb8104 11 | user_description: ABB Multimove Robot 12 | serial_number: 123456789 13 | device_classes: 14 | - class_identifier: 15 | name: robot 16 | uuid: 39b513e7-21b9-4b49-8654-7537473030eb 17 | subclasses: 18 | - multimove 19 | implemented_types: 20 | - experimental.robotics.motion_program.MotionProgramRobot 21 | robot_type: serial 22 | robot_capabilities: 23 | - jog_command 24 | - trajectory_command 25 | - position_command 26 | chains: 27 | - kin_chain_identifier: robot_arm1 28 | H: 29 | - x: 0.0 30 | y: 0.0 31 | z: 1.0 32 | - x: 0.0 33 | y: 1.0 34 | z: 0.0 35 | - x: 0.0 36 | y: 1.0 37 | z: 0.0 38 | - x: 1.0 39 | y: 0.0 40 | z: 0.0 41 | - x: 0.0 42 | y: 1.0 43 | z: 0.0 44 | - x: 1.0 45 | y: 0.0 46 | z: 0.0 47 | P: 48 | - x: 0.0 49 | y: 0.0 50 | z: 0.3991 51 | - x: 0.0 52 | y: 0.0 53 | z: 0.0 54 | - x: 0.0 55 | y: 0.0 56 | z: 0.448 57 | - x: 0.0 58 | y: 0.0 59 | z: 0.042 60 | - x: 0.451 61 | y: 0.0 62 | z: 0.0 63 | - x: 0.082 64 | y: 0.0 65 | z: 0.0 66 | - x: 0.0 67 | y: 0.0 68 | z: 0.0 69 | flange_identifier: robot1_tool0 70 | flange_pose: 71 | orientation: 72 | w: 0.7071067811882787 73 | x: 0.0 74 | y: 0.7071067811848163 75 | z: 0.0 76 | position: 77 | x: 0.0 78 | y: 0.0 79 | z: 0.0 80 | joint_numbers: 81 | - 0 82 | - 1 83 | - 2 84 | - 3 85 | - 4 86 | - 5 87 | - kin_chain_identifier: robot_arm2 88 | H: 89 | - x: 0.0 90 | y: 0.0 91 | z: 1.0 92 | - x: 0.0 93 | y: 1.0 94 | z: 0.0 95 | - x: 0.0 96 | y: 1.0 97 | z: 0.0 98 | - x: 1.0 99 | y: 0.0 100 | z: 0.0 101 | - x: 0.0 102 | y: 1.0 103 | z: 0.0 104 | - x: 1.0 105 | y: 0.0 106 | z: 0.0 107 | P: 108 | - x: 0.0 109 | y: 0.0 110 | z: 0.780 111 | - x: 0.320 112 | y: 0.0 113 | z: 0.0 114 | - x: 0.0 115 | y: 0.0 116 | z: 1.075 117 | - x: 0.0 118 | y: 0.0 119 | z: 0.200 120 | - x: 1.1425 121 | y: 0.0 122 | z: 0.0 123 | - x: 0.200 124 | y: 0.0 125 | z: 0.0 126 | - x: 0.0 127 | y: 0.0 128 | z: 0.0 129 | flange_identifier: robot2_tool0 130 | flange_pose: 131 | orientation: 132 | w: 0.7071067811882787 133 | x: 0.0 134 | y: 0.7071067811848163 135 | z: 0.0 136 | position: 137 | x: 0.0 138 | y: 0.0 139 | z: 0.0 140 | joint_numbers: 141 | - 6 142 | - 7 143 | - 8 144 | - 9 145 | - 10 146 | - 11 147 | joint_info: 148 | - default_effort_units: newton_meter 149 | default_units: radian 150 | joint_identifier: joint_1 151 | joint_limits: 152 | effort: 1000.0 153 | lower: -2.967 154 | upper: 2.967 155 | velocity: 5.027 156 | acceleration: 10 157 | joint_type: revolute 158 | - default_effort_units: newton_meter 159 | default_units: radian 160 | joint_identifier: joint_2 161 | joint_limits: 162 | effort: 1000.0 163 | lower: -1.745 164 | upper: 2.269 165 | velocity: 4.189 166 | acceleration: 15 167 | joint_type: revolute 168 | - default_effort_units: newton_meter 169 | default_units: radian 170 | joint_identifier: joint_3 171 | joint_limits: 172 | effort: 1000.0 173 | lower: -3.491 174 | upper: 1.222 175 | velocity: 5.236 176 | acceleration: 15 177 | joint_type: revolute 178 | - default_effort_units: newton_meter 179 | default_units: radian 180 | joint_identifier: joint_4 181 | joint_limits: 182 | effort: 1000.0 183 | lower: -4.712 184 | upper: 4.712 185 | velocity: 6.981 186 | acceleration: 20 187 | joint_type: revolute 188 | - default_effort_units: newton_meter 189 | default_units: radian 190 | joint_identifier: joint_5 191 | joint_limits: 192 | effort: 1000.0 193 | lower: -2.269 194 | upper: 2.269 195 | velocity: 7.069 196 | acceleration: 20 197 | joint_type: revolute 198 | - default_effort_units: newton_meter 199 | default_units: radian 200 | joint_identifier: joint_6 201 | joint_limits: 202 | effort: 1000.0 203 | lower: -6.283 204 | upper: 6.283 205 | velocity: 10.472 206 | acceleration: 20 207 | joint_type: revolute 208 | - default_effort_units: newton_meter 209 | default_units: radian 210 | joint_identifier: joint_1 211 | joint_limits: 212 | effort: 1000.0 213 | lower: -2.967 214 | upper: 2.967 215 | velocity: 1.745 216 | acceleration: 10 217 | joint_type: revolute 218 | passive: false 219 | - default_effort_units: newton_meter 220 | default_units: radian 221 | joint_identifier: joint_2 222 | joint_limits: 223 | effort: 1000.0 224 | lower: -1.134 225 | upper: 1.484 226 | velocity: 1.571 227 | acceleration: 15 228 | joint_type: revolute 229 | passive: false 230 | - default_effort_units: newton_meter 231 | default_units: radian 232 | joint_identifier: joint_3 233 | joint_limits: 234 | effort: 1000.0 235 | lower: -3.142 236 | upper: 1.222 237 | velocity: 1.571 238 | acceleration: 15 239 | joint_type: revolute 240 | passive: false 241 | - default_effort_units: newton_meter 242 | default_units: radian 243 | joint_identifier: joint_4 244 | joint_limits: 245 | effort: 1000.0 246 | lower: -5.236 247 | upper: 5.236 248 | velocity: 3.316 249 | acceleration: 20 250 | joint_type: revolute 251 | passive: false 252 | - default_effort_units: newton_meter 253 | default_units: radian 254 | joint_identifier: joint_5 255 | joint_limits: 256 | effort: 1000.0 257 | lower: -2.094 258 | upper: 2.094 259 | velocity: 2.443 260 | acceleration: 20 261 | joint_type: revolute 262 | passive: false 263 | - default_effort_units: newton_meter 264 | default_units: radian 265 | joint_identifier: joint_6 266 | joint_limits: 267 | effort: 1000.0 268 | lower: -6.283 269 | upper: 6.283 270 | velocity: 3.316 271 | acceleration: 20 272 | joint_type: revolute 273 | passive: false 274 | supported_commands: 275 | - MoveAbsJ 276 | - MoveJ 277 | - MoveL 278 | - MoveC 279 | - WaitTime 280 | - SetTool 281 | - SetPayload 282 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = source 9 | BUILDDIR = build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /docs/abb_motion_program_exec/api/abb_motion_program_exec.rst: -------------------------------------------------------------------------------- 1 | abb_motion_program_exec 2 | ======================= 3 | 4 | ABB IRC5 Controller Motion Program command client. 5 | 6 | This module contains types found on the ABB Robot Controller. Documentation is taken from the 7 | ABB Robotics manual "Technical reference manual RAPID Instructions, Functions and Data types, 8 | Document ID: 3HAC 16581-1". Note that most of the following data structures are all direct copies of the 9 | underlying ABB types. 10 | 11 | abb_motion_program_exec 12 | ----------------------- 13 | 14 | .. automodule:: abb_motion_program_exec 15 | :members: speeddata, zonedata, jointtarget, pose, confdata, robtarget, loaddata, CirPathModeSwitch, tooldata, 16 | wobjdata, egm_minmax, EGMStreamConfig, EGMJointTargetConfig, egmframetype, EGMPoseTargetConfig, 17 | EGMPathCorrectionConfig, MotionProgramExecClient 18 | 19 | .. autoclass:: MotionProgram 20 | :members: 21 | 22 | .. method:: MoveAbsJ(to_joint_pos: jointtarget, speed: speeddata, zone: zonedata) 23 | 24 | Append Move Absolute Joint command to motion program 25 | 26 | :param to_joint_pos: The destination absolute joint position of the robot and external axes. 27 | :type to_joint_pos: jointtarget 28 | :param speed: The speed data that applies to movements. 29 | :type speed: speeddata 30 | :param zone: Zone data for the movement. 31 | :type zone: zonedata 32 | 33 | .. method:: MoveJ(to_point: robtarget, speed: speeddata, zone: zonedata) 34 | 35 | Append move to position in joint space command to motion program 36 | 37 | :param to_point: The destination point of the robot and external axes. 38 | :type to_point: robtarget 39 | :param speed: The speed data that applies to movements. 40 | :type speed: speeddata 41 | :param zone: Zone data for the movement. 42 | :type zone: zonedata 43 | 44 | .. method:: MoveL(to_point: robtarget, speed: speeddata, zone: zonedata) 45 | 46 | Append move to position in straight line command to motion program 47 | 48 | :param to_point: The destination point of the robot and external axes. 49 | :type to_point: robtarget 50 | :param speed: The speed data that applies to movements. 51 | :type speed: speeddata 52 | :param zone: Zone data for the movement. 53 | :type zone: zonedata 54 | 55 | .. method:: MoveC(cir_point: robtarget, to_point: robtarget, speed: speeddata, zone: zonedata) 56 | 57 | Append move to position circularly through a point command to motion program 58 | 59 | :param cir_point: The circle point of the robot and external axes. 60 | :type cir_point: robtarget 61 | :param to_point: The destination point of the robot and external axes. 62 | :type to_point: robtarget 63 | :param speed: The speed data that applies to movements. 64 | :type speed: speeddata 65 | :param zone: Zone data for the movement. 66 | :type zone: zonedata 67 | 68 | .. method:: WaitTime(t: float) 69 | 70 | Append wait for a specified time in seconds command 71 | 72 | :param t: The time to wait in seconds 73 | :type t: float 74 | 75 | .. method:: CirPathMode(switch: CirPathModeSwitch) 76 | 77 | Append select reorientation method during circular moves command 78 | 79 | :param switch: The selected circular reorientation method 80 | :type switch: CirPathModeSwitch 81 | 82 | .. method:: SyncMoveOn() 83 | 84 | Append enable synchronous move command. Only valid on MultiMove systems 85 | 86 | .. method:: SyncMoveOff() 87 | 88 | Append disable synchronous move command. Only valid on MultiMove system 89 | 90 | .. method:: EGMRunJoint(cond_time: float, ramp_in_time: float, ramp_out_time: float) 91 | 92 | Append run EGM joint target streaming motion command. Use ``MotionProgramExecClient.stop_egm()`` to 93 | exit EGM command mode. 94 | 95 | :param cond_time: Condition time for the move. Set to a very large number to continue running until stopped. 96 | :type cond_time: float 97 | :param ramp_in_time: Motion ramp in time. Normally set to minimum value of 0.05 98 | :type ramp_in_tme: float 99 | :param ramp_out_time: Motion ramp out time. Normally set to minimum value of 0.05 100 | :type ramp_out_tme: float 101 | 102 | .. method:: EGMRunPose(cond_time: float, ramp_in_time: float, ramp_out_time: float) 103 | 104 | Apnned run EGM pose target streaming motion command. Use ``MotionProgramExecClient.stop_egm()`` to exit 105 | EGM command mode. 106 | 107 | :param cond_time: Condition time for the move. Set to a very large number to continue running until stopped. 108 | :type cond_time: float 109 | :param ramp_in_time: Motion ramp in time. Normally set to minimum value of 0.05 110 | :type ramp_in_tme: float 111 | :param ramp_out_time: Motion ramp out time. Normally set to minimum value of 0.05 112 | :type ramp_out_tme: float 113 | :param offset: Offset of commanded pose. Typically set to zero offset and no rotation 114 | :type offset: pose 115 | 116 | .. method:: EGMMoveL(to_point: robtarget, speed: speeddata, zone: zonedata) 117 | 118 | Append move to position in straight line with EGM correction command to motion program 119 | 120 | :param to_point: The destination point of the robot and external axes. 121 | :type to_point: robtarget 122 | :param speed: The speed data that applies to movements. 123 | :type speed: speeddata 124 | :param zone: Zone data for the movement. 125 | :type zone: zonedata 126 | 127 | 128 | .. method:: EGMMoveC(cir_point: robtarget, to_point: robtarget, speed: speeddata, zone: zonedata) 129 | 130 | Append move to position circularly through a point with EGM correction command to motion program 131 | 132 | :param cir_point: The circle point of the robot and external axes. 133 | :type cir_point: robtarget 134 | :param to_point: The destination point of the robot and external axes. 135 | :type to_point: robtarget 136 | :param speed: The speed data that applies to movements. 137 | :type speed: speeddata 138 | :param zone: Zone data for the movement. 139 | :type zone: zonedata 140 | 141 | -------------------------------------------------------------------------------- /docs/abb_motion_program_exec/api/abb_motion_program_exec_client_aio.rst: -------------------------------------------------------------------------------- 1 | abb_motion_program_exec_client_aio 2 | ========================================================== 3 | 4 | ABB IRC5 Controller Motion Program AsyncIO command client. 5 | 6 | This module contains types found on the ABB Robot Controller. Documentation is taken from the 7 | ABB Robotics manual "Technical reference manual RAPID Instructions, Functions and Data types, 8 | Document ID: 3HAC 16581-1". Note that most of the following data structures are all direct copies of the 9 | underlying ABB types. 10 | 11 | This module contains the ``MotionProgramExecClientAIO`` class. This class is functionally identical to 12 | ``MotionProgramExecClient``, except it uses AsyncIO instead of synchronous blocking operations. This module 13 | does not contain the various RAPID types. See :mod:`abb_motion_program_exec` for details on these classes. They 14 | should be imported from :mod:`abb_motion_program_exec`. 15 | 16 | 17 | abb_motion_program_exec.abb_motion_program_exec_client_aio 18 | ---------------------------------------------------------- 19 | 20 | .. automodule:: abb_motion_program_exec.abb_motion_program_exec_client_aio 21 | :members: -------------------------------------------------------------------------------- /docs/abb_motion_program_exec/api_reference.rst: -------------------------------------------------------------------------------- 1 | API Reference 2 | ================ 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | 7 | api/abb_motion_program_exec 8 | api/abb_motion_program_exec_client_aio -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # For the full list of built-in configuration values, see the documentation: 4 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 5 | 6 | # -- Project information ----------------------------------------------------- 7 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information 8 | 9 | project = 'abb_motion_program_exec' 10 | copyright = '2022, Wason Technology LLC, Rensselaer Polytechnic Institute' 11 | author = 'John Wason' 12 | 13 | import abb_motion_program_exec 14 | 15 | # The short X.Y version. 16 | # version = abb_motion_program_exec.__version__ 17 | # The full version, including alpha/beta/rc tags. 18 | # release = abb_motion_program_exec.__version__ 19 | 20 | # -- General configuration --------------------------------------------------- 21 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration 22 | 23 | extensions = [ 24 | "sphinx.ext.autodoc", 25 | "sphinx_autodoc_typehints", 26 | "recommonmark" 27 | ] 28 | 29 | source_suffix = [".rst", ".md"] 30 | 31 | templates_path = ['_templates'] 32 | exclude_patterns = [] 33 | 34 | 35 | 36 | # -- Options for HTML output ------------------------------------------------- 37 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output 38 | 39 | html_theme = "sphinx_rtd_theme" 40 | html_static_path = ['_static'] 41 | 42 | # https://www.lieret.net/2021/05/20/include-readme-sphinx/ 43 | import pathlib 44 | 45 | # The readme that already exists 46 | readme_path = pathlib.Path(__file__).parent.resolve().parent / "README.md" 47 | # We copy a modified version here 48 | readme_target = pathlib.Path(__file__).parent / "readme.md" 49 | 50 | with readme_target.open("w") as outf: 51 | # Change the title to "Readme" 52 | outf.write( 53 | "\n".join( 54 | [ 55 | "Readme", 56 | "======", 57 | ] 58 | ) 59 | ) 60 | lines = [] 61 | for line in readme_path.read_text().split("\n"): 62 | if line.startswith("# "): 63 | # Skip title, because we now use "Readme" 64 | # Could also simply exclude first line for the same effect 65 | continue 66 | line = line.replace("docs/figures", "figures/") 67 | line = line.replace("docs/", "") 68 | lines.append(line) 69 | outf.write("\n".join(lines)) 70 | -------------------------------------------------------------------------------- /docs/egm.md: -------------------------------------------------------------------------------- 1 | # EGM Support 2 | 3 | Externally Guided Motion (EGM) is an optional feature for ABB robots to provide real-time feedback and position 4 | control. A UDP and protobuf based protocol is used to communicate with an external computer, with feedback 5 | data sent to the computer from the computer, and commands sent to robot, typically at an update rate of 250 Hz. 6 | EGM is a complex feature, and users should refer to the official documentation *Application Manual Externall Guided 7 | Motion Document ID: 3HAC073318-001* before attempting to use EGM. 8 | 9 | **Warning: Using EGM bypasses many safety features of the robot, including collision avoidance. Use of EGM can 10 | be extremely hazardous if done carelessly. Software should be tested in simulation in RobotStudio before being run on 11 | a physical robot.** 12 | 13 | `abb_motion_program_exec` provides commands to initiate and execute EGM RAPID commands. The `abb_robot_client` `EGM` 14 | class is used to communicate with EGM. When configuring the robot for EGM, the configuration files from the 15 | `/robot/config_params_egm` must be loaded instead of the normal configuration 16 | `/robot/config_params`. 17 | 18 | The EGM is configured in two places under the controller configuration tree. "Communication" -> "Transmission Protocol". 19 | There should be a `UCdevice" listed. Double click on the row, and set the "Remote Address" to the IP address 20 | of the computer running `abb_motion_program_exec`. You may need to disable the firewall of the computer to allow 21 | incoming UDP packets. The computer will automatically send response packets back to the robot without needing 22 | to configure the robot's information. 23 | 24 | The second configuration is under "Motion" -> "External Motion Interface Data". This table configures profiles for the 25 | performance of the EGM. Three profiles are loaded during `abb_motion_program_exec` setup: `joint`, `pose`, 26 | and `path_corr`. These are used for joint target, pose target, and path correction respectively. The joint and pose 27 | targets are configured to be raw setpoint control with minimal filtering. These parameters can be modified depending 28 | on project requirements. 29 | 30 | ## EGM Operational Modes 31 | 32 | EGM can operate in three modes: 33 | * Joint target: Joint targets are sent to the robot 34 | * Pose target: Pose targets are sent to the robot, using a specified tool and work object frame 35 | * Path correction: Special commands `EGMMoveL` and `EGMMoveC` are used to create motion programs. The EGM can then 36 | correct the position of the end effector by providing an offset in path coordinates. 37 | 38 | ### EGM Joint Target control 39 | 40 | The simplest control method is joint control. The computer streams an array of joint array every 4 ms to the 41 | robot, and the robot will track the requested joint target. The settings loaded during setup will attempt to minimize 42 | the latency and filtering, but due to the limitations of EGM there may be significant latency, and this should 43 | be considered when designing an operation using EGM. Feedback controllers may become unstable due to unexpectedly high 44 | latency. The `joint` EGM config is used. 45 | 46 | See `examples/egm/egm_joint_target_example.py` for an example using joint control. 47 | 48 | Joint target control is initialized by passing `EGMJointTargetConfig` to the constructor of `MotionProgram`: 49 | 50 | ```python 51 | mm = abb.egm_minmax(-1e-3,1e-3) 52 | egm_config = abb.EGMJointTargetConfig( 53 | mm, mm, mm, mm, mm ,mm, 1000, 1000 54 | ) 55 | mp = abb.MotionProgram(egm_config = egm_config) 56 | ``` 57 | 58 | This configuration will initialize the EGM for use when the motion program is loaded on the controller. It uses 59 | the ABB Rapid commands `EGMSetupUC` and `EGMActJoint` to prepare for joint target control. See 60 | `EGMActJoint` in the ABB manual for the meaning of the parameters sent to `EGMJointTargetConfig`. 61 | 62 | Next, run the EGM operation using `EGMRunJoint` and non-blocking motion program execution. It is recommended 63 | to move the robot to a known-safe location before starting EGM. The `EGM` class from `abb_robot_client` is used 64 | to receive robot state and send the joint command. Note that joint units are in degrees! The example 65 | sends a sine wave into the joints. Once done, call `client.stop_egm()` to break out of EGM control. The rest of 66 | this snippet waits for the program to stop and collects the log results. 67 | 68 | ```python 69 | mp.MoveAbsJ(j1,abb.v5000,abb.fine) 70 | mp.EGMRunJoint(10, 0.05, 0.05) 71 | 72 | client = abb.MotionProgramExecClient(base_url="http://127.0.0.1:80") 73 | lognum = client.execute_motion_program(mp, wait=False) 74 | 75 | t1 = time.perf_counter() 76 | 77 | egm = EGM() 78 | t2 = t1 79 | while (t2 - t1) < 5 : 80 | t2 = time.perf_counter() 81 | res, feedback = egm.receive_from_robot(timeout=0.05) 82 | if res: 83 | egm.send_to_robot(np.ones((6,))*np.sin(t2-t1)*5) 84 | 85 | 86 | client.stop_egm() 87 | 88 | while client.is_motion_program_running(): 89 | time.sleep(0.05) 90 | 91 | log_results = client.read_motion_program_result_log(lognum) 92 | ``` 93 | 94 | ### EGM Pose Target control 95 | 96 | The pose target control mode streams a desired robot pose every 4 ms. The robot will use the "closest" robot joint 97 | configuration. The user must be careful to avoid singularities. The settings loaded during setup will attempt to minimize 98 | the latency and filtering, but due to the limitations of EGM there may be significant latency, and this should 99 | be considered when designing an operation using EGM. Feedback controllers may become unstable due to unexpectedly high 100 | latency. The `pose` EGM config is used. 101 | 102 | See `examples/egm/egm_pose_target_example.py` for an example using joint control. 103 | 104 | Pose target control is initialized by passing `EGMPoseTargetConfig` to the constructor of `MotionProgram`: 105 | 106 | ```python 107 | mm = abb.egm_minmax(-1e-3,1e-3) 108 | 109 | corr_frame = abb.pose([0,0,0],[1,0,0,0]) 110 | corr_fr_type = abb.egmframetype.EGM_FRAME_WOBJ 111 | sense_frame = abb.pose([0,0,0],[1,0,0,0]) 112 | sense_fr_type = abb.egmframetype.EGM_FRAME_WOBJ 113 | egm_offset = abb.pose([0,0,0],[1,0,0,0]) 114 | 115 | egm_config = abb.EGMPoseTargetConfig(corr_frame, corr_fr_type, sense_frame, sense_fr_type, 116 | mm, mm, mm, mm, mm ,mm, 1000, 1000 117 | ) 118 | 119 | mp = abb.MotionProgram(egm_config = egm_config) 120 | ``` 121 | 122 | This configuration will initialize the EGM for use when the motion program is loaded on the controller. It uses 123 | the ABB Rapid commands `EGMSetupUC` and `EGMActPose` to prepare for joint target control. See 124 | `EGMActPose` in the ABB manual for the meaning of the parameters sent to `EGMPoseTargetConfig`. These parameters 125 | are fairly complicated, and the user should carefully review the ABB manual before using pose control. The 126 | pose uses the tool frame and workobj frame provided as parameters to constructor of `MotionProgram`. By default 127 | the tool is the robot flange, and the work object is the robot base. 128 | 129 | Next, run the EGM operation using `EGMRunPose` and non-blocking motion program execution. It is recommended 130 | to move the robot to a known-safe location before starting EGM. The `EGM` class from `abb_robot_client` is used 131 | to receive robot state and send the pose command using `egm.send_to_robot_cart()` The first parameters is the 132 | position in millimeters, and the second parameter is the orientation in quaternians. Both are numpy arrays or python 133 | lists. The quaternion is in `[w,x,y,z]` format. Once done, call `client.stop_egm()` to break out of EGM control. 134 | The rest of this snippet waits for the program to stop and collects the log results. The example moves the robot 135 | along a line: 136 | 137 | ```python 138 | client = abb.MotionProgramExecClient(base_url="http://127.0.0.1:80") 139 | lognum = client.execute_motion_program(mp, wait=False) 140 | 141 | t1 = time.perf_counter() 142 | 143 | r2 = copy.copy(r1) 144 | 145 | egm = EGM() 146 | t2 = t1 147 | while (t2 - t1) < 5 : 148 | t2 = time.perf_counter() 149 | res, feedback = egm.receive_from_robot(timeout=0.05) 150 | if res: 151 | r2.trans[1]=(t2-t1)*100. 152 | egm.send_to_robot_cart(r2.trans, r2.rot) 153 | 154 | 155 | client.stop_egm() 156 | 157 | while client.is_motion_program_running(): 158 | time.sleep(0.05) 159 | 160 | log_results = client.read_motion_program_result_log(lognum) 161 | ``` 162 | 163 | ### EGM Path Correction 164 | 165 | EGM path correction allows for a nominal path to be programed using RAPID commands, and then use EGM to "correct" 166 | the position of the tool using an offset streamed to the robot. The commands `EGMMoveL` and `EGMMoveC` provide 167 | similar behavior to the normal `MoveL` and `MoveC` commands, but add the offset provided by EGM. The offset uses 168 | "path coordinates", which relate the direction of movement of the end effector. See `CorrConn` command in 169 | *Technical reference manual RAPID Instructions, Functions and Data types* for a detailed description of path 170 | coordinates. 171 | 172 | EGM correction operates a relatively slow update period of 48 ms. EGM may stream faster, but the controller receives 173 | updates at a relatively slow rate. 174 | 175 | The `pose` EGM config is used. Unlike joint and pose mode, the parameters are left as the ABB default when loaded. 176 | 177 | See `examples/egm/egm_path_correction_example.py` for an example using joint control. 178 | 179 | Joint target control is initialized by passing `EGMPathCorrectionConfig` to the constructor of `MotionProgram`: 180 | 181 | ```python 182 | sensor_frame = abb.pose([0,0,0],[1,0,0,0]) 183 | 184 | egm_config = abb.EGMPathCorrectionConfig(sensor_frame) 185 | 186 | mp = abb.MotionProgram(egm_config = egm_config) 187 | ``` 188 | 189 | This configuration will initialize the EGM for use when the motion program is loaded on the controller. It uses 190 | the ABB Rapid commands `EGMSetupUC` and `EGMActPose` to prepare for path correction control. See 191 | `EGMActMove` in the ABB manual for the meaning of the parameters sent to `EGMPathCorrectionConfig`. The default 48 ms 192 | update period is used. 193 | 194 | Next, run an EGM program containing `EGMMoveL` and/or `EGMMoveC` commands. Execute the motion program 195 | in non-blocking mode. The `EGM` class from `abb_robot_client` is used to receive robot state and send the path 196 | correction command. The function `egm.send_to_robot_path_corr()` is used to send the path correction. The 197 | first parameter is the path correction offset, in millimeters, in "path" coordinates. There is no need to stop 198 | the EGM process, since th program will complete once all commands have executed. 199 | 200 | ```python 201 | r1 = abb.robtarget([400,0,600],[0.7071068, 0., 0.7071068, 0.],abb.confdata(0,0,0,1),[0]*6) 202 | r2 = abb.robtarget([400,200,600],[0.7071068, 0., 0.7071068, 0.],abb.confdata(0,0,0,1),[0]*6) 203 | r3 = abb.robtarget([400,400,800],[0.7071068, 0., 0.7071068, 0.],abb.confdata(0,0,0,1),[0]*6) 204 | 205 | mp.MoveJ(r1,abb.v5000,abb.fine) 206 | # Using a fine point between EGMMove* will cause a controller error? 207 | mp.EGMMoveL(r2,abb.v50,abb.z1) 208 | mp.EGMMoveL(r1,abb.v50,abb.z1) 209 | mp.EGMMoveC(r2,r3,abb.v50,abb.z1) 210 | client = abb.MotionProgramExecClient(base_url="http://127.0.0.1:80") 211 | lognum = client.execute_motion_program(mp, wait=False) 212 | t1 = time.perf_counter() 213 | 214 | egm = EGM() 215 | t2 = t1 216 | while (t2 - t1) < 20: 217 | t2 = time.perf_counter() 218 | res, feedback = egm.receive_from_robot(timeout=0.1) 219 | if res: 220 | # Inject a sine wave correction 221 | # `pos` is in "ABB Path Coordinates" See "CorrConn" RAPID command documentation 222 | egm.send_to_robot_path_corr([0,math.sin((t2-t1)*10)*10.,0]) 223 | 224 | while client.is_motion_program_running(): 225 | time.sleep(0.05) 226 | 227 | log_results = client.read_motion_program_result_log(lognum) 228 | ``` 229 | 230 | This example injects a small sine wave offset overlaying the `EGMMoveC` and `EGMMoveL` commands. A real world use 231 | case would use sensor or feedback data to compute the offset value. Note that there is a very high latency for 232 | corrections, and this **must** be taken into account for feedback control, or the system may become unstable. 233 | -------------------------------------------------------------------------------- /docs/figures/arm_logo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/arm_logo.jpg -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_irb1200_select2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_irb1200_select2.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_add_irb1200.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_add_irb1200.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_base_frame1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_base_frame1.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_base_frame5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_base_frame5.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_base_frame6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_base_frame6.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_controller_name_location.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_controller_name_location.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_controller_options1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_controller_options1.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_controller_options2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_controller_options2.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_controller_options3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_controller_options3.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_controller_options_finish.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_controller_options_finish.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_controller_tasks.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_controller_tasks.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_controller_tasks3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_controller_tasks3.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_installed1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_installed1.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_installed2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_installed2.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_move_irb1200.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_move_irb1200.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_new_controller.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_new_controller.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_new_solution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_new_solution.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_open_home.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_open_home.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_rapid_copied.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_rapid_copied.png -------------------------------------------------------------------------------- /docs/figures/multimove/robotstudio_multimove_two_robots.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/multimove/robotstudio_multimove_two_robots.png -------------------------------------------------------------------------------- /docs/figures/nys_logo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/nys_logo.jpg -------------------------------------------------------------------------------- /docs/figures/robotstudio_addin_install_package.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_addin_install_package.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_addin_installed1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_addin_installed1.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_addin_installed2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_addin_installed2.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_addin_installed3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_addin_installed3.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_addin_installed4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_addin_installed4.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_addin_installed5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_addin_installed5.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_addin_installed6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_addin_installed6.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_addin_installed7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_addin_installed7.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_addin_mp_installed.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_addin_mp_installed.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_addin_robotware_irc5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_addin_robotware_irc5.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_addin_tab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_addin_tab.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_change_options.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_change_options.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_change_options2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_change_options2.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_change_options3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_change_options3.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_install_manager1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_install_manager1.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_install_manager2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_install_manager2.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_install_manager3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_install_manager3.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_install_manager4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_install_manager4.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_irb1200_select.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_irb1200_select.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_load_parameters.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_load_parameters.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_new_solution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_new_solution.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_open_home.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_open_home.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_rapid_copied.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_rapid_copied.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_restart.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_restart.png -------------------------------------------------------------------------------- /docs/figures/robotstudio_start_install_manager.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/docs/figures/robotstudio_start_install_manager.png -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | .. abb_motion_program_exec documentation master file 2 | 3 | 4 | Welcome to abb_motion_program_exec's documentation! 5 | ============================================ 6 | 7 | .. toctree:: 8 | :maxdepth: 2 9 | :caption: Contents: 10 | 11 | readme 12 | robot_setup 13 | egm 14 | abb_motion_program_exec/api_reference 15 | 16 | Indices and tables 17 | ================== 18 | 19 | * :ref:`genindex` 20 | * :ref:`modindex` 21 | * :ref:`search` 22 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0..\ 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=docs 11 | set BUILDDIR=build 12 | 13 | %SPHINXBUILD% >NUL 2>NUL 14 | if errorlevel 9009 ( 15 | echo. 16 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 17 | echo.installed, then set the SPHINXBUILD environment variable to point 18 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 19 | echo.may add the Sphinx directory to PATH. 20 | echo. 21 | echo.If you don't have Sphinx installed, grab it from 22 | echo.https://www.sphinx-doc.org/ 23 | exit /b 1 24 | ) 25 | 26 | if "%1" == "" goto help 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /docs/requirements.txt: -------------------------------------------------------------------------------- 1 | sphinx_autodoc_typehints 2 | recommonmark 3 | setuptools 4 | requests 5 | numpy 6 | protobuf 7 | websocket-client 8 | httpx 9 | websockets 10 | abb-robot-client 11 | sphinx-rtd-theme 12 | -e . 13 | -------------------------------------------------------------------------------- /docs/robot_multimove_setup_manual.md: -------------------------------------------------------------------------------- 1 | # ABB Motion Program Exec Manual Multi-Move Robot Setup 2 | 3 | ABB Multi-Move allows for a single controller to synchronously control up to 4 robots. The ABB 4 | Motion Program Exec software can be used with Multi-Move to control two robots. It can 5 | be modified to control more, but is currently only configured to control two robots. This document 6 | covers setting up a RobotStudio solution to simulate two robots, and install the ABB Motion 7 | Program Exec RAPID software for twe robot operation. This example uses IRB1200 and IRB6640 robots 8 | as examples. These can be substituted for other 6-axis ABB robots. 9 | 10 | See robot_setup_manual.md for instructions to set up for single robot use. 11 | 12 | ## Step 1: Install RobotWare 6.14 13 | 14 | **This section only needs to be completed once on RobotWare installation.** 15 | 16 | Begin by installing and starting RobotStudio. See 17 | http://new.abb.com/products/robotics/robotstudio/downloads for downloads. 18 | 19 | Once installed, open RobotStudio and click on the "Add-Ins" tab on the top of the window. 20 | 21 | ![](figures/robotstudio_addin_tab.png) 22 | 23 | First, install the newest version of RobotWare 6.14 for IRC5 controllers. Versions greater 24 | than RobotWare 6.14 may work, but they have not been tested. In the "RobotApps" -> "Gallery" window, 25 | select RobotWare for IRC5. On the right, select "Version" to be the greatest version of 6.14. 26 | In this example, version 6.14.00.01 is the greatest version available. Click "Add", accept the next 27 | window, and wait for the installation to finish. 28 | 29 | ![](figures/robotstudio_addin_robotware_irc5.png) 30 | 31 | RobotStudio is now ready to create a solution. 32 | 33 | ## 2. Create Solution 34 | 35 | Click on the "File" tab, "New", and then "Project". Uncheck "Include a Robot and Virtual Solution". 36 | Change the project "Name" if desired. Click "Create". 37 | 38 | ![](figures/multimove/robotstudio_multimove_new_solution.png) 39 | 40 | ## 3. Add Robots 41 | 42 | New we will add two robots to the solution. First, add the IRB 1200. Click "Home" -> "ABB Library" 43 | -> "IRB 1200". 44 | 45 | ![](figures/multimove/robotstudio_multimove_add_irb1200.png) 46 | 47 | If a window "IRB 1200" appears, select "Standard", "0.9 m", "Standard". The bottom right should show 48 | "IRB1200_5_90_STD_03". Click "OK". 49 | 50 | ![](figures/multimove/robotstudio_irb1200_select2.png) 51 | 52 | Right click on "IRB1200_5_90_STD_03" under "Layout", click "Position" -> "Set Position". In the 53 | "Set Position" box in the upper left, type in xyz = (1000,0,500), rpy = (0,0,180). Click "Apply" 54 | to move the robot. 55 | 56 | ![](figures/multimove/robotstudio_multimove_move_irb1200.png) 57 | 58 | Add another robot, this time an IRB 6640. Click "ABB Library" -> "IRB 6640". Select Version 59 | "IRB 6640-180" and click "OK". 60 | 61 | Right click on "IRB6640_180_255__04" under "Layout", click "Position" -> "Set Position". Set the 62 | position to xyz = (-1500,0,0), rpy = (0,0,0). There should now be two robots in the scene. 63 | 64 | ![](figures/multimove/robotstudio_multimove_two_robots.png) 65 | 66 | ## 4. Create Controller 67 | 68 | Now we will create a controller configured using Multi-Move to control both robots. Select "Home" 69 | -> "Virtual Controller" -> "From Layout". 70 | 71 | ![](figures/multimove/robotstudio_multimove_new_controller.png) 72 | 73 | Select defaults, or modify if desired for "Controller Name and Location". Click "Next". 74 | 75 | ![](figures/multimove/robotstudio_multimove_controller_name_location.png) 76 | 77 | Click "Next" to select both mechanisms. 78 | 79 | Use the default Controller Tasks, with Task 1 for the IRB 1200 and Task 2 for the IRB 6640. 80 | Click "Next" 81 | 82 | ![](figures/multimove/robotstudio_multimove_controller_tasks.png) 83 | 84 | On the "Controller Options" panel that is now visible, click "Options". 85 | 86 | ![](figures/multimove/robotstudio_multimove_controller_options1.png) 87 | 88 | ![](figures/multimove/robotstudio_multimove_controller_options2.png) 89 | 90 | Confirm that "640-1 MultiMove Coordinated" and "623-1 Multitasking" are selected on the right. 91 | If not, search on the top and select them. 92 | 93 | Click on "Communication", and select "616-1 PC Interface" and "617-1 FlexPendant Interface". 94 | Click "OK" 95 | 96 | ![](figures/multimove/robotstudio_multimove_controller_options3.png) 97 | 98 | **Uncheck "Task Frame(s) aligned with"**. Click "Finish". 99 | 100 | ![](figures/multimove/robotstudio_multimove_controller_options_finish.png) 101 | 102 | RobotStudio will work for a minute or two to complete creation of the controller. Click on 103 | "RAPID", and confirm that the tasks "T_ROB1" and "T_ROB2" were created in the tree on the left. 104 | 105 | ![](figures/multimove/robotstudio_multimove_controller_tasks3.png) 106 | 107 | ## 5. Install RAPID Software 108 | 109 | Next, copy over the RAPID files to the controller "HOME" directory. For the virtual controller, 110 | right click on "HOME" in the controller tree. (This is the same controller that options were 111 | changed on previously.) Click "Open Folder". 112 | 113 | ![](figures/multimove/robotstudio_multimove_open_home.png) 114 | 115 | Copy `error_reporter.mod`, `motion_program_exec.mod`, 116 | `motion_program_logger.mod`, and 117 | `motion_program_shared.sys` from `/robot/HOME` to the folder opened by clicking "Open Folder". 118 | The four files should now be visible under "HOME" in the tree view in RobotStudio. 119 | 120 | ![](figures/multimove/robotstudio_multimove_rapid_copied.png) 121 | 122 | Right click on "Configuration" in the controller tree, and click on "Load Parameters". Browse to 123 | `/robot/config_params_multimove` and select `SYS.cfg`. Click OK to confirm loading 124 | parameters, 125 | and OK to acknowledge restart is required. Repeat for `EIO.cfg` in the same directory. Make sure 126 | "Load parameters and replace duplicates" is selected in the file browser window for both. Click 127 | "Controller" -> "Restart (drop down)" -> "Reset RAPID (P-Start)" to restart the controller with 128 | the new software. Select "OK" to confirm RAPID reset. 129 | 130 | The installation should now be complete. The following should match, and can be checked against 131 | your system to verify installation. Newer versions may add more signals: 132 | 133 | ![](figures/multimove/robotstudio_multimove_installed1.png) 134 | 135 | ![](figures/multimove/robotstudio_multimove_installed2.png) 136 | 137 | ![](figures/robotstudio_addin_installed4.png) 138 | 139 | ![](figures/robotstudio_addin_installed7.png) 140 | 141 | ![](figures/robotstudio_addin_installed6.png) 142 | 143 | ![](figures/robotstudio_addin_installed5.png) 144 | 145 | ## 6. Adjust Robot Base Frame 146 | 147 | *This step is optional. Applications can arbitrarily define robot base frames as best suits the 148 | application.* 149 | 150 | Robot Studio will by default change the controller base frame to match the location in Robot Studio. 151 | The base frame of the robots are configured on the "Motion" -> "Robot" configuration page. Since we unchecked 152 | "Task frames aligned with:" during controller creation, the robot base origins match their positions during 153 | scene creation: 154 | 155 | ![](figures/multimove/robotstudio_multimove_base_frame1.png) 156 | 157 | The base frame values con be adjusted as desired. Robot Studio will attempt to keep these values in sync if 158 | the robots are moved in the scene. 159 | 160 | When a robot is moved, **DO NOT UPDATE THE TASK FRAME** when prompted. This will move the origin of the task frame out 161 | of sync with the origin of the Robot Studio scene, which can be very confusing. The task frame can be directly edited 162 | by clicking "Home" -> "Paths and Targets" -> Right click on "Controller1" in the tree view, and click "Task Frames". 163 | All entries should be zero to keep the task frame aligned with the Robot Studio world frame. 164 | 165 | ![](figures/multimove/robotstudio_multimove_base_frame5.png) 166 | 167 | Click "Yes" when prompted to "update controller configuration and restart". This will update the "Motion" -> "Robot" 168 | origins discussed previously. 169 | 170 | ![](figures/multimove/robotstudio_multimove_base_frame6.png) 171 | 172 | 173 | ## 7. Run Programs 174 | 175 | The robot is now ready to run programs! The `abb_motion_program_exec_client.py` can be used as a 176 | module to control the robot. For controlling two robots, two motion programs must be created, 177 | one for each robot. They must have exactly the same number of motion commands. The commands 178 | are passed with the `\ID` parameter corresponding to the command number. `SyncMoveOn` is activated 179 | to cause the robots to move in sync if required. 180 | 181 | See README.md for the example multi-move program and the `examples` directory. 182 | 183 | By default, the virtual controller listens on `http://localhost:80` for requests. The Python 184 | module uses ABB WebServices for communication. 185 | 186 | ## Updating RAPID modules 187 | 188 | To update RAPID modules, copy the updated files to `HOME`, and click Controller -> 189 | Restart (dropdown)-> Reset RAPID (P-start). -------------------------------------------------------------------------------- /docs/robot_setup.rst: -------------------------------------------------------------------------------- 1 | Robot Setup 2 | =========== 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | 7 | robot_setup_manual 8 | robot_multimove_setup_manual -------------------------------------------------------------------------------- /docs/robot_setup_manual.md: -------------------------------------------------------------------------------- 1 | # ABB Motion Program Exec Manual Robot Setup 2 | 3 | ABB Motion Program Exec requires software to be installed on the robot. This software can be 4 | installed manually by copying files to the robot controller and importing configuration files. 5 | This documents covers manually installing the software on a virtual controller in Robot Studio. 6 | 7 | ## Step 1: Install RobotWare 6.14 8 | 9 | **This section only needs to be completed once on RobotWare installation.** 10 | 11 | Begin by installing and starting RobotStudio. See 12 | http://new.abb.com/products/robotics/robotstudio/downloads for downloads. 13 | 14 | Once installed, open RobotStudio and click on the "Add-Ins" tab on the top of the window. 15 | 16 | ![](figures/robotstudio_addin_tab.png) 17 | 18 | First, install the newest version of RobotWare 6.14 for IRC5 controllers. Versions greater 19 | than RobotWare 6.14 may work, but they have not been tested. In the "RobotApps" -> "Gallery" window, 20 | select RobotWare for IRC5. On the right, select "Version" to be the greatest version of 6.14. 21 | In this example, version 6.14.00.01 is the greatest version available. Click "Add", accept the next 22 | window, and wait for the installation to finish. 23 | 24 | ![](figures/robotstudio_addin_robotware_irc5.png) 25 | 26 | RobotStudio is now ready to create a solution. 27 | 28 | ## 2. Create Solution and Install Robot 29 | 30 | Click on the "File" tab, and then "New". Check "Include a Robot and Virtual Controller". Select the 31 | RobotWare version installed in Step 1, in this case "6.14.00.01". For this example, the 32 | "IRB 1200 5kg 0.9m" robot is used. Any 6 or 7 axis single-arm robot can be used instead. Change 33 | the project "Name" if desired. Uncheck "Customize Options". Click "Create" when ready. 34 | 35 | ![](figures/robotstudio_new_solution.png) 36 | 37 | If a window "Select library for '1200.09_5_TypeB (ROB_1)" appears, select "IRB1200_5_90_STD_03" 38 | and click "OK". 39 | 40 | ![](figures/robotstudio_irb1200_select.png) 41 | 42 | Check which model the "real" robot is if using a robot other than IRB1200. Normally the "STD" 43 | version is fine for simulation. **The variant in terms of reach and payload is very important.** The 44 | other options often don't affect simulation. 45 | 46 | Click on the "Controller" tab on the top. Then right click on the virtual control listed in 47 | the left tree view under "Current Station". In this example, the controller is "IRB1200_5_90", but 48 | it will vary depending on the project configuration. Click "Change Options" on the right-click 49 | popup menu. 50 | 51 | ![](figures/robotstudio_change_options.png) 52 | 53 | In the "Change Options" window, select "Communication" -> "616-1 PC Interface" and 54 | "Engineering Tools" -> "623-1 Multitasking". If using EGM, also select "Engineering Tools" -> 55 | "689-1 Externally Guided Motion". 56 | 57 | ![](figures/robotstudio_change_options2.png) 58 | 59 | ![](figures/robotstudio_change_options3.png) 60 | 61 | Click OK. Click "Yes" to restart the controller. Click 62 | "OK" to confirm I-start. 63 | 64 | Next, copy over the RAPID files to the controller "HOME" directory. For the virtual controller, 65 | right click on "HOME" in the controller tree. (This is the same controller that options were 66 | changed on previously.) Click "Open Folder". 67 | 68 | ![](figures/robotstudio_open_home.png) 69 | 70 | Copy `error_reporter.mod`, `motion_program_exec.mod`, `motion_program_logger.mod`, and 71 | `motion_program_shared.sys` from `/robot/HOME` to the folder opened by clicking "Open Folder". The four 72 | files should now be visible under "HOME" in the tree view in RobotStudio. If using EGM, also copy 73 | `motion_program_exec_egm.mod` to the "HOME" folder. 74 | 75 | ![](figures/robotstudio_rapid_copied.png) 76 | 77 | Right click on "Configuration" in the controller tree, and click on "Load Parameters". Browse to 78 | `/robot/config_params` and select `SYS.cfg`. Click OK to confirm loading parameters, 79 | and OK to acknowledge restart is required. Repeat for `EIO.cfg` in the same directory. Make sure 80 | "Load parameters and replace duplicates" is selected in the file browser window for both. Click 81 | "Controller" -> "Restart (drop down)" -> "Reset RAPID (P-Start)" to restart the controller with 82 | the new software. Select "OK" to confirm RAPID reset. 83 | 84 | If using EGM, use the configuration files from `/robot/config_params_egm`, and load `SYS.cfg`, 85 | `EIO.cfg`, and `MOC.cfg` instead of the files in `config_params`. P-Start once loaded to load the changes. (It is 86 | safe to load these different configuration files after the normal files.) 87 | 88 | ![](figures/robotstudio_restart.png) 89 | 90 | The installation should now be complete. The following should match, and can be checked against 91 | your system to verify installation (if using EGM, there will be additional files and signals. Newer versions may add 92 | more signals): 93 | 94 | ![](figures/robotstudio_addin_installed1.png) 95 | 96 | ![](figures/robotstudio_addin_installed2.png) 97 | 98 | ![](figures/robotstudio_addin_installed3.png) 99 | 100 | ![](figures/robotstudio_addin_installed4.png) 101 | 102 | ![](figures/robotstudio_addin_installed7.png) 103 | 104 | ![](figures/robotstudio_addin_installed6.png) 105 | 106 | ![](figures/robotstudio_addin_installed5.png) 107 | 108 | ## 3. Run Programs 109 | 110 | Install the module using `pip install .`. 111 | 112 | The robot is now ready to run programs! The `/examples` directory contains examples to use the scripts. 113 | 114 | ``` 115 | python example.py 116 | ``` 117 | 118 | On Linux, it may be necessary to run `python3` 119 | 120 | ``` 121 | python3 example.py 122 | ``` 123 | 124 | EGM examples can be found in `/examples/egm`. 125 | 126 | By default, the virtual controller listens on `http://localhost:80` for requests. The Python 127 | module uses ABB WebServices for communication. 128 | 129 | ## Updating RAPID modules 130 | 131 | To update RAPID modules, copy the updated files to `HOME`, and click Controller -> 132 | Restart (dropdown)-> Reset RAPID (P-start). 133 | -------------------------------------------------------------------------------- /examples/egm/egm_joint_target_example.py: -------------------------------------------------------------------------------- 1 | from abb_robot_client.egm import EGM 2 | import abb_motion_program_exec as abb 3 | import time 4 | import numpy as np 5 | 6 | mm = abb.egm_minmax(-1e-3,1e-3) 7 | 8 | egm_config = abb.EGMJointTargetConfig( 9 | mm, mm, mm, mm, mm ,mm, 1000, 1000 10 | ) 11 | 12 | j1 = abb.jointtarget([0,0,0,0,0,0],[0]*6) 13 | 14 | mp = abb.MotionProgram(egm_config = egm_config) 15 | mp.MoveAbsJ(j1,abb.v5000,abb.fine) 16 | mp.EGMRunJoint(10, 0.05, 0.05) 17 | 18 | client = abb.MotionProgramExecClient(base_url="http://127.0.0.1:80") 19 | lognum = client.execute_motion_program(mp, wait=False) 20 | 21 | t1 = time.perf_counter() 22 | 23 | egm = EGM() 24 | t2 = t1 25 | while (t2 - t1) < 5 : 26 | t2 = time.perf_counter() 27 | res, feedback = egm.receive_from_robot(timeout=0.05) 28 | if res: 29 | egm.send_to_robot(np.ones((6,))*np.sin(t2-t1)*5) 30 | 31 | 32 | client.stop_egm() 33 | 34 | while client.is_motion_program_running(): 35 | time.sleep(0.05) 36 | 37 | log_results = client.read_motion_program_result_log(lognum) 38 | 39 | # log_results.data is a numpy array 40 | import matplotlib.pyplot as plt 41 | fig, ax1 = plt.subplots() 42 | lns1 = ax1.plot(log_results.data[:,0], log_results.data[:,2:]) 43 | ax1.set_xlabel("Time (s)") 44 | ax1.set_ylabel("Joint angle (deg)") 45 | ax2 = ax1.twinx() 46 | lns2 = ax2.plot(log_results.data[:,0], log_results.data[:,1], '-k') 47 | ax2.set_ylabel("Command number") 48 | ax2.set_yticks(range(-1,int(max(log_results.data[:,1]))+1)) 49 | ax1.legend(lns1 + lns2, log_results.column_headers[2:] + ["cmdnum"]) 50 | ax1.set_title("Joint motion") 51 | plt.show() -------------------------------------------------------------------------------- /examples/egm/egm_path_correction_example.py: -------------------------------------------------------------------------------- 1 | from abb_robot_client.egm import EGM 2 | import abb_motion_program_exec as abb 3 | import time 4 | import numpy as np 5 | import copy 6 | import math 7 | 8 | sensor_frame = abb.pose([0,0,0],[1,0,0,0]) 9 | 10 | egm_config = abb.EGMPathCorrectionConfig(sensor_frame) 11 | 12 | r1 = abb.robtarget([400,0,600],[0.7071068, 0., 0.7071068, 0.],abb.confdata(0,0,0,1),[0]*6) 13 | r2 = abb.robtarget([400,200,600],[0.7071068, 0., 0.7071068, 0.],abb.confdata(0,0,0,1),[0]*6) 14 | r3 = abb.robtarget([400,400,800],[0.7071068, 0., 0.7071068, 0.],abb.confdata(0,0,0,1),[0]*6) 15 | 16 | mp = abb.MotionProgram(egm_config = egm_config) 17 | mp.MoveJ(r1,abb.v5000,abb.fine) 18 | # Using a fine point between EGMMove* will cause a controller error? 19 | mp.EGMMoveL(r2,abb.v50,abb.z1) 20 | mp.EGMMoveL(r1,abb.v50,abb.z1) 21 | mp.EGMMoveC(r2,r3,abb.v50,abb.z1) 22 | client = abb.MotionProgramExecClient(base_url="http://127.0.0.1:80") 23 | lognum = client.execute_motion_program(mp, wait=False) 24 | t1 = time.perf_counter() 25 | 26 | egm = EGM() 27 | t2 = t1 28 | while (t2 - t1) < 20: 29 | t2 = time.perf_counter() 30 | res, feedback = egm.receive_from_robot(timeout=0.1) 31 | if res: 32 | # Inject a sine wave correction 33 | # `pos` is in "ABB Path Coordinates" See "CorrConn" RAPID command documentation 34 | egm.send_to_robot_path_corr([0,math.sin((t2-t1)*10)*10.,0]) 35 | 36 | while client.is_motion_program_running(): 37 | time.sleep(0.05) 38 | 39 | log_results = client.read_motion_program_result_log(lognum) 40 | 41 | # log_results.data is a numpy array 42 | import matplotlib.pyplot as plt 43 | fig, ax1 = plt.subplots() 44 | lns1 = ax1.plot(log_results.data[:,0], log_results.data[:,2:]) 45 | ax1.set_xlabel("Time (s)") 46 | ax1.set_ylabel("Joint angle (deg)") 47 | ax2 = ax1.twinx() 48 | lns2 = ax2.plot(log_results.data[:,0], log_results.data[:,1], '-k') 49 | ax2.set_ylabel("Command number") 50 | ax2.set_yticks(range(-1,int(max(log_results.data[:,1]))+1)) 51 | ax1.legend(lns1 + lns2, log_results.column_headers[2:] + ["cmdnum"]) 52 | ax1.set_title("Joint motion") 53 | plt.show() -------------------------------------------------------------------------------- /examples/egm/egm_pose_target_example.py: -------------------------------------------------------------------------------- 1 | from abb_robot_client.egm import EGM 2 | import abb_motion_program_exec as abb 3 | import time 4 | import numpy as np 5 | import copy 6 | 7 | mm = abb.egm_minmax(-1e-3,1e-3) 8 | 9 | corr_frame = abb.pose([0,0,0],[1,0,0,0]) 10 | corr_fr_type = abb.egmframetype.EGM_FRAME_WOBJ 11 | sense_frame = abb.pose([0,0,0],[1,0,0,0]) 12 | sense_fr_type = abb.egmframetype.EGM_FRAME_WOBJ 13 | egm_offset = abb.pose([0,0,0],[1,0,0,0]) 14 | 15 | egm_config = abb.EGMPoseTargetConfig(corr_frame, corr_fr_type, sense_frame, sense_fr_type, 16 | mm, mm, mm, mm, mm ,mm, 1000, 1000 17 | ) 18 | 19 | r1 = abb.robtarget([400,0,600],[0.7071068, 0., 0.7071068, 0.],abb.confdata(0,0,0,1),[0]*6) 20 | 21 | mp = abb.MotionProgram(egm_config = egm_config) 22 | mp.MoveJ(r1,abb.v5000,abb.fine) 23 | mp.EGMRunPose(10, 0.05, 0.05, egm_offset) 24 | client = abb.MotionProgramExecClient(base_url="http://127.0.0.1:80") 25 | lognum = client.execute_motion_program(mp, wait=False) 26 | 27 | t1 = time.perf_counter() 28 | 29 | r2 = copy.copy(r1) 30 | 31 | egm = EGM() 32 | t2 = t1 33 | while (t2 - t1) < 5 : 34 | t2 = time.perf_counter() 35 | res, feedback = egm.receive_from_robot(timeout=0.05) 36 | if res: 37 | r2.trans[1]=(t2-t1)*100. 38 | egm.send_to_robot_cart(r2.trans, r2.rot) 39 | 40 | 41 | client.stop_egm() 42 | 43 | while client.is_motion_program_running(): 44 | time.sleep(0.05) 45 | 46 | log_results = client.read_motion_program_result_log(lognum) 47 | 48 | # log_results.data is a numpy array 49 | import matplotlib.pyplot as plt 50 | fig, ax1 = plt.subplots() 51 | lns1 = ax1.plot(log_results.data[:,0], log_results.data[:,2:]) 52 | ax1.set_xlabel("Time (s)") 53 | ax1.set_ylabel("Joint angle (deg)") 54 | ax2 = ax1.twinx() 55 | lns2 = ax2.plot(log_results.data[:,0], log_results.data[:,1], '-k') 56 | ax2.set_ylabel("Command number") 57 | ax2.set_yticks(range(-1,int(max(log_results.data[:,1]))+1)) 58 | ax1.legend(lns1 + lns2, log_results.column_headers[2:] + ["cmdnum"]) 59 | ax1.set_title("Joint motion") 60 | plt.show() -------------------------------------------------------------------------------- /examples/example.py: -------------------------------------------------------------------------------- 1 | import abb_motion_program_exec as abb 2 | 3 | j1 = abb.jointtarget([10,20,30,40,50,60],[0]*6) 4 | j2 = abb.jointtarget([-10,15,35,10,95,-95],[0]*6) 5 | j3 = abb.jointtarget([15,-5,25,83,-84,85],[0]*6) 6 | 7 | my_tool = abb.tooldata(True,abb.pose([0,0,0.1],[1,0,0,0]),abb.loaddata(0.001,[0,0,0.001],[1,0,0,0],0,0,0)) 8 | 9 | mp = abb.MotionProgram(tool=my_tool) 10 | mp.MoveAbsJ(j1,abb.v1000,abb.fine) 11 | mp.MoveAbsJ(j2,abb.v5000,abb.fine) 12 | mp.MoveAbsJ(j3,abb.v500,abb.fine) 13 | mp.MoveAbsJ(j2,abb.v5000,abb.z50) 14 | mp.MoveAbsJ(j3,abb.v500,abb.z200) 15 | mp.MoveAbsJ(j2,abb.v5000,abb.fine) 16 | mp.WaitTime(1) 17 | 18 | r1 = abb.robtarget([350., -100., 600.], [ 0.0868241, -0.0868241, 0.9924039, 0.0075961 ], abb.confdata(-1,0,-1,0),[0]*6) 19 | r2 = abb.robtarget([370., 120., 620. ], [ 0.0868241, 0.0868241, 0.9924039, -0.0075961], abb.confdata(0,-1,0,0),[0]*6) 20 | 21 | r3 = abb.robtarget([400., -200., 500.], [0.7071068, 0., 0.7071068, 0.], abb.confdata( -1., -3., 2., 0.),[0]*6) 22 | r4 = abb.robtarget([400., 0., 580.], [0.7071068, 0., 0.7071068, 0.], abb.confdata(0., -3., 2., 0.), [0]*6) 23 | r5 = abb.robtarget([400., 200., 500.], [0.7071068, 0., 0.7071068, 0.], abb.confdata(0., -2., 1., 0.),[0]*6) 24 | 25 | mp.MoveJ(r1,abb.v500,abb.fine) 26 | mp.MoveJ(r2,abb.v400,abb.fine) 27 | mp.MoveJ(r1,abb.v500,abb.z100) 28 | mp.MoveJ(r2,abb.v400,abb.z100) 29 | mp.MoveJ(r1,abb.v500,abb.fine) 30 | mp.WaitTime(1.5) 31 | 32 | mp.MoveJ(r3,abb.v5000,abb.fine) 33 | mp.MoveL(r4,abb.v200,abb.fine) 34 | mp.MoveL(r3,abb.v200,abb.fine) 35 | mp.MoveL(r4,abb.v1000,abb.z100) 36 | mp.MoveL(r3,abb.v1000,abb.z100) 37 | mp.MoveL(r4,abb.v1000,abb.fine) 38 | mp.WaitTime(2.5) 39 | 40 | mp.MoveJ(r3,abb.v5000,abb.fine) 41 | 42 | mp.MoveC(r4,r5,abb.v200,abb.z10) 43 | mp.MoveC(r4,r3,abb.v50,abb.fine) 44 | 45 | # Print out RAPID module of motion program for debugging 46 | print(mp.get_program_rapid()) 47 | 48 | # Execute the motion program on the robot 49 | # Change base_url to the robot IP address 50 | client = abb.MotionProgramExecClient(base_url="http://127.0.0.1:80") 51 | log_results = client.execute_motion_program(mp) 52 | 53 | # log_results.data is a numpy array 54 | import matplotlib.pyplot as plt 55 | fig, ax1 = plt.subplots() 56 | lns1 = ax1.plot(log_results.data[:,0], log_results.data[:,2:]) 57 | ax1.set_xlabel("Time (s)") 58 | ax1.set_ylabel("Joint angle (deg)") 59 | ax2 = ax1.twinx() 60 | lns2 = ax2.plot(log_results.data[:,0], log_results.data[:,1], '-k') 61 | ax2.set_ylabel("Command number") 62 | ax2.set_yticks(range(-1,int(max(log_results.data[:,1]))+1)) 63 | ax1.legend(lns1 + lns2, log_results.column_headers[2:] + ["cmdnum"]) 64 | ax1.set_title("Joint motion") 65 | plt.show() 66 | -------------------------------------------------------------------------------- /examples/multimove_example.py: -------------------------------------------------------------------------------- 1 | import abb_motion_program_exec as abb 2 | 3 | # Fill motion program for T_ROB1 4 | t1 = abb.robtarget([575,-200,1280],[0,-.707,0,.707],abb.confdata(0,0,-1,1),[0]*6) 5 | t2 = abb.robtarget([575,200,1480],[0,-.707,0,.707],abb.confdata(-1,-1,0,1),[0]*6) 6 | t3 = abb.robtarget([575,0,1280],[0,-.707,0,.707],abb.confdata(-1,-1,0,1),[0]*6) 7 | 8 | my_tool = abb.tooldata(True,abb.pose([0,0,0.1],[1,0,0,0]),abb.loaddata(0.001,[0,0,0.001],[1,0,0,0],0,0,0)) 9 | 10 | mp = abb.MotionProgram(tool=my_tool) 11 | mp.SyncMoveOn() 12 | mp.MoveAbsJ(abb.jointtarget([5,-20,30,27,-11,-27],[0]*6),abb.v1000,abb.fine) 13 | mp.MoveL(t1,abb.v1000,abb.fine) 14 | mp.MoveJ(t2,abb.v5000,abb.fine) 15 | mp.MoveL(t3,abb.v500,abb.fine) 16 | mp.WaitTime(1) 17 | mp.MoveL(t1,abb.v5000,abb.z50) 18 | mp.MoveJ(t2,abb.v500,abb.z200) 19 | mp.MoveL(t3,abb.v5000,abb.fine) 20 | 21 | # Fill motion program for T_ROB2. Both programs must have 22 | # same number of commands 23 | t1_2 = abb.robtarget([250,-200,1280],[.707,0,.707,0],abb.confdata(-1,-1,0,1),[0]*6) 24 | t2_2 = abb.robtarget([250,200,1480],[.707,0,.707,0],abb.confdata(0,0,-1,1),[0]*6) 25 | t3_2 = abb.robtarget([250,0,1280],[.707,0,.707,0],abb.confdata(0,0,0,1),[0]*6) 26 | 27 | my_tool2 = abb.tooldata(True,abb.pose([0,0,0.5],[1,0,0,0]),abb.loaddata(0.1,[0,0,0.1],[1,0,0,0],0,0,0)) 28 | 29 | mp2 = abb.MotionProgram(tool=my_tool2) 30 | mp2.SyncMoveOn() 31 | mp2.MoveAbsJ(abb.jointtarget([1,1,40,2,-40,-2],[0]*6),abb.v1000,abb.fine) 32 | mp2.MoveJ(t1_2,abb.v1000,abb.fine) 33 | mp2.MoveL(t2_2,abb.v5000,abb.fine) 34 | mp2.MoveL(t3_2,abb.v500,abb.fine) 35 | mp2.WaitTime(1) 36 | mp2.MoveL(t1_2,abb.v5000,abb.z50) 37 | mp2.MoveL(t2_2,abb.v500,abb.z200) 38 | mp2.MoveL(t3_2,abb.v5000,abb.fine) 39 | 40 | 41 | # Execute the motion program on the robot 42 | # Change base_url to the robot IP address 43 | client = abb.MotionProgramExecClient(base_url="http://127.0.0.1:80") 44 | 45 | # Execute both motion programs simultaneously 46 | log_results = client.execute_multimove_motion_program([mp,mp2]) 47 | 48 | # log_results.data is a numpy array 49 | import matplotlib.pyplot as plt 50 | fig, ax1 = plt.subplots() 51 | lns1 = ax1.plot(log_results.data[:,0], log_results.data[:,2:8]) 52 | ax1.set_xlabel("Time (s)") 53 | ax1.set_ylabel("Joint angle (deg)") 54 | ax2 = ax1.twinx() 55 | lns2 = ax2.plot(log_results.data[:,0], log_results.data[:,1], '-k') 56 | ax2.set_ylabel("Command number") 57 | ax2.set_yticks(range(-1,int(max(log_results.data[:,1]))+1)) 58 | ax1.legend(lns1 + lns2, log_results.column_headers[2:8] + ["cmdnum"]) 59 | ax1.set_title("Robot 1 joint motion") 60 | fig, ax1 = plt.subplots() 61 | lns1 = ax1.plot(log_results.data[:,0], log_results.data[:,8:]) 62 | ax1.set_xlabel("Time (s)") 63 | ax1.set_ylabel("Joint angle (deg)") 64 | ax2 = ax1.twinx() 65 | lns2 = ax2.plot(log_results.data[:,0], log_results.data[:,1], '-k') 66 | ax2.set_ylabel("Command number") 67 | ax2.set_yticks(range(-1,int(max(log_results.data[:,1]))+1)) 68 | ax1.legend(lns1 + lns2, log_results.column_headers[8:] + ["cmdnum"]) 69 | ax1.set_title("Robot 2 joint motion") 70 | plt.show() 71 | 72 | 73 | -------------------------------------------------------------------------------- /examples/multimove_preempt_example.py: -------------------------------------------------------------------------------- 1 | import abb_motion_program_exec as abb 2 | import time 3 | 4 | # Fill motion program for T_ROB1 5 | t1 = abb.robtarget([575,-200,1280],[0,-.707,0,.707],abb.confdata(0,0,-1,1),[0]*6) 6 | t2 = abb.robtarget([575,200,1480],[0,-.707,0,.707],abb.confdata(-1,-1,0,1),[0]*6) 7 | t3 = abb.robtarget([575,0,1280],[0,-.707,0,.707],abb.confdata(-1,-1,0,1),[0]*6) 8 | t4 = abb.robtarget([575,0,1680],[0,-.707,0,.707],abb.confdata(-1,-1,0,1),[0]*6) 9 | 10 | my_tool = abb.tooldata(True,abb.pose([0,0,0.1],[1,0,0,0]),abb.loaddata(0.001,[0,0,0.001],[1,0,0,0],0,0,0)) 11 | 12 | mp = abb.MotionProgram(tool=my_tool) 13 | mp.SyncMoveOn() 14 | mp.MoveAbsJ(abb.jointtarget([5,-20,30,27,-11,-27],[0]*6),abb.v1000,abb.fine) 15 | mp.MoveL(t1,abb.v1000,abb.fine) 16 | mp.MoveJ(t2,abb.v5000,abb.fine) 17 | mp.MoveL(t3,abb.v500,abb.fine) 18 | mp.WaitTime(1) 19 | 20 | mp_p1 = abb.MotionProgram(tool=my_tool, first_cmd_num=5) 21 | mp_p1.MoveJ(t1,abb.v5000,abb.z50) 22 | mp_p1.MoveJ(t2,abb.v500,abb.z200) 23 | mp_p1.MoveL(t4,abb.v5000,abb.fine) 24 | 25 | # Fill motion program for T_ROB2. Both programs must have 26 | # same number of commands 27 | t1_2 = abb.robtarget([250,-200,1280],[.707,0,.707,0],abb.confdata(-1,-1,0,1),[0]*6) 28 | t2_2 = abb.robtarget([250,200,1480],[.707,0,.707,0],abb.confdata(0,0,-1,1),[0]*6) 29 | t3_2 = abb.robtarget([250,0,1280],[.707,0,.707,0],abb.confdata(0,0,0,1),[0]*6) 30 | t4_2 = abb.robtarget([250,0,1080],[.707,0,.707,0],abb.confdata(0,0,0,1),[0]*6) 31 | 32 | my_tool2 = abb.tooldata(True,abb.pose([0,0,0.5],[1,0,0,0]),abb.loaddata(0.1,[0,0,0.1],[1,0,0,0],0,0,0)) 33 | 34 | mp2 = abb.MotionProgram(tool=my_tool2) 35 | mp2.SyncMoveOn() 36 | mp2.MoveAbsJ(abb.jointtarget([1,1,40,2,-40,-2],[0]*6),abb.v1000,abb.fine) 37 | mp2.MoveJ(t1_2,abb.v1000,abb.fine) 38 | mp2.MoveL(t2_2,abb.v5000,abb.fine) 39 | mp2.MoveL(t3_2,abb.v500,abb.fine) 40 | mp2.WaitTime(1) 41 | 42 | mp2_p1 = abb.MotionProgram(tool=my_tool2, first_cmd_num=5) 43 | mp2_p1.MoveJ(t1_2,abb.v5000,abb.z50) 44 | mp2_p1.MoveL(t2_2,abb.v500,abb.z200) 45 | mp2_p1.MoveL(t4_2,abb.v5000,abb.fine) 46 | 47 | 48 | # Execute the motion program on the robot 49 | # Change base_url to the robot IP address 50 | client = abb.MotionProgramExecClient(base_url="http://127.0.0.1:80") 51 | 52 | # Each preempting must have an incrementing preempt_number 53 | preempt_number = 1 54 | 55 | # Set wait=False so execute_motion_program returns immediately 56 | lognum = client.execute_multimove_motion_program([mp,mp2],wait=False) 57 | time.sleep(0.25) 58 | 59 | # Request preemption after command number 4 60 | client.preempt_multimove_motion_program([mp_p1,mp2_p1], preempt_number=1, preempt_cmdnum=4) 61 | while client.is_motion_program_running(): 62 | # Currently executing command number 63 | cmd_num = client.get_current_cmdnum() 64 | # Current preemption number 65 | preempt_num = client.get_current_preempt_number() 66 | # Current command number that has been queued for execution 67 | queued_num = client.get_queued_cmdnum() 68 | print(f"preempt_num={preempt_num}, cmd_num={cmd_num}, queued_cmd_num={queued_num}") 69 | time.sleep(0.05) 70 | 71 | # Read the results once motion program stops running 72 | log_results = client.read_motion_program_result_log(lognum) 73 | 74 | # log_results.data is a numpy array 75 | import matplotlib.pyplot as plt 76 | fig, ax1 = plt.subplots() 77 | lns1 = ax1.plot(log_results.data[:,0], log_results.data[:,2:8]) 78 | ax1.set_xlabel("Time (s)") 79 | ax1.set_ylabel("Joint angle (deg)") 80 | ax2 = ax1.twinx() 81 | lns2 = ax2.plot(log_results.data[:,0], log_results.data[:,1], '-k') 82 | ax2.set_ylabel("Command number") 83 | ax2.set_yticks(range(-1,int(max(log_results.data[:,1]))+1)) 84 | ax1.legend(lns1 + lns2, log_results.column_headers[2:8] + ["cmdnum"]) 85 | ax1.set_title("Robot 1 joint motion") 86 | fig, ax1 = plt.subplots() 87 | lns1 = ax1.plot(log_results.data[:,0], log_results.data[:,8:]) 88 | ax1.set_xlabel("Time (s)") 89 | ax1.set_ylabel("Joint angle (deg)") 90 | ax2 = ax1.twinx() 91 | lns2 = ax2.plot(log_results.data[:,0], log_results.data[:,1], '-k') 92 | ax2.set_ylabel("Command number") 93 | ax2.set_yticks(range(-1,int(max(log_results.data[:,1]))+1)) 94 | ax1.legend(lns1 + lns2, log_results.column_headers[8:] + ["cmdnum"]) 95 | ax1.set_title("Robot 2 joint motion") 96 | plt.show() 97 | 98 | 99 | -------------------------------------------------------------------------------- /examples/multimove_relative_example.py: -------------------------------------------------------------------------------- 1 | # Multi-Move example using relative robot end effector poses 2 | 3 | import abb_motion_program_exec as abb 4 | 5 | 6 | # Fill motion program for T_ROB1 7 | 8 | # Hold constant relative position for this example 9 | t1 = abb.robtarget([0,0,-200],[1,0,0,0],abb.confdata(0,0,1,1),[0]*6) 10 | t2 = t1 11 | t3 = t1 12 | 13 | 14 | my_tool = abb.tooldata(True,abb.pose([0,0,0],[1,0,0,0]),abb.loaddata(0.001,[0,0,0.001],[1,0,0,0],0,0,0)) 15 | my_wobj = abb.wobjdata(False,False,"ROB_2",abb.pose([0,0,0],[1,0,0,0]),abb.pose([0,0,0],[0,0,1,0])) 16 | 17 | mp = abb.MotionProgram(tool=my_tool,wobj=my_wobj) 18 | mp.SyncMoveOn() 19 | mp.MoveAbsJ(abb.jointtarget([5,-20,30,27,-11,172],[0]*6),abb.v1000,abb.fine) 20 | mp.WaitTime(0.1) 21 | mp.MoveJ(t1,abb.v1000,abb.fine) 22 | mp.MoveJ(t1,abb.v1000,abb.fine) 23 | mp.MoveL(t1,abb.v1000,abb.fine) 24 | 25 | # Fill motion program for T_ROB2. Both programs must have 26 | # same number of commands 27 | t1_2 = abb.robtarget([250,-200,1280],[.707,0,.707,0],abb.confdata(-1,-1,0,1),[0]*6) 28 | t2_2 = abb.robtarget([250,200,1480],[.707,0,.707,0],abb.confdata(0,0,-1,1),[0]*6) 29 | t3_2 = abb.robtarget([250,0,1280],[.707,0,.707,0],abb.confdata(0,0,0,1),[0]*6) 30 | 31 | my_tool2 = abb.tooldata(True,abb.pose([0,0,0.5],[1,0,0,0]),abb.loaddata(0.1,[0,0,0.1],[1,0,0,0],0,0,0)) 32 | 33 | mp2 = abb.MotionProgram(tool=my_tool2) 34 | mp2.SyncMoveOn() 35 | mp2.MoveAbsJ(abb.jointtarget([1,1,40,2,-40,-2],[0]*6),abb.v1000,abb.fine) 36 | mp2.WaitTime(0.1) 37 | mp2.MoveL(t1_2,abb.v1000,abb.fine) 38 | mp2.MoveL(t2_2,abb.v5000,abb.fine) 39 | mp2.MoveL(t3_2,abb.v500,abb.fine) 40 | 41 | # Print out rapid programs for debugging (optional) 42 | print(mp.get_program_rapid(module_name="TROB1_MODULE",sync_move=True)) 43 | print() 44 | print(mp2.get_program_rapid(module_name="TROB2_MODULE", sync_move=True)) 45 | 46 | # Execute the motion program on the robot 47 | # Change base_url to the robot IP address 48 | client = abb.MotionProgramExecClient(base_url="http://127.0.0.1:80") 49 | 50 | # Execute both motion programs simultaneously 51 | log_results = client.execute_multimove_motion_program([mp,mp2]) 52 | 53 | # log_results.data is a numpy array 54 | import matplotlib.pyplot as plt 55 | fig, ax1 = plt.subplots() 56 | lns1 = ax1.plot(log_results.data[:,0], log_results.data[:,2:8]) 57 | ax1.set_xlabel("Time (s)") 58 | ax1.set_ylabel("Joint angle (deg)") 59 | ax2 = ax1.twinx() 60 | lns2 = ax2.plot(log_results.data[:,0], log_results.data[:,1], '-k') 61 | ax2.set_ylabel("Command number") 62 | ax2.set_yticks(range(-1,int(max(log_results.data[:,1]))+1)) 63 | ax1.legend(lns1 + lns2, log_results.column_headers[2:8] + ["cmdnum"]) 64 | ax1.set_title("Robot 1 joint motion") 65 | fig, ax1 = plt.subplots() 66 | lns1 = ax1.plot(log_results.data[:,0], log_results.data[:,8:]) 67 | ax1.set_xlabel("Time (s)") 68 | ax1.set_ylabel("Joint angle (deg)") 69 | ax2 = ax1.twinx() 70 | lns2 = ax2.plot(log_results.data[:,0], log_results.data[:,1], '-k') 71 | ax2.set_ylabel("Command number") 72 | ax2.set_yticks(range(-1,int(max(log_results.data[:,1]))+1)) 73 | ax1.legend(lns1 + lns2, log_results.column_headers[8:] + ["cmdnum"]) 74 | ax1.set_title("Robot 2 joint motion") 75 | plt.show() 76 | 77 | 78 | -------------------------------------------------------------------------------- /examples/preempt_example.py: -------------------------------------------------------------------------------- 1 | # Single robot preemption example 2 | 3 | import abb_motion_program_exec as abb 4 | import time 5 | 6 | j1 = abb.jointtarget([10,20,30,40,50,60],[0]*6) 7 | j2 = abb.jointtarget([-10,15,35,10,95,-95],[0]*6) 8 | j3 = abb.jointtarget([15,-5,25,83,-84,85],[0]*6) 9 | 10 | my_tool = abb.tooldata(True,abb.pose([0,0,0.1],[1,0,0,0]),abb.loaddata(0.001,[0,0,0.001],[1,0,0,0],0,0,0)) 11 | 12 | # mp is the nominal motion program 13 | 14 | mp = abb.MotionProgram(tool=my_tool) 15 | mp.MoveAbsJ(j2,abb.v5000,abb.fine) 16 | 17 | 18 | r1 = abb.robtarget([350., -100., 600.], [ 0.0868241, -0.0868241, 0.9924039, 0.0075961 ], abb.confdata(-1,0,-1,0),[0]*6) 19 | r2 = abb.robtarget([370., 120., 620. ], [ 0.0868241, 0.0868241, 0.9924039, -0.0075961], abb.confdata(0,-1,0,0),[0]*6) 20 | 21 | r3 = abb.robtarget([400., -200., 500.], [0.7071068, 0., 0.7071068, 0.], abb.confdata( -1., -3., 2., 0.),[0]*6) 22 | r4 = abb.robtarget([400., 0., 580.], [0.7071068, 0., 0.7071068, 0.], abb.confdata(0., -3., 2., 0.), [0]*6) 23 | r5 = abb.robtarget([400., 200., 500.], [0.7071068, 0., 0.7071068, 0.], abb.confdata(0., -2., 1., 0.),[0]*6) 24 | 25 | mp.MoveJ(r1,abb.v500,abb.fine) 26 | mp.MoveJ(r2,abb.v400,abb.fine) 27 | mp.MoveJ(r1,abb.v500,abb.z100) 28 | mp.MoveJ(r2,abb.v400,abb.z100) 29 | mp.MoveJ(r1,abb.v500,abb.fine) 30 | 31 | # Create preempting motion program. This may be generated in real time based on sensor feedback. 32 | # first_cmd_num must be one increment after preemption number 33 | 34 | mp_p1 = abb.MotionProgram(first_cmd_num=5) 35 | mp_p1.MoveJ(r3,abb.v5000,abb.fine) 36 | 37 | mp_p1.MoveC(r4,r5,abb.v200,abb.z10) 38 | mp_p1.MoveC(r4,r3,abb.v50,abb.fine) 39 | 40 | # Print out RAPID module of nominal motion program for debugging 41 | print(mp.get_program_rapid()) 42 | 43 | # Execute the motion program on the robot 44 | # Change base_url to the robot IP address 45 | client = abb.MotionProgramExecClient(base_url="http://127.0.0.1:80") 46 | 47 | # Each preempting must have an incrementing preempt_number 48 | preempt_number = 1 49 | 50 | # Set wait=False so execute_motion_program returns immediately 51 | lognum = client.execute_motion_program(mp, wait=False) 52 | time.sleep(0.25) 53 | 54 | # Request preemption after command number 4 55 | client.preempt_motion_program(mp_p1, preempt_number=1, preempt_cmdnum=4) 56 | while client.is_motion_program_running(): 57 | # Currently executing command number 58 | cmd_num = client.get_current_cmdnum() 59 | # Current preemption number 60 | preempt_num = client.get_current_preempt_number() 61 | # Current command number that has been queued for execution 62 | queued_num = client.get_queued_cmdnum() 63 | print(f"preempt_num={preempt_num}, cmd_num={cmd_num}, queued_cmd_num={queued_num}") 64 | time.sleep(0.05) 65 | 66 | # Read the results once motion program stops running 67 | log_results = client.read_motion_program_result_log(lognum) 68 | 69 | # log_results.data is a numpy array 70 | import matplotlib.pyplot as plt 71 | fig, ax1 = plt.subplots() 72 | lns1 = ax1.plot(log_results.data[:,0], log_results.data[:,2:]) 73 | ax1.set_xlabel("Time (s)") 74 | ax1.set_ylabel("Joint angle (deg)") 75 | ax2 = ax1.twinx() 76 | lns2 = ax2.plot(log_results.data[:,0], log_results.data[:,1], '-k') 77 | ax2.set_ylabel("Command number") 78 | ax2.set_yticks(range(-1,int(max(log_results.data[:,1]))+1)) 79 | ax1.legend(lns1 + lns2, log_results.column_headers[2:] + ["cmdnum"]) 80 | ax1.set_title("Joint motion") 81 | plt.show() 82 | -------------------------------------------------------------------------------- /examples/robotraconteur/example_client.py: -------------------------------------------------------------------------------- 1 | from RobotRaconteur.Client import * 2 | import numpy as np 3 | 4 | c = RRN.ConnectService("rr+tcp://localhost:59843?service=mp_robot") 5 | 6 | robot_pose_type = RRN.GetStructureType("experimental.robotics.motion_program.RobotPose",c) 7 | moveabsj_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveAbsJCommand",c) 8 | movej_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveJCommand",c) 9 | movel_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveLCommand",c) 10 | movec_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveCCommand",c) 11 | settool_type = RRN.GetStructureType("experimental.robotics.motion_program.SetToolCommand",c) 12 | motionprogram_type = RRN.GetStructureType("experimental.robotics.motion_program.MotionProgram",c) 13 | toolinfo_type = RRN.GetStructureType("com.robotraconteur.robotics.tool.ToolInfo",c) 14 | transform_dt = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Transform",c) 15 | spatialinertia_dt = RRN.GetNamedArrayDType("com.robotraconteur.geometry.SpatialInertia",c) 16 | 17 | toolinfo = toolinfo_type() 18 | toolinfo.tcp = RRN.ArrayToNamedArray([1,0,0,0,0,0,0.001],transform_dt) 19 | toolinfo.inertia = RRN.ArrayToNamedArray([0.1,0,0,0.01,.001,0,0,.001,0,.001],spatialinertia_dt) 20 | 21 | settool = settool_type() 22 | settool.tool_info = toolinfo 23 | 24 | j1 = np.deg2rad(np.array([10,20,30,40,50,60],dtype=np.float64)) 25 | j2 = np.deg2rad(np.array([-10,15,35,10,95,-95],dtype=np.float64)) 26 | j3 = np.deg2rad(np.array([15,-5,25,83,-84,85],dtype=np.float64)) 27 | 28 | 29 | def robot_pose(p,q,conf): 30 | ret = robot_pose_type() 31 | ret.tcp_pose[0]["orientation"]["w"] = q[0] 32 | ret.tcp_pose[0]["orientation"]["x"] = q[1] 33 | ret.tcp_pose[0]["orientation"]["y"] = q[2] 34 | ret.tcp_pose[0]["orientation"]["z"] = q[3] 35 | ret.tcp_pose[0]["position"]["x"] = p[0]*1e-3 36 | ret.tcp_pose[0]["position"]["y"] = p[1]*1e-3 37 | ret.tcp_pose[0]["position"]["z"] = p[2]*1e-3 38 | 39 | ret.joint_position_seed=np.zeros((6,)) 40 | ret.joint_position_seed[0] = conf[0]*np.pi/2 41 | ret.joint_position_seed[3] = conf[1]*np.pi/2 42 | ret.joint_position_seed[5] = conf[2]*np.pi/2 43 | 44 | return ret 45 | 46 | 47 | r1 = robot_pose([ 350., -100., 600. ], [ 0.0868241, -0.0868241, 0.9924039, 0.0075961 ], (-1,0,-1,0)) 48 | r2 = robot_pose([ 370., 120., 620. ], [ 0.0868241, 0.0868241, 0.9924039, -0.0075961 ], (0,-1,0,0)) 49 | 50 | r3 = robot_pose([400., -200., 500.], [ 0.7071068, 0., 0.7071068, 0. ], ( -1., -3., 2., 0. )) 51 | r4 = robot_pose([400., 0., 580. ], [0.7071068, 0., 0.7071068, 0. ], ( 0., -3, -2., 0.)) 52 | r5 = robot_pose([ 400., 200., 500.], [0.7071068, 0., 0.7071068, 0.], (0., -2., 1., 0.)) 53 | 54 | setup_cmds = [] 55 | mp_cmds = [] 56 | 57 | def moveabsj(j,velocity,blend_radius,fine_point): 58 | cmd = moveabsj_type() 59 | cmd.joint_position = j 60 | cmd.tcp_velocity = velocity 61 | cmd.blend_radius = blend_radius 62 | cmd.fine_point = fine_point 63 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveAbsJCommand") 64 | 65 | def movel(robot_pose,velocity,blend_radius,fine_point): 66 | cmd = movel_type() 67 | cmd.tcp_pose = robot_pose 68 | cmd.tcp_velocity = velocity 69 | cmd.blend_radius = blend_radius 70 | cmd.fine_point = fine_point 71 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveLCommand") 72 | 73 | def movej(robot_pose,velocity,blend_radius,fine_point): 74 | cmd = movej_type() 75 | cmd.tcp_pose = robot_pose 76 | cmd.tcp_velocity = velocity 77 | cmd.blend_radius = blend_radius 78 | cmd.fine_point = fine_point 79 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveJCommand") 80 | 81 | def movec(robot_pose,robot_via_pose,velocity,blend_radius,fine_point): 82 | cmd = movec_type() 83 | cmd.tcp_pose = robot_pose 84 | cmd.tcp_via_pose = robot_via_pose 85 | cmd.tcp_velocity = velocity 86 | cmd.blend_radius = blend_radius 87 | cmd.fine_point = fine_point 88 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveCCommand") 89 | 90 | setup_cmds.append(RR.VarValue(settool,"experimental.robotics.motion_program.SetToolCommand")) 91 | mp_cmds.append(moveabsj(j1,0.5,0.2,False)) 92 | mp_cmds.append(moveabsj(j2,1,0.1,True)) 93 | mp_cmds.append(moveabsj(j3,2,0.3,False)) 94 | mp_cmds.append(moveabsj(j1,0.5,0.2,True)) 95 | 96 | mp_cmds.append(movel(r1,0.5,0.02,False)) 97 | mp_cmds.append(movel(r2,0.5,0.2,True)) 98 | 99 | mp_cmds.append(movej(r1,0.5,0.02,False)) 100 | mp_cmds.append(movej(r3,0.5,0.2,True)) 101 | 102 | mp_cmds.append(movec(r5,r4,0.5,0.2,True)) 103 | 104 | mp = motionprogram_type() 105 | 106 | mp.motion_program_commands = mp_cmds 107 | mp.motion_setup_commands = setup_cmds 108 | 109 | # If multimove is available, optionally use a different robot other than first robot. 110 | # See multimove example for demonstration of synchronizing multiple robots 111 | # mp.extended = { 112 | # "groups": RR.VarValue("T_ROB2", "string") 113 | # } 114 | 115 | mp_gen = c.execute_motion_program_record(mp, False) 116 | res = None 117 | 118 | status = None 119 | while True: 120 | res, status1 = mp_gen.TryNext() 121 | if not res: 122 | break 123 | status = status1 124 | print(status) 125 | 126 | 127 | print(f"recording_handle: {status.recording_handle}") 128 | 129 | robot_recording = c.read_recording(status.recording_handle).NextAll()[0] 130 | 131 | print(robot_recording.time) 132 | print(robot_recording.command_number) 133 | print(robot_recording.joints) 134 | 135 | c.clear_recordings() 136 | 137 | print("Done!") -------------------------------------------------------------------------------- /examples/robotraconteur/example_client_confdata.py: -------------------------------------------------------------------------------- 1 | from RobotRaconteur.Client import * 2 | import numpy as np 3 | 4 | c = RRN.ConnectService("rr+tcp://localhost:59843?service=mp_robot") 5 | 6 | robot_pose_type = RRN.GetStructureType("experimental.robotics.motion_program.RobotPose",c) 7 | moveabsj_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveAbsJCommand",c) 8 | movej_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveJCommand",c) 9 | movel_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveLCommand",c) 10 | movec_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveCCommand",c) 11 | settool_type = RRN.GetStructureType("experimental.robotics.motion_program.SetToolCommand",c) 12 | motionprogram_type = RRN.GetStructureType("experimental.robotics.motion_program.MotionProgram",c) 13 | toolinfo_type = RRN.GetStructureType("com.robotraconteur.robotics.tool.ToolInfo",c) 14 | transform_dt = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Transform",c) 15 | spatialinertia_dt = RRN.GetNamedArrayDType("com.robotraconteur.geometry.SpatialInertia",c) 16 | 17 | toolinfo = toolinfo_type() 18 | toolinfo.tcp = RRN.ArrayToNamedArray([1,0,0,0,0,0,0.001],transform_dt) 19 | toolinfo.inertia = RRN.ArrayToNamedArray([0.1,0,0,0.01,.001,0,0,.001,0,.001],spatialinertia_dt) 20 | 21 | settool = settool_type() 22 | settool.tool_info = toolinfo 23 | 24 | j1 = np.deg2rad(np.array([10,20,30,40,50,60],dtype=np.float64)) 25 | j2 = np.deg2rad(np.array([90,-91,60,-93,94,-95],dtype=np.float64)) 26 | j3 = np.deg2rad(np.array([-80,81,-82,83,-84,85],dtype=np.float64)) 27 | 28 | 29 | def robot_pose(p,q,conf): 30 | ret = robot_pose_type() 31 | ret.tcp_pose[0]["orientation"]["w"] = q[0] 32 | ret.tcp_pose[0]["orientation"]["x"] = q[1] 33 | ret.tcp_pose[0]["orientation"]["y"] = q[2] 34 | ret.tcp_pose[0]["orientation"]["z"] = q[3] 35 | ret.tcp_pose[0]["position"]["x"] = p[0]*1e-3 36 | ret.tcp_pose[0]["position"]["y"] = p[1]*1e-3 37 | ret.tcp_pose[0]["position"]["z"] = p[2]*1e-3 38 | 39 | ret.joint_position_seed=np.zeros((6,)) 40 | ret.joint_position_seed[0] = conf[0]*np.pi/2 41 | ret.joint_position_seed[3] = conf[1]*np.pi/2 42 | ret.joint_position_seed[5] = conf[2]*np.pi/2 43 | 44 | return ret 45 | 46 | 47 | r1 = robot_pose([0.1649235*1e3, 0.1169957*1e3, 0.9502961*1e3], [ 0.6776466, -0.09003431, 0.6362069, 0.3576725 ], (0,0,0,0)) 48 | r2 = robot_pose([ 0.6243948*1e3, -0.479558*1e3 , 0.7073749*1e3], [ 0.6065634, -0.2193409, 0.6427138, -0.4133877], (-1,-1,0,1)) 49 | 50 | r3 = robot_pose([417.9236, 276.9956, 885.2959], [ 0.8909725 , -0.1745558 , 0.08864544, 0.4096832 ], ( 0., 1., -2., 0.)) 51 | r4 = robot_pose([417.9235 , -11.00438, 759.2958 ], [0.7161292 , 0.1868255 , 0.01720813, 0.6722789 ], ( 0., 2., -2., 0.)) 52 | r5 = robot_pose([ 417.9235, -173.0044, 876.2958], [0.6757616, 0.3854275, 0.2376617, 0.5816431], (-1., 1., -1., 0.)) 53 | 54 | setup_cmds = [] 55 | mp_cmds = [] 56 | 57 | def moveabsj(j,velocity,blend_radius,fine_point): 58 | cmd = moveabsj_type() 59 | cmd.joint_position = j 60 | cmd.tcp_velocity = velocity 61 | cmd.blend_radius = blend_radius 62 | cmd.fine_point = fine_point 63 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveAbsJCommand") 64 | 65 | def movel(robot_pose,velocity,blend_radius,fine_point, conf): 66 | cmd = movel_type() 67 | cmd.tcp_pose = robot_pose 68 | cmd.tcp_velocity = velocity 69 | cmd.blend_radius = blend_radius 70 | cmd.fine_point = fine_point 71 | cmd.extended = { 72 | "confdata": RR.VarValue(conf,"double[]") 73 | } 74 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveLCommand") 75 | 76 | def movej(robot_pose,velocity,blend_radius,fine_point,conf): 77 | cmd = movej_type() 78 | cmd.tcp_pose = robot_pose 79 | cmd.tcp_velocity = velocity 80 | cmd.blend_radius = blend_radius 81 | cmd.fine_point = fine_point 82 | cmd.extended = { 83 | "confdata": RR.VarValue(conf,"double[]") 84 | } 85 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveJCommand") 86 | 87 | def movec(robot_pose,robot_via_pose,velocity,blend_radius,fine_point, conf, conf_via): 88 | cmd = movec_type() 89 | cmd.tcp_pose = robot_pose 90 | cmd.tcp_via_pose = robot_via_pose 91 | cmd.tcp_velocity = velocity 92 | cmd.blend_radius = blend_radius 93 | cmd.fine_point = fine_point 94 | cmd.extended = { 95 | "confdata": RR.VarValue(conf,"double[]"), 96 | "confdata_via": RR.VarValue(conf_via,"double[]") 97 | } 98 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveCCommand") 99 | 100 | setup_cmds.append(RR.VarValue(settool,"experimental.robotics.motion_program.SetToolCommand")) 101 | mp_cmds.append(moveabsj(j1,0.5,0.2,False)) 102 | mp_cmds.append(moveabsj(j2,1,0.1,True)) 103 | mp_cmds.append(moveabsj(j3,2,0.3,False)) 104 | mp_cmds.append(moveabsj(j1,0.5,0.2,True)) 105 | 106 | mp_cmds.append(movel(r1,0.5,0.02,False,(0,0,0,0))) 107 | mp_cmds.append(movel(r2,0.5,0.2,True,(-1,-1,0,1))) 108 | 109 | mp_cmds.append(movej(r1,0.5,0.02,False,(0,0,0,0))) 110 | mp_cmds.append(movej(r3,0.5,0.2,True,( 0., 1., -2., 0.))) 111 | 112 | # mp_cmds.append(movec(r4,r5,0.5,0.2,True)) 113 | 114 | mp = motionprogram_type() 115 | 116 | mp.motion_program_commands = mp_cmds 117 | mp.motion_setup_commands = setup_cmds 118 | 119 | # If multimove is available, optionally use a different robot other than first robot. 120 | # See multimove example for demonstration of synchronizing multiple robots 121 | # mp.extended = { 122 | # "groups": RR.VarValue("T_ROB2", "string") 123 | # } 124 | 125 | mp_gen = c.execute_motion_program_record(mp, False) 126 | res = None 127 | 128 | status = None 129 | while True: 130 | res, status1 = mp_gen.TryNext() 131 | if not res: 132 | break 133 | status = status1 134 | print(status) 135 | 136 | 137 | print(f"recording_handle: {status.recording_handle}") 138 | 139 | robot_recording = c.read_recording(status.recording_handle).NextAll()[0] 140 | 141 | print(robot_recording.time) 142 | print(robot_recording.command_number) 143 | print(robot_recording.joints) 144 | 145 | c.clear_recordings() 146 | 147 | print("Done!") -------------------------------------------------------------------------------- /examples/robotraconteur/example_client_egm_realtime_feedback.py: -------------------------------------------------------------------------------- 1 | # This example demonstrates running a motion program and receiving real-time feedback using EGM 2 | # and abb-robot-client. Both the abb-motion-program-robotraconteur and abb-robot-client-robotraconteur 3 | # services must be started. The abb-robot-client-robotraconteur service provides low-level RWS interface 4 | # and EGM interface to the robot. The abb-motion-program-robotraconteur service generates and 5 | # commands motion programs. 6 | # 7 | # See the abb-robot-client GitHub repository for more information: https://github.com/rpiRobotics/abb_robot_client 8 | # 9 | # Run the two services in separate terminals: 10 | # 11 | # abb-motion-program-exec-robotraconteur --mp-robot-base-url=http://127.0.0.1:80 --mp-robot-info-file=abb_motion_program_exec/config/abb_1200_5_90_motion_program_robot_default_config.yml 12 | # 13 | # abb-robot-client-robotraconteur --robot-url=http://127.0.0.1:80 --robot-info-file=abb_robot_client/config/abb_1200_5_90_rws_default_config.yml 14 | # 15 | # Replace the IP address with the IP address of the robot controller. 16 | # The EGM must be configured to send UDP data to the IP address running the robot client on the port 6510. 17 | 18 | from RobotRaconteur.Client import * 19 | import numpy as np 20 | import time 21 | 22 | c = RRN.ConnectService("rr+tcp://localhost:59843?service=mp_robot") 23 | c_abb_client = RRN.ConnectService('rr+tcp://localhost:59926?service=robot') 24 | 25 | # Connect the robot_state and egm_state wires 26 | robot_state_wire = c_abb_client.robot_state.Connect() 27 | egm_state_wire = c_abb_client.egm_state.Connect() 28 | 29 | robot_pose_type = RRN.GetStructureType("experimental.robotics.motion_program.RobotPose",c) 30 | moveabsj_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveAbsJCommand",c) 31 | movej_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveJCommand",c) 32 | movel_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveLCommand",c) 33 | movec_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveCCommand",c) 34 | settool_type = RRN.GetStructureType("experimental.robotics.motion_program.SetToolCommand",c) 35 | motionprogram_type = RRN.GetStructureType("experimental.robotics.motion_program.MotionProgram",c) 36 | toolinfo_type = RRN.GetStructureType("com.robotraconteur.robotics.tool.ToolInfo",c) 37 | transform_dt = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Transform",c) 38 | spatialinertia_dt = RRN.GetNamedArrayDType("com.robotraconteur.geometry.SpatialInertia",c) 39 | 40 | toolinfo = toolinfo_type() 41 | toolinfo.tcp = RRN.ArrayToNamedArray([1,0,0,0,0,0,0.001],transform_dt) 42 | toolinfo.inertia = RRN.ArrayToNamedArray([0.1,0,0,0.01,.001,0,0,.001,0,.001],spatialinertia_dt) 43 | 44 | settool = settool_type() 45 | settool.tool_info = toolinfo 46 | 47 | j1 = np.deg2rad(np.array([10,20,30,40,50,60],dtype=np.float64)) 48 | j2 = np.deg2rad(np.array([90,-91,60,-93,94,-95],dtype=np.float64)) 49 | j3 = np.deg2rad(np.array([-80,81,-82,83,-84,85],dtype=np.float64)) 50 | 51 | 52 | def robot_pose(p,q,conf): 53 | ret = robot_pose_type() 54 | ret.tcp_pose[0]["orientation"]["w"] = q[0] 55 | ret.tcp_pose[0]["orientation"]["x"] = q[1] 56 | ret.tcp_pose[0]["orientation"]["y"] = q[2] 57 | ret.tcp_pose[0]["orientation"]["z"] = q[3] 58 | ret.tcp_pose[0]["position"]["x"] = p[0]*1e-3 59 | ret.tcp_pose[0]["position"]["y"] = p[1]*1e-3 60 | ret.tcp_pose[0]["position"]["z"] = p[2]*1e-3 61 | 62 | ret.joint_position_seed=np.zeros((6,)) 63 | ret.joint_position_seed[0] = conf[0]*np.pi/2 64 | ret.joint_position_seed[3] = conf[1]*np.pi/2 65 | ret.joint_position_seed[5] = conf[2]*np.pi/2 66 | 67 | return ret 68 | 69 | 70 | r1 = robot_pose([0.1649235*1e3, 0.1169957*1e3, 0.9502961*1e3], [ 0.6776466, -0.09003431, 0.6362069, 0.3576725 ], (0,0,0,0)) 71 | r2 = robot_pose([ 0.6243948*1e3, -0.479558*1e3 , 0.7073749*1e3], [ 0.6065634, -0.2193409, 0.6427138, -0.4133877], (-1,-1,0,1)) 72 | 73 | r3 = robot_pose([417.9236, 276.9956, 885.2959], [ 0.8909725 , -0.1745558 , 0.08864544, 0.4096832 ], ( 0., 1., -2., 0.)) 74 | r4 = robot_pose([417.9235 , -11.00438, 759.2958 ], [0.7161292 , 0.1868255 , 0.01720813, 0.6722789 ], ( 0., 2., -2., 0.)) 75 | r5 = robot_pose([ 417.9235, -173.0044, 876.2958], [0.6757616, 0.3854275, 0.2376617, 0.5816431], (-1., 1., -1., 0.)) 76 | 77 | setup_cmds = [] 78 | mp_cmds = [] 79 | 80 | def moveabsj(j,velocity,blend_radius,fine_point): 81 | cmd = moveabsj_type() 82 | cmd.joint_position = j 83 | cmd.tcp_velocity = velocity 84 | cmd.blend_radius = blend_radius 85 | cmd.fine_point = fine_point 86 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveAbsJCommand") 87 | 88 | def movel(robot_pose,velocity,blend_radius,fine_point): 89 | cmd = movel_type() 90 | cmd.tcp_pose = robot_pose 91 | cmd.tcp_velocity = velocity 92 | cmd.blend_radius = blend_radius 93 | cmd.fine_point = fine_point 94 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveLCommand") 95 | 96 | def movej(robot_pose,velocity,blend_radius,fine_point): 97 | cmd = movej_type() 98 | cmd.tcp_pose = robot_pose 99 | cmd.tcp_velocity = velocity 100 | cmd.blend_radius = blend_radius 101 | cmd.fine_point = fine_point 102 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveJCommand") 103 | 104 | def movec(robot_pose,robot_via_pose,velocity,blend_radius,fine_point): 105 | cmd = movec_type() 106 | cmd.tcp_pose = robot_pose 107 | cmd.tcp_via_pose = robot_via_pose 108 | cmd.tcp_velocity = velocity 109 | cmd.blend_radius = blend_radius 110 | cmd.fine_point = fine_point 111 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveCCommand") 112 | 113 | setup_cmds.append(RR.VarValue(settool,"experimental.robotics.motion_program.SetToolCommand")) 114 | 115 | # Add an "EGM start streaming" command to setup_cmds 116 | egm_streaming_cmd = RRN.NewStructure("experimental.abb_robot.motion_program.EGMStreamConfigCommand",c) 117 | setup_cmds.append(RR.VarValue(egm_streaming_cmd, "experimental.abb_robot.motion_program.EGMStreamConfigCommand")) 118 | 119 | mp_cmds.append(moveabsj(j1,0.5,0.2,False)) 120 | mp_cmds.append(moveabsj(j2,1,0.1,True)) 121 | mp_cmds.append(moveabsj(j3,2,0.3,False)) 122 | mp_cmds.append(moveabsj(j1,0.5,0.2,True)) 123 | 124 | mp_cmds.append(movel(r1,0.5,0.02,False)) 125 | mp_cmds.append(movel(r2,0.5,0.2,True)) 126 | 127 | mp_cmds.append(movej(r1,0.5,0.02,False)) 128 | mp_cmds.append(movej(r3,0.5,0.2,True)) 129 | 130 | # mp_cmds.append(movec(r4,r5,0.5,0.2,True)) 131 | 132 | mp = motionprogram_type() 133 | 134 | mp.motion_program_commands = mp_cmds 135 | mp.motion_setup_commands = setup_cmds 136 | 137 | # If multimove is available, optionally use a different robot other than first robot. 138 | # See multimove example for demonstration of synchronizing multiple robots 139 | # mp.extended = { 140 | # "groups": RR.VarValue("T_ROB2", "string") 141 | # } 142 | 143 | mp_gen = c.execute_motion_program(mp, False) 144 | res = None 145 | 146 | 147 | # Use AsyncNext to communicate with executor generator. A thread could also be used, but this 148 | # solution is usually more efficient. 149 | def next_cb(res1, err): 150 | global done 151 | if err is not None: 152 | if isinstance(err, RR.StopIterationException): 153 | # Normal end of generator 154 | done = True 155 | return 156 | else: 157 | # Error in generator 158 | done = True 159 | # In a real program, handle the error. For this example, just print 160 | print(err) 161 | return 162 | print(res1) 163 | # Call AsyncNext again to continue 164 | mp_gen.AsyncNext(None, next_cb) 165 | 166 | done = False 167 | 168 | # Call AsyncNext to start the generator 169 | mp_gen.AsyncNext(None, next_cb) 170 | 171 | while not done: 172 | print(robot_state_wire.InValue) 173 | print(egm_state_wire.InValue) 174 | time.sleep(0.05) 175 | 176 | print("Done!") -------------------------------------------------------------------------------- /examples/robotraconteur/multimove_example_client.py: -------------------------------------------------------------------------------- 1 | from RobotRaconteur.Client import * 2 | import numpy as np 3 | 4 | c = RRN.ConnectService("rr+tcp://localhost:59843?service=mp_robot") 5 | 6 | robot_pose_type = RRN.GetStructureType("experimental.robotics.motion_program.RobotPose",c) 7 | moveabsj_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveAbsJCommand",c) 8 | movej_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveJCommand",c) 9 | movel_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveLCommand",c) 10 | movec_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveCCommand",c) 11 | settool_type = RRN.GetStructureType("experimental.robotics.motion_program.SetToolCommand",c) 12 | motionprogram_type = RRN.GetStructureType("experimental.robotics.motion_program.MotionProgram",c) 13 | toolinfo_type = RRN.GetStructureType("com.robotraconteur.robotics.tool.ToolInfo",c) 14 | transform_dt = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Transform",c) 15 | spatialinertia_dt = RRN.GetNamedArrayDType("com.robotraconteur.geometry.SpatialInertia",c) 16 | 17 | sync_move_on_type = RRN.GetStructureType("experimental.abb_robot.motion_program.SyncMoveOnCommand",c) 18 | sync_move_off_type = RRN.GetStructureType("experimental.abb_robot.motion_program.SyncMoveOnCommand",c) 19 | 20 | toolinfo = toolinfo_type() 21 | toolinfo.tcp = RRN.ArrayToNamedArray([1,0,0,0,0,0,0.1],transform_dt) 22 | toolinfo.inertia = RRN.ArrayToNamedArray([0.1,0,0,0.01,.001,0,0,.001,0,.001],spatialinertia_dt) 23 | 24 | settool = settool_type() 25 | settool.tool_info = toolinfo 26 | 27 | toolinfo_2 = toolinfo_type() 28 | toolinfo_2.tcp = RRN.ArrayToNamedArray([1,0,0,0,0,0,0.5],transform_dt) 29 | toolinfo_2.inertia = RRN.ArrayToNamedArray([0.1,0,0,0.01,.001,0,0,.001,0,.001],spatialinertia_dt) 30 | 31 | settool_2 = settool_type() 32 | settool_2.tool_info = toolinfo_2 33 | 34 | def robot_pose(p,q,conf): 35 | ret = robot_pose_type() 36 | ret.tcp_pose[0]["orientation"]["w"] = q[0] 37 | ret.tcp_pose[0]["orientation"]["x"] = q[1] 38 | ret.tcp_pose[0]["orientation"]["y"] = q[2] 39 | ret.tcp_pose[0]["orientation"]["z"] = q[3] 40 | ret.tcp_pose[0]["position"]["x"] = p[0]*1e-3 41 | ret.tcp_pose[0]["position"]["y"] = p[1]*1e-3 42 | ret.tcp_pose[0]["position"]["z"] = p[2]*1e-3 43 | 44 | # ret.joint_position_seed=np.zeros((6,)) 45 | # ret.joint_position_seed[0] = conf[0]*np.pi/2 46 | # ret.joint_position_seed[3] = conf[1]*np.pi/2 47 | # ret.joint_position_seed[5] = conf[2]*np.pi/2 48 | 49 | # Using JointSeed is the most portable way to specify robot configuration, but can be unreliable. 50 | # Use confdata instead for ABB robots in the waypoint extended field 51 | confdata = RR.VarValue(np.array(conf,dtype=np.float64),"double[]") 52 | return ret, confdata 53 | 54 | j1 = np.deg2rad(np.array([5,-20,30,27,-11,-27],dtype=np.float64)) 55 | t1 = robot_pose([575,-200,1280],[0,-.707,0,.707],(0,0,-1,1),) 56 | t2 = robot_pose([575,200,1480],[0,-.707,0,.707],(-1,-1,0,1),) 57 | t3 = robot_pose([575,0,1280],[0,-.707,0,.707],(-1,-1,0,1),) 58 | 59 | j1_2 = np.deg2rad(np.array([1,1,40,2,-40,-2],dtype=np.float64)) 60 | t1_2 = robot_pose([250,-200,1280],[.707,0,.707,0],(-1,-1,0,1)) 61 | t2_2 = robot_pose([250,200,1480],[.707,0,.707,0],(0,0,-1,1)) 62 | t3_2 = robot_pose([250,0,1280],[.707,0,.707,0],(0,0,0,1)) 63 | 64 | 65 | setup_cmds = [] 66 | mp_cmds = [] 67 | 68 | setup_cmds_2 = [] 69 | mp_cmds_2 = [] 70 | 71 | def moveabsj(j,velocity,blend_radius,fine_point): 72 | cmd = moveabsj_type() 73 | cmd.joint_position = j 74 | cmd.tcp_velocity = velocity 75 | cmd.blend_radius = blend_radius 76 | cmd.fine_point = fine_point 77 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveAbsJCommand") 78 | 79 | def movel(robot_pose,velocity,blend_radius,fine_point): 80 | cmd = movel_type() 81 | cmd.tcp_pose = robot_pose[0] 82 | cmd.tcp_velocity = velocity 83 | cmd.blend_radius = blend_radius 84 | cmd.fine_point = fine_point 85 | cmd.extended = { 86 | "confdata": robot_pose[1] 87 | } 88 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveLCommand") 89 | 90 | def movej(robot_pose,velocity,blend_radius,fine_point): 91 | cmd = movej_type() 92 | cmd.tcp_pose = robot_pose[0] 93 | cmd.tcp_velocity = velocity 94 | cmd.blend_radius = blend_radius 95 | cmd.fine_point = fine_point 96 | cmd.extended = { 97 | "confdata": robot_pose[1] 98 | } 99 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveJCommand") 100 | 101 | def movec(robot_pose,robot_via_pose,velocity,blend_radius,fine_point): 102 | cmd = movec_type() 103 | cmd.tcp_pose = robot_pose[0] 104 | cmd.tcp_via_pose = robot_via_pose[0] 105 | cmd.tcp_velocity = velocity 106 | cmd.blend_radius = blend_radius 107 | cmd.fine_point = fine_point 108 | cmd.extended = { 109 | "confdata": robot_pose[1], 110 | "confdata_via": robot_via_pose[1] 111 | } 112 | return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveCCommand") 113 | 114 | def sync_move_on(): 115 | cmd = sync_move_on_type() 116 | return RR.VarValue(cmd,"experimental.abb_robot.motion_program.SyncMoveOnCommand") 117 | 118 | def sync_move_off(): 119 | cmd = sync_move_off_type() 120 | return RR.VarValue(cmd,"experimental.abb_robot.motion_program.SyncMoveOffCommand") 121 | 122 | setup_cmds.append(RR.VarValue(settool,"experimental.robotics.motion_program.SetToolCommand")) 123 | mp_cmds.append(sync_move_on()) 124 | mp_cmds.append(moveabsj(j1,0.5,0.2,True)) 125 | mp_cmds.append(movej(t1,0.5,0.02,True)) 126 | mp_cmds.append(movej(t2,0.5,0.2,True)) 127 | mp_cmds.append(movej(t3,0.5,0.2,True)) 128 | 129 | setup_cmds_2.append(RR.VarValue(settool_2,"experimental.robotics.motion_program.SetToolCommand")) 130 | mp_cmds_2.append(sync_move_on()) 131 | mp_cmds_2.append(moveabsj(j1_2,0.5,0.2,True)) 132 | mp_cmds_2.append(movej(t1_2,0.5,0.02,True)) 133 | mp_cmds_2.append(movej(t2_2,0.5,0.2,True)) 134 | mp_cmds_2.append(movej(t3_2,0.5,0.2,True)) 135 | 136 | 137 | # mp_cmds.append(movec(r4,r5,0.5,0.2,True)) 138 | 139 | mp = motionprogram_type() 140 | 141 | mp.motion_program_commands = mp_cmds 142 | mp.motion_setup_commands = setup_cmds 143 | 144 | # Build up second motion program 145 | mp_2 = motionprogram_type() 146 | mp_2.motion_program_commands = mp_cmds_2 147 | mp_2.motion_setup_commands = setup_cmds_2 148 | 149 | # Set the groups and add the second motion program using "extended" field 150 | mp.extended = { 151 | "multi_motion_programs": RR.VarValue([mp_2], "experimental.robotics.motion_program.MotionProgram{list}"), 152 | "groups": RR.VarValue(["T_ROB1", "T_ROB2"], "string{list}") 153 | } 154 | 155 | # Optionally use integers for groups. Zero indexed 156 | # mp.extended = { 157 | # "groups": RR.VarValue([0, 1], "int32{list}") 158 | # } 159 | 160 | mp_gen = c.execute_motion_program_record(mp, False) 161 | res = None 162 | 163 | status = None 164 | while True: 165 | res, status1 = mp_gen.TryNext() 166 | if not res: 167 | break 168 | status = status1 169 | print(status) 170 | 171 | 172 | print(f"recording_handle: {status.recording_handle}") 173 | 174 | robot_recording = c.read_recording(status.recording_handle).NextAll()[0] 175 | 176 | print(robot_recording.time) 177 | print(robot_recording.command_number) 178 | print(robot_recording.joints) 179 | 180 | c.clear_recordings() 181 | 182 | print("Done!") -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools", "wheel"] 3 | 4 | [project] 5 | name = "abb_motion_program_exec" 6 | version = "0.7.4" 7 | authors = [ 8 | {name ="John Wason", email = "wason@wasontech.com"} 9 | ] 10 | description = "Python package to execute motion commands on ABB robots and log results" 11 | license = {text = "Apache-2.0"} 12 | requires-python = ">=3.6" 13 | dependencies = [ 14 | "requests", 15 | "numpy", 16 | "abb-robot-client[aio]", 17 | "dataclasses; python_version<'3.7'" 18 | ] 19 | readme = "README.md" 20 | 21 | [project.urls] 22 | homepage = "https://github.com/rpiRobotics/abb_motion_program_exec" 23 | documentation = "https://abb-motion-program-exec.readthedocs.io/en/latest/" 24 | repository = "https://github.com/rpiRobotics/abb_motion_program_exec" 25 | 26 | [project.scripts] 27 | abb-motion-program-exec-robotraconteur = "abb_motion_program_exec.robotraconteur.abb_motion_program_exec_robotraconteur:main" 28 | 29 | [project.optional-dependencies] 30 | test = [ 31 | "pytest", 32 | ] 33 | robotraconteur = [ 34 | "robotraconteur", 35 | "robotraconteurcompanion", 36 | "drekar-launch-process", 37 | "robotraconteur-abstract-robot" 38 | ] 39 | 40 | [tool.setuptools.package-data] 41 | "abb_motion_program_exec.robotraconteur" = ["*.robdef"] -------------------------------------------------------------------------------- /robot/HOME/error_reporter.mod: -------------------------------------------------------------------------------- 1 | MODULE error_reporter 2 | 3 | VAR intnum motion_program_err_interrupt; 4 | 5 | PROC main() 6 | CONNECT motion_program_err_interrupt WITH motion_program_trap_err; 7 | IError COMMON_ERR, TYPE_ERR, motion_program_err_interrupt; 8 | WHILE TRUE DO 9 | WaitTime 1; 10 | ENDWHILE 11 | 12 | ENDPROC 13 | 14 | TRAP motion_program_trap_err 15 | VAR trapdata err_data; 16 | VAR errdomain err_domain; 17 | VAR num err_number; 18 | VAR errtype err_type; 19 | VAR string titlestr; 20 | VAR string string1; 21 | VAR string string2; 22 | VAR num seqno; 23 | VAR string err_filename; 24 | VAR iodev err_file; 25 | 26 | GetTrapData err_data; 27 | ReadErrData err_data, err_domain, err_number, err_type \Title:=titlestr \Str1:=string1 \Str2:=string2; 28 | 29 | ErrWrite \W, "Motion Program Failed", "Motion Program Failed at command number " + NumToStr(motion_program_state{1}.current_cmd_num,0) 30 | \RL2:= "with error code " + NumToStr(err_domain*10000+err_number,0) 31 | \RL3:= "error title '" + titlestr + "'" 32 | \RL4:= "error string '" + string1 + "'"; 33 | 34 | seqno := motion_program_seqno_started; 35 | IF seqno > 0 THEN 36 | err_filename := "motion_program_err---seqno-" + NumToStr(seqno,0) + ".json"; 37 | Open "RAMDISK:"\File:=err_filename,err_file\Write; 38 | Write err_file, "{""seqno"": " + NumToStr(seqno,0)\NoNewLine; 39 | Write err_file, ",""error_domain"": " + NumToStr(err_domain,0)\NoNewLine; 40 | Write err_file, ",""error_number"": " + NumToStr(err_number,0)\NoNewLine; 41 | Write err_file, ",""error_title"": """ + titlestr + """"\NoNewLine; 42 | Write err_file, ",""error_string"": """ + string1 + """"\NoNewLine; 43 | Write err_file, ",""error_string2"": """ + string2 + """"\NoNewLine; 44 | Write err_file, "}"; 45 | Close err_file; 46 | ENDIF 47 | 48 | 49 | ENDTRAP 50 | ENDMODULE -------------------------------------------------------------------------------- /robot/HOME/motion_program_exec_egm.mod: -------------------------------------------------------------------------------- 1 | MODULE motion_program_exec_egm 2 | 3 | CONST num egm_sample_rate:=4; 4 | 5 | TASK PERS bool egm_symbol_fail:=FALSE; 6 | TASK PERS bool have_egm:=TRUE; 7 | LOCAL VAR egmident egmID1; 8 | LOCAL VAR egmstate egmSt1; 9 | 10 | LOCAL VAR errnum ERR_NO_EGM:=-1; 11 | 12 | CONST num MOTION_PROGRAM_CMD_EGMJOINT:=50001; 13 | CONST num MOTION_PROGRAM_CMD_EGMPOSE:=50002; 14 | CONST num MOTION_PROGRAM_CMD_EGMMOVEL:=50003; 15 | CONST num MOTION_PROGRAM_CMD_EGMMOVEC:=50004; 16 | 17 | PROC motion_program_egm_init() 18 | 19 | BookErrNo ERR_NO_EGM; 20 | 21 | IF egm_symbol_fail THEN 22 | RETURN ; 23 | ENDIF 24 | 25 | EGMReset egmID1; 26 | WaitTime 0.005; 27 | EGMGetId egmID1; 28 | egmSt1:=EGMGetState(egmID1); 29 | 30 | 31 | EGMSetupUC ROB_1,egmID1,"joint","UCdevice"\Joint\CommTimeout:=100000; 32 | 33 | 34 | !IF egmST1=EGM_STATE_RUNNING THEN 35 | ! EGMStop egmID1,EGM_STOP_HOLD; 36 | !ENDIF 37 | 38 | EGMStreamStop egmID1; 39 | 40 | IF NOT have_egm THEN 41 | have_egm:=TRUE; 42 | ENDIF 43 | ENDPROC 44 | 45 | PROC motion_program_egm_start_stream() 46 | IF have_egm THEN 47 | EGMStreamStart egmID1\SampleRate:=egm_sample_rate; 48 | ENDIF 49 | ENDPROC 50 | 51 | PROC motion_program_egm_enable() 52 | VAR num egm_cmd; 53 | IF NOT try_motion_program_read_num(egm_cmd) THEN 54 | RAISE ERR_INVALID_MP_FILE; 55 | ENDIF 56 | 57 | ! EGM no command specified, enable streaming 58 | IF egm_cmd=0 THEN 59 | IF have_egm THEN 60 | EGMStreamStart egmID1\SampleRate:=egm_sample_rate; 61 | ENDIF 62 | RETURN ; 63 | ENDIF 64 | 65 | IF NOT have_egm THEN 66 | RAISE ERR_NO_EGM; 67 | ENDIF 68 | 69 | ! EGM joint position command 70 | IF egm_cmd=1 THEN 71 | motion_program_egm_enable_joint; 72 | RETURN ; 73 | ENDIF 74 | 75 | ! EGM pose command 76 | IF egm_cmd=2 THEN 77 | motion_program_egm_enable_pose; 78 | RETURN ; 79 | ENDIF 80 | 81 | ! EGM pose command 82 | IF egm_cmd=3 THEN 83 | motion_program_egm_enable_corr; 84 | RETURN ; 85 | ENDIF 86 | 87 | RAISE ERR_INVALID_OPCODE; 88 | 89 | ENDPROC 90 | 91 | PROC motion_program_egm_enable_joint() 92 | VAR egm_minmax minmax{6}; 93 | VAR num pos_dev; 94 | VAR num speed_dev; 95 | VAR num i; 96 | VAR jointtarget joints; 97 | FOR i FROM 1 TO 6 DO 98 | IF NOT try_read_egm_minmax(minmax{i}) THEN 99 | RAISE ERR_INVALID_MP_FILE; 100 | ENDIF 101 | ENDFOR 102 | 103 | IF NOT try_motion_program_read_num(pos_dev) AND try_motion_program_read_num(speed_dev) THEN 104 | RAISE ERR_INVALID_MP_FILE; 105 | ENDIF 106 | 107 | ! "Move" a tiny amount to start EGM 108 | joints:=CJointT(); 109 | joints.robax.rax_6:=joints.robax.rax_6+.0001; 110 | MoveAbsj joints,v1000,fine,motion_program_tool\WObj:=motion_program_wobj; 111 | 112 | EGMActJoint egmID1,\Tool:=motion_program_tool,\WObj:=motion_program_wobj\J1:=minmax{1} 113 | \J2:=minmax{2}\J3:=minmax{3}\J4:=minmax{4}\J5:=minmax{5}\J6:=minmax{6}\MaxPosDeviation:=pos_dev\MaxSpeedDeviation:=speed_dev; 114 | ENDPROC 115 | 116 | PROC motion_program_egm_enable_pose() 117 | VAR pose posecor; 118 | VAR egmframetype posecor_frtype; 119 | VAR pose posesens; 120 | VAR egmframetype posesens_frtype; 121 | VAR egm_minmax minmax{6}; 122 | VAR num pos_dev; 123 | VAR num speed_dev; 124 | VAR num i; 125 | VAR jointtarget joints; 126 | 127 | IF NOT ( 128 | try_motion_program_read_pose(posecor) 129 | AND try_read_egm_frtype(posecor_frtype) 130 | AND try_motion_program_read_pose(posesens) 131 | AND try_read_egm_frtype(posesens_frtype) 132 | ) THEN 133 | RAISE ERR_INVALID_MP_FILE; 134 | ENDIF 135 | 136 | FOR i FROM 1 TO 6 DO 137 | IF NOT try_read_egm_minmax(minmax{i}) THEN 138 | RAISE ERR_INVALID_MP_FILE; 139 | ENDIF 140 | ENDFOR 141 | 142 | IF NOT try_motion_program_read_num(pos_dev) AND try_motion_program_read_num(speed_dev) THEN 143 | RAISE ERR_INVALID_MP_FILE; 144 | ENDIF 145 | 146 | EGMSetupUC ROB_1,egmID1,"pose","UCdevice"\Pose\CommTimeout:=100000; 147 | 148 | ! "Move" a tiny amount to start EGM 149 | joints:=CJointT(); 150 | joints.robax.rax_6:=joints.robax.rax_6+.0001; 151 | MoveAbsj joints,v1000,fine,motion_program_tool\WObj:=motion_program_wobj; 152 | 153 | EGMActPose egmID1,\StreamStart,\Tool:=motion_program_tool,\WObj:=motion_program_wobj,posecor,posecor_frtype,posesens,posesens_frtype, 154 | \X:=minmax{1}\Y:=minmax{2}\Z:=minmax{3}\Rx:=minmax{4}\Ry:=minmax{5}\Rz:=minmax{6}\MaxPosDeviation:=pos_dev\MaxSpeedDeviation:=speed_dev; 155 | ENDPROC 156 | 157 | PROC motion_program_egm_enable_corr() 158 | VAR pose posesens; 159 | VAR jointtarget joints; 160 | IF NOT try_motion_program_read_pose(posesens) THEN 161 | RAISE ERR_INVALID_MP_FILE; 162 | ENDIF 163 | 164 | EGMSetupUC ROB_1,egmID1,"path_corr","UCdevice"\PathCorr\APTR; 165 | 166 | joints:=CJointT(); 167 | joints.robax.rax_6:=joints.robax.rax_6+.0001; 168 | MoveAbsj joints,v1000,fine,motion_program_tool\WObj:=motion_program_wobj; 169 | 170 | EGMActMove egmID1,posesens\SampleRate:=48; 171 | 172 | ENDPROC 173 | 174 | FUNC bool try_read_egm_minmax(INOUT egm_minmax minmax) 175 | RETURN (try_motion_program_read_num(minmax.min) AND try_motion_program_read_num(minmax.max)); 176 | ENDFUNC 177 | 178 | FUNC bool try_read_egm_frtype(INOUT egmframetype frtype) 179 | VAR num frtype_code; 180 | IF NOT try_motion_program_read_num(frtype_code) THEN 181 | RETURN FALSE; 182 | ENDIF 183 | 184 | IF frtype_code=1 THEN 185 | frtype:=EGM_FRAME_TOOL; 186 | ELSEIF frtype_code=2 THEN 187 | frtype:=EGM_FRAME_WOBJ; 188 | ELSEIF frtype_code=3 THEN 189 | frtype:=EGM_FRAME_WORLD; 190 | ELSEIF frtype_code=4 THEN 191 | frtype:=EGM_FRAME_JOINT; 192 | ELSE 193 | frtype:=EGM_FRAME_BASE; 194 | ENDIF 195 | RETURN TRUE; 196 | ENDFUNC 197 | 198 | FUNC bool try_motion_program_egmrunjoint() 199 | VAR num condtime; 200 | VAR num rampin; 201 | VAR num rampout; 202 | 203 | IF NOT ( 204 | try_motion_program_read_num(condtime) 205 | AND try_motion_program_read_num(rampin) 206 | AND try_motion_program_read_num(rampout) 207 | ) THEN 208 | RETURN FALSE; 209 | ENDIF 210 | 211 | SetDO motion_program_stop_egm,0; 212 | EGMRunJoint egmID1,EGM_STOP_HOLD,\NoWaitCond\J1\J2\J3\J4\J5\J6\CondTime:=condtime\RampInTime:=rampin; 213 | WaitDO motion_program_stop_egm,1; 214 | EGMStop egmID1,EGM_STOP_HOLD\RampOutTime:=rampout; 215 | EGMStreamStart egmID1\SampleRate:=egm_sample_rate; 216 | 217 | RETURN TRUE; 218 | ENDFUNC 219 | 220 | FUNC bool try_motion_program_egmrunpose() 221 | VAR num condtime; 222 | VAR num rampin; 223 | VAR num rampout; 224 | VAR pose offset; 225 | 226 | IF NOT ( 227 | try_motion_program_read_num(condtime) 228 | AND try_motion_program_read_num(rampin) 229 | AND try_motion_program_read_num(rampout) 230 | AND try_motion_program_read_pose(offset) 231 | ) THEN 232 | RETURN FALSE; 233 | ENDIF 234 | 235 | SetDO motion_program_stop_egm,0; 236 | EGMRunPose egmID1,EGM_STOP_HOLD,\NoWaitCond\x\y\z\Rx\Ry\Rz\CondTime:=condtime\RampInTime:=rampin\RampOutTime:=rampout\Offset:=offset; 237 | WaitDO motion_program_stop_egm,1; 238 | EGMStop egmID1,EGM_STOP_HOLD\RampOutTime:=rampout; 239 | EGMStreamStart egmID1\SampleRate:=egm_sample_rate; 240 | RETURN TRUE; 241 | ENDFUNC 242 | 243 | FUNC bool try_motion_program_run_egmmovel(num cmd_num) 244 | VAR robtarget rt; 245 | VAR speeddata sd; 246 | VAR zonedata zd; 247 | IF NOT ( 248 | try_motion_program_read_rt(rt) 249 | AND try_motion_program_read_sd(sd) 250 | AND try_motion_program_read_zd(zd) 251 | ) THEN 252 | RETURN FALSE; 253 | ENDIF 254 | 255 | EGMMoveL egmID1,rt,sd,zd,motion_program_tool\WObj:=motion_program_wobj; 256 | 257 | RETURN TRUE; 258 | 259 | ENDFUNC 260 | 261 | FUNC bool try_motion_program_run_egmmovec(num cmd_num) 262 | VAR robtarget rt1; 263 | VAR robtarget rt2; 264 | VAR speeddata sd; 265 | VAR zonedata zd; 266 | IF NOT ( 267 | try_motion_program_read_rt(rt1) 268 | AND try_motion_program_read_rt(rt2) 269 | AND try_motion_program_read_sd(sd) 270 | AND try_motion_program_read_zd(zd) 271 | ) THEN 272 | RETURN FALSE; 273 | ENDIF 274 | 275 | EGMMoveC egmID1,rt1,rt2,sd,zd,motion_program_tool\WObj:=motion_program_wobj; 276 | 277 | RETURN TRUE; 278 | 279 | ENDFUNC 280 | 281 | FUNC bool try_motion_program_run_egm_cmd(num cmd_num,num cmd_op) 282 | TEST cmd_op 283 | CASE MOTION_PROGRAM_CMD_EGMJOINT: 284 | RETURN try_motion_program_egmrunjoint(); 285 | CASE MOTION_PROGRAM_CMD_EGMPOSE: 286 | RETURN try_motion_program_egmrunpose(); 287 | CASE MOTION_PROGRAM_CMD_EGMMOVEL: 288 | RETURN try_motion_program_run_egmmovel(cmd_num); 289 | CASE MOTION_PROGRAM_CMD_EGMMOVEC: 290 | RETURN try_motion_program_run_egmmovec(cmd_num); 291 | DEFAULT: 292 | RAISE ERR_INVALID_OPCODE; 293 | ENDTEST 294 | ENDFUNC 295 | 296 | ENDMODULE -------------------------------------------------------------------------------- /robot/HOME/motion_program_logger.mod: -------------------------------------------------------------------------------- 1 | MODULE motion_program_logger 2 | 3 | LOCAL VAR bool log_file_open:=FALSE; 4 | LOCAL VAR iodev log_io_device; 5 | LOCAL VAR clock time_stamp_clock; 6 | 7 | LOCAL VAR intnum rmqint_open; 8 | LOCAL VAR string rmq_timestamp; 9 | LOCAL VAR string rmq_filename; 10 | LOCAL VAR string rmq_req{2}; 11 | 12 | LOCAL VAR intnum logger_err_interrupt; 13 | LOCAL VAR num robot_count:=0; 14 | 15 | 16 | PROC motion_program_logger_main() 17 | 18 | VAR num clk_now; 19 | VAR num c:=0; 20 | VAR num loop_count:=0; 21 | VAR num clk_diff; 22 | 23 | VAR num mechunit_listnum:=0; 24 | VAR string mechunit_name:=""; 25 | VAR bool mechunit_is_robot:=FALSE; 26 | 27 | CONNECT rmqint_open WITH rmq_message_string; 28 | IRMQMessage rmq_req, rmqint_open; 29 | 30 | CONNECT logger_err_interrupt WITH err_handler; 31 | IError COMMON_ERR, TYPE_ERR, logger_err_interrupt; 32 | 33 | WHILE GetNextMechUnit(mechunit_listnum, mechunit_name\TCPRob:=mechunit_is_robot) DO 34 | IF mechunit_is_robot THEN 35 | robot_count:=robot_count+1; 36 | ENDIF 37 | ENDWHILE 38 | 39 | ClkReset time_stamp_clock; 40 | ClkStart time_stamp_clock; 41 | WHILE TRUE DO 42 | clk_now:=ClkRead(time_stamp_clock \HighRes); 43 | motion_program_state{1}.clk_time:=clk_now; 44 | motion_program_state{1}.joint_position:=CJointT(\TaskRef:=T_ROB1Id); 45 | IF robot_count >= 2 THEN 46 | motion_program_state{2}.joint_position:=CJointT(\TaskName:="T_ROB2"); 47 | ENDIF 48 | IF log_file_open THEN 49 | clk_diff:= loop_count*0.004 - clk_now; 50 | loop_count := loop_count+1; 51 | IF clk_diff > 0 THEN 52 | WaitTime clk_diff; 53 | ENDIF 54 | ELSE 55 | loop_count := 0; 56 | ClkStop time_stamp_clock; 57 | ClkReset time_stamp_clock; 58 | WaitTime 0.004; 59 | ClkStart time_stamp_clock; 60 | ENDIF 61 | 62 | IDisable; 63 | IF motion_program_executing <> 0 THEN 64 | IF log_file_open THEN 65 | motion_program_log_data; 66 | ENDIF 67 | ENDIF 68 | IEnable; 69 | 70 | ENDWHILE 71 | ENDPROC 72 | 73 | PROC pack_num(num val, VAR rawbytes b) 74 | PackRawBytes val, b, (RawBytesLen(b)+1)\Float4; 75 | ENDPROC 76 | 77 | PROC pack_str(string val, VAR rawbytes b) 78 | PackRawBytes val, b, (RawBytesLen(b)+1)\ASCII; 79 | ENDPROC 80 | 81 | PROC pack_jointtarget(jointtarget jt, VAR rawbytes b) 82 | pack_num jt.robax.rax_1, b; 83 | pack_num jt.robax.rax_2, b; 84 | pack_num jt.robax.rax_3, b; 85 | pack_num jt.robax.rax_4, b; 86 | pack_num jt.robax.rax_5, b; 87 | pack_num jt.robax.rax_6, b; 88 | ENDPROC 89 | 90 | PROC motion_program_log_open() 91 | VAR string log_filename; 92 | VAR string header_str:="timestamp,cmdnum,J1,J2,J3,J4,J5,J6"; 93 | VAR num header_str_len; 94 | VAR num rmq_timestamp_len; 95 | VAR rawbytes header_bytes; 96 | 97 | IF robot_count >= 2 THEN 98 | header_str:="timestamp,cmdnum,J1,J2,J3,J4,J5,J6,J1_2,J2_2,J3_2,J4_2,J5_2,J6_2"; 99 | ENDIF 100 | 101 | header_str_len:=StrLen(header_str); 102 | rmq_timestamp_len:=StrLen(rmq_timestamp); 103 | log_filename := "log-" + rmq_filename + ".bin"; 104 | 105 | pack_num motion_program_file_version, header_bytes; 106 | pack_num rmq_timestamp_len, header_bytes; 107 | pack_str rmq_timestamp, header_bytes; 108 | pack_num header_str_len, header_bytes; 109 | pack_str header_str, header_bytes; 110 | Open "RAMDISK:" \File:=log_filename, log_io_device, \Write\Bin; 111 | WriteRawBytes log_io_device, header_bytes; 112 | ErrWrite \I, "Motion Program Log File Opened", "Motion Program Log File Opened with filename: " + log_filename; 113 | log_file_open := TRUE; 114 | ENDPROC 115 | 116 | PROC motion_program_log_close() 117 | Close log_io_device; 118 | log_file_open := FALSE; 119 | ErrWrite \I, "Motion Program Log File Closed", "Motion Program Log File Closed"; 120 | ERROR 121 | SkipWarn; 122 | TRYNEXT; 123 | ENDPROC 124 | 125 | PROC motion_program_log_data() 126 | VAR rawbytes data_bytes; 127 | pack_num ClkRead(time_stamp_clock \HighRes), data_bytes; 128 | pack_num motion_program_state{1}.current_cmd_num, data_bytes; 129 | pack_jointtarget motion_program_state{1}.joint_position, data_bytes; 130 | IF robot_count >= 2 THEN 131 | pack_jointtarget motion_program_state{2}.joint_position, data_bytes; 132 | ENDIF 133 | WriteRawBytes log_io_device, data_bytes; 134 | ENDPROC 135 | 136 | TRAP rmq_message_string 137 | VAR rmqmessage rmqmsg; 138 | IDisable; 139 | RMQGetMessage rmqmsg; 140 | RMQGetMsgData rmqmsg, rmq_req; 141 | rmq_timestamp:=rmq_req{1}; 142 | rmq_filename:=rmq_req{2}; 143 | IF log_file_open THEN 144 | motion_program_log_close; 145 | ENDIF 146 | IF StrLen(rmq_timestamp) > 0 AND motion_program_log_motion > 0 THEN 147 | motion_program_log_open; 148 | ENDIF 149 | IEnable; 150 | ENDTRAP 151 | 152 | 153 | TRAP err_handler 154 | 155 | VAR trapdata err_data; 156 | VAR errdomain err_domain; 157 | VAR num err_number; 158 | VAR errtype err_type; 159 | IDisable; 160 | ISleep logger_err_interrupt; 161 | 162 | GetTrapData err_data; 163 | ReadErrData err_data, err_domain, err_number, err_type; 164 | IF log_file_open THEN 165 | motion_program_log_close; 166 | ENDIF 167 | 168 | IWatch logger_err_interrupt; 169 | IEnable; 170 | 171 | ENDTRAP 172 | 173 | ENDMODULE -------------------------------------------------------------------------------- /robot/HOME/motion_program_shared.sys: -------------------------------------------------------------------------------- 1 | MODULE motion_program_shared 2 | 3 | RECORD motion_program_state_type 4 | bool running; 5 | num current_cmd_num; 6 | num queued_cmd_num; 7 | num preempt_current; 8 | string motion_program_filename; 9 | num clk_time; 10 | jointtarget joint_position; 11 | string program_timestamp; 12 | num program_seqno; 13 | ENDRECORD 14 | 15 | CONST num motion_program_file_version:=10011; 16 | 17 | PERS motion_program_state_type motion_program_state{2}; 18 | 19 | ENDMODULE 20 | 21 | -------------------------------------------------------------------------------- /robot/config_params/EIO.cfg: -------------------------------------------------------------------------------- 1 | EIO:CFG_1.0:6:1:: 2 | # 3 | SYSSIG_OUT: 4 | 5 | -Status "TaskExecuting" -Signal "motion_program_executing" -Arg2 "T_ROB1" 6 | 7 | -Status "Error" -Signal "motion_program_error" -Arg2 "T_ROB1" 8 | # 9 | EIO_SIGNAL: 10 | 11 | -Name "motion_program_executing" -SignalType "DO" 12 | 13 | -Name "motion_program_preempt" -SignalType "AO" -Access "All" 14 | 15 | -Name "motion_program_preempt_cmd_num" -SignalType "AO" -Access "All" 16 | 17 | -Name "motion_program_preempt_current" -SignalType "AO" 18 | 19 | -Name "motion_program_current_cmd_num" -SignalType "AO" 20 | 21 | -Name "motion_program_queued_cmd_num" -SignalType "AO" 22 | 23 | -Name "motion_program_seqno" -SignalType "AO" -Access "All" 24 | 25 | -Name "motion_program_log_motion" -SignalType "DO" -Access "All"\ 26 | -Default 1 27 | 28 | -Name "motion_program_error" -SignalType "DO" 29 | 30 | -Name "motion_program_seqno_command" -SignalType "AO" -Access "All" 31 | 32 | -Name "motion_program_seqno_complete" -SignalType "AO" -Access "All" 33 | 34 | -Name "motion_program_driver_abort" -SignalType "DO" -Access "All" 35 | 36 | -Name "motion_program_seqno_started" -SignalType "AO" -Access "All" 37 | -------------------------------------------------------------------------------- /robot/config_params/SYS.cfg: -------------------------------------------------------------------------------- 1 | SYS:CFG_1.0:6:0:: 2 | 3 | # 4 | CAB_TASK_MODULES: 5 | 6 | -File "HOME:/motion_program_shared.sys" -Shared 7 | 8 | -File "HOME:/motion_program_exec.mod" -ModName "motion_program_exec"\ 9 | -Task "T_ROB1" 10 | 11 | -File "HOME:/motion_program_logger.mod" -ModName "motion_program_logger"\ 12 | -Task "logger" 13 | 14 | -File "HOME:/error_reporter.mod" -ModName "error_reporter"\ 15 | -Task "error_reporter" 16 | # 17 | CAB_TASKS: 18 | 19 | -Name "T_ROB1" -Type "NORMAL" -BindRef 0 -Entry "motion_program_main"\ 20 | -MotionTask 21 | 22 | -Name "logger" -Entry "motion_program_logger_main" -TrustLevel "None"\ 23 | -RmqType "Internal" 24 | 25 | -Name "error_reporter" -TrustLevel "None" 26 | -------------------------------------------------------------------------------- /robot/config_params_egm/EIO.cfg: -------------------------------------------------------------------------------- 1 | EIO:CFG_1.0:6:1:: 2 | # 3 | SYSSIG_OUT: 4 | 5 | -Status "TaskExecuting" -Signal "motion_program_executing" -Arg2 "T_ROB1" 6 | 7 | -Status "Error" -Signal "motion_program_error" -Arg2 "T_ROB1" 8 | # 9 | EIO_SIGNAL: 10 | 11 | -Name "motion_program_executing" -SignalType "DO" 12 | 13 | -Name "motion_program_preempt" -SignalType "AO" -Access "All" 14 | 15 | -Name "motion_program_preempt_cmd_num" -SignalType "AO" -Access "All" 16 | 17 | -Name "motion_program_preempt_current" -SignalType "AO" 18 | 19 | -Name "motion_program_current_cmd_num" -SignalType "AO" 20 | 21 | -Name "motion_program_queued_cmd_num" -SignalType "AO" 22 | 23 | -Name "motion_program_stop_egm" -SignalType "DO" -Access "All" 24 | 25 | -Name "motion_program_egm_active" -SignalType "AO" -Access "All" 26 | 27 | -Name "motion_program_seqno" -SignalType "AO" -Access "All" 28 | 29 | -Name "motion_program_log_motion" -SignalType "DO" -Access "All"\ 30 | -Default 1 31 | 32 | -Name "motion_program_error" -SignalType "DO" 33 | 34 | -Name "motion_program_seqno_command" -SignalType "AO" -Access "All" 35 | 36 | -Name "motion_program_seqno_complete" -SignalType "AO" -Access "All" 37 | 38 | -Name "motion_program_driver_abort" -SignalType "DO" -Access "All" 39 | 40 | -Name "motion_program_seqno_started" -SignalType "AO" -Access "All" 41 | -------------------------------------------------------------------------------- /robot/config_params_egm/MOC.cfg: -------------------------------------------------------------------------------- 1 | MOC:CFG_1.0:6:0:: 2 | # 3 | EXT_MOTION_DATA: 4 | 5 | -name "joint" -ext_motion_level 0 -do_not_restart_after_motors_off \ 6 | -ramp_time 0.005 -ext_motion_Kp 20 -ext_motion_filter_bandwidth 100 7 | 8 | -name "pose" -ext_motion_level 0 -do_not_restart_after_motors_off \ 9 | -ramp_time 0.005 -ext_motion_Kp 20 -ext_motion_filter_bandwidth 100 10 | 11 | -name "path_corr" -ext_motion_level 2 -do_not_restart_after_motors_off -------------------------------------------------------------------------------- /robot/config_params_egm/SYS.cfg: -------------------------------------------------------------------------------- 1 | SYS:CFG_1.0:6:0:: 2 | 3 | # 4 | CAB_TASK_MODULES: 5 | 6 | -File "HOME:/motion_program_shared.sys" -Shared 7 | 8 | -File "HOME:/motion_program_exec.mod" -ModName "motion_program_exec"\ 9 | -Task "T_ROB1" 10 | 11 | -File "HOME:/motion_program_exec_egm.mod" -ModName "motion_program_exec_egm"\ 12 | -Task "T_ROB1" 13 | 14 | -File "HOME:/motion_program_logger.mod" -ModName "motion_program_logger"\ 15 | -Task "logger" 16 | 17 | -File "HOME:/error_reporter.mod" -ModName "error_reporter"\ 18 | -Task "error_reporter" 19 | # 20 | CAB_TASKS: 21 | 22 | -Name "T_ROB1" -Type "NORMAL" -BindRef 0 -Entry "motion_program_main"\ 23 | -MotionTask 24 | 25 | -Name "logger" -Entry "motion_program_logger_main" -TrustLevel "None"\ 26 | -RmqType "Internal" 27 | 28 | -Name "error_reporter" -TrustLevel "None" 29 | -------------------------------------------------------------------------------- /robot/config_params_multimove/EIO.cfg: -------------------------------------------------------------------------------- 1 | EIO:CFG_1.0:6:1:: 2 | # 3 | SYSSIG_OUT: 4 | 5 | -Status "TaskExecuting" -Signal "motion_program_executing" -Arg2 "T_ROB1" 6 | 7 | -Status "Error" -Signal "motion_program_error" -Arg2 "T_ROB1" 8 | # 9 | EIO_SIGNAL: 10 | 11 | -Name "motion_program_executing" -SignalType "DO" 12 | 13 | -Name "motion_program_preempt" -SignalType "AO" -Access "All" 14 | 15 | -Name "motion_program_preempt_cmd_num" -SignalType "AO" -Access "All" 16 | 17 | -Name "motion_program_preempt_current" -SignalType "AO" 18 | 19 | -Name "motion_program_current_cmd_num" -SignalType "AO" 20 | 21 | -Name "motion_program_queued_cmd_num" -SignalType "AO" 22 | 23 | -Name "motion_program_seqno" -SignalType "AO" -Access "All" 24 | 25 | -Name "motion_program_log_motion" -SignalType "DO" -Access "All"\ 26 | -Default 1 27 | 28 | -Name "motion_program_error" -SignalType "DO" 29 | 30 | -Name "motion_program_seqno_command" -SignalType "AO" -Access "All" 31 | 32 | -Name "motion_program_seqno_complete" -SignalType "AO" -Access "All" 33 | 34 | -Name "motion_program_driver_abort" -SignalType "DO" -Access "All" 35 | 36 | -Name "motion_program_seqno_started" -SignalType "AO" -Access "All" 37 | -------------------------------------------------------------------------------- /robot/config_params_multimove/SYS.cfg: -------------------------------------------------------------------------------- 1 | SYS:CFG_1.0:6:0:: 2 | 3 | # 4 | CAB_TASK_MODULES: 5 | 6 | -File "HOME:/motion_program_shared.sys" -Shared 7 | 8 | -File "HOME:/motion_program_exec.mod" -ModName "motion_program_exec"\ 9 | -Task "T_ROB1" 10 | 11 | -File "HOME:/motion_program_exec.mod" -ModName "motion_program_exec"\ 12 | -Task "T_ROB2" 13 | 14 | -File "HOME:/motion_program_logger.mod" -ModName "motion_program_logger"\ 15 | -Task "logger" 16 | 17 | -File "HOME:/error_reporter.mod" -ModName "error_reporter"\ 18 | -Task "error_reporter" 19 | # 20 | CAB_TASKS: 21 | 22 | -Name "T_ROB1" -Type "NORMAL" -BindRef 0 -Entry "motion_program_main"\ 23 | -UseMechanicalUnitGroup "rob1" -MotionTask 24 | 25 | -Name "T_ROB2" -Type "NORMAL" -BindRef 0 -Entry "motion_program_main"\ 26 | -UseMechanicalUnitGroup "rob2" -MotionTask 27 | 28 | -Name "logger" -Entry "motion_program_logger_main" -TrustLevel "None"\ 29 | -RmqType "Internal" 30 | 31 | -Name "error_reporter" -TrustLevel "None" 32 | -------------------------------------------------------------------------------- /src/abb_motion_program_exec/__init__.py: -------------------------------------------------------------------------------- 1 | from .abb_motion_program_exec_client import * -------------------------------------------------------------------------------- /src/abb_motion_program_exec/commands/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/src/abb_motion_program_exec/commands/__init__.py -------------------------------------------------------------------------------- /src/abb_motion_program_exec/commands/command_base.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Wason Technology LLC, Rensselaer Polytechnic Institute 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import inspect 16 | 17 | class command_append_method: 18 | def __init__(self, command_cls): 19 | self._command_cls = command_cls 20 | self.__doc__ = command_cls._append_method_doc 21 | 22 | def __get__(self, obj, cls=None): 23 | if obj is None: 24 | raise Exception("command_append_method must be called on an instance") 25 | 26 | def command_append_func(*args, **kwargs): 27 | cmd = self._command_cls(*args, **kwargs) 28 | obj._append_command(cmd) 29 | return cmd 30 | 31 | ret = command_append_func 32 | ret.__doc__ = self._command_cls._append_method_doc 33 | sig = inspect.signature(self._command_cls.__init__) 34 | sig = sig.replace(parameters=tuple(sig.parameters.values())[1:]) 35 | ret.__signature__ = sig 36 | return ret 37 | 38 | class CommandBase: 39 | pass -------------------------------------------------------------------------------- /src/abb_motion_program_exec/commands/commands.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Wason Technology LLC, Rensselaer Polytechnic Institute 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from .command_base import CommandBase 16 | from dataclasses import dataclass 17 | from .rapid_types import * 18 | import io 19 | from . import util 20 | 21 | @dataclass 22 | class MoveAbsJCommand(CommandBase): 23 | command_opcode = 1 24 | 25 | to_joint_pos: jointtarget 26 | speed: speeddata 27 | zone: zonedata 28 | 29 | def write_params(self, f: io.IOBase): 30 | to_joint_pos_b = util.jointtarget_to_bin(self.to_joint_pos) 31 | speed_b = util.speeddata_to_bin(self.speed) 32 | zone_b = util.zonedata_to_bin(self.zone) 33 | f.write(to_joint_pos_b) 34 | f.write(speed_b) 35 | f.write(zone_b) 36 | 37 | def to_rapid(self, sync_move = False, cmd_num = 0, **kwargs): 38 | to_joint_pos_str = self.to_joint_pos.to_rapid() 39 | speed_str = self.speed.to_rapid() 40 | zone_str = self.zone.to_rapid() 41 | sync_id = "" if not sync_move else f"\\ID:={cmd_num}," 42 | return f"MoveAbsJ {to_joint_pos_str}, {sync_id}{speed_str}, {zone_str}, motion_program_tool\Wobj:=motion_program_wobj;" 43 | 44 | _append_method_doc = "" 45 | 46 | @dataclass 47 | class MoveJCommand(CommandBase): 48 | command_opcode = 2 49 | 50 | to_point: robtarget 51 | speed: speeddata 52 | zone: zonedata 53 | 54 | def write_params(self, f: io.IOBase): 55 | to_point_b = util.robtarget_to_bin(self.to_point) 56 | speed_b = util.speeddata_to_bin(self.speed) 57 | zone_b = util.zonedata_to_bin(self.zone) 58 | f.write(to_point_b) 59 | f.write(speed_b) 60 | f.write(zone_b) 61 | 62 | def to_rapid(self, sync_move = False, cmd_num = 0, **kwargs): 63 | to_point_str = self.to_point.to_rapid() 64 | speed_str = self.speed.to_rapid() 65 | zone_str = self.zone.to_rapid() 66 | 67 | sync_id = "" if not sync_move else f"\\ID:={cmd_num}," 68 | return f"MoveJ {to_point_str}, {sync_id}{speed_str}, {zone_str}, motion_program_tool\Wobj:=motion_program_wobj;" 69 | 70 | _append_method_doc = "" 71 | 72 | @dataclass 73 | class MoveLCommand(CommandBase): 74 | command_opcode = 3 75 | 76 | to_point: robtarget 77 | speed: speeddata 78 | zone: zonedata 79 | 80 | def write_params(self, f: io.IOBase): 81 | to_point_b = util.robtarget_to_bin(self.to_point) 82 | speed_b = util.speeddata_to_bin(self.speed) 83 | zone_b = util.zonedata_to_bin(self.zone) 84 | 85 | f.write(to_point_b) 86 | f.write(speed_b) 87 | f.write(zone_b) 88 | 89 | def to_rapid(self, sync_move = False, cmd_num = 0, **kwargs): 90 | to_point_str = self.to_point.to_rapid() 91 | speed_str = self.speed.to_rapid() 92 | zone_str = self.zone.to_rapid() 93 | 94 | sync_id = "" if not sync_move else f"\\ID:={cmd_num}," 95 | return f"MoveL {to_point_str}, {sync_id}{speed_str}, {zone_str}, motion_program_tool\Wobj:=motion_program_wobj;" 96 | 97 | _append_method_doc = "" 98 | 99 | @dataclass 100 | class MoveCCommand(CommandBase): 101 | command_opcode = 4 102 | 103 | cir_point: robtarget 104 | to_point: robtarget 105 | speed: speeddata 106 | zone: zonedata 107 | 108 | def write_params(self, f: io.IOBase): 109 | cir_point_b = util.robtarget_to_bin(self.cir_point) 110 | to_point_b = util.robtarget_to_bin(self.to_point) 111 | speed_b = util.speeddata_to_bin(self.speed) 112 | zone_b = util.zonedata_to_bin(self.zone) 113 | 114 | f.write(cir_point_b) 115 | f.write(to_point_b) 116 | f.write(speed_b) 117 | f.write(zone_b) 118 | 119 | def to_rapid(self, sync_move = False, cmd_num = 0, **kwargs): 120 | cir_point_str = self.cir_point.to_rapid() 121 | to_point_str = self.to_point.to_rapid() 122 | speed_str = self.speed.to_rapid() 123 | zone_str = self.zone.to_rapid() 124 | sync_id = "" if not sync_move else f"\\ID:={cmd_num}," 125 | return f"MoveC {cir_point_str}, {sync_id}{to_point_str}, {speed_str}, {zone_str}, motion_program_tool\Wobj:=motion_program_wobj;" 126 | 127 | _append_method_doc = "" 128 | 129 | @dataclass 130 | class WaitTimeCommand(CommandBase): 131 | command_opcode=5 132 | 133 | t: float 134 | 135 | def write_params(self, f: io.IOBase): 136 | f.write(util.num_to_bin(self.t)) 137 | 138 | def to_rapid(self, **kwargs): 139 | return f"WaitTime {self.t};" 140 | 141 | _append_method_doc = "" 142 | 143 | @dataclass 144 | class CirPathModeCommand(CommandBase): 145 | command_opcode = 6 146 | 147 | switch: CirPathModeSwitch 148 | 149 | def write_params(self, f: io.IOBase): 150 | val = self.switch.value 151 | if not (val >=1 and val <= 6): 152 | raise Exception("Invalid CirPathMode switch") 153 | f.write(util.num_to_bin(val)) 154 | 155 | def to_rapid(self, **kwargs): 156 | if self.switch == 1: 157 | return r"CirPathMode\PathFrame;" 158 | if self.switch == 2: 159 | return r"CirPathMode\ObjectFrame;" 160 | if self.switch == 3: 161 | return r"CirPathMode\CirPointOri;" 162 | if self.switch == 4: 163 | return r"CirPathMode\Wrist45;" 164 | if self.switch == 5: 165 | return r"CirPathMode\Wrist46;" 166 | if self.switch == 6: 167 | return r"CirPathMode\Wrist56;" 168 | raise Exception("Invalid CirPathMode switch") 169 | 170 | _append_method_doc = "" 171 | 172 | @dataclass 173 | class SyncMoveOnCommand(CommandBase): 174 | command_opcode = 7 175 | 176 | def write_params(self, f: io.IOBase): 177 | pass 178 | 179 | def to_rapid(self, **kwargs): 180 | return "SyncMoveOn motion_program_sync1,task_list;" 181 | 182 | _append_method_doc = "" 183 | 184 | class SyncMoveOffCommand(CommandBase): 185 | command_opcode = 8 186 | 187 | def write_params(self, f: io.IOBase): 188 | pass 189 | 190 | def to_rapid(self, **kwargs): 191 | return "SyncMoveOff motion_program_sync2;" 192 | 193 | _append_method_doc = "" 194 | -------------------------------------------------------------------------------- /src/abb_motion_program_exec/commands/egm_commands.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Wason Technology LLC, Rensselaer Polytechnic Institute 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from .command_base import CommandBase 16 | from dataclasses import dataclass 17 | from .rapid_types import * 18 | import io 19 | from . import util 20 | from typing import Union 21 | 22 | @dataclass 23 | class EGMRunJointCommand(CommandBase): 24 | command_opcode = 50001 25 | 26 | cond_time: float 27 | ramp_in_time: float 28 | ramp_out_time: float 29 | 30 | def write_params(self, f: io.IOBase): 31 | f.write(util.num_to_bin(self.cond_time)) 32 | f.write(util.num_to_bin(self.ramp_in_time)) 33 | f.write(util.num_to_bin(self.ramp_out_time)) 34 | 35 | def to_rapid(self, **kwargs): 36 | raise NotImplementedError("EGM not supported for RAPID generation") 37 | 38 | _append_method_doc = "" 39 | 40 | @dataclass 41 | class EGMRunPoseCommand(CommandBase): 42 | command_opcode = 50002 43 | 44 | cond_time: float 45 | ramp_in_time: float 46 | ramp_out_time: float 47 | offset: pose 48 | 49 | def write_params(self, f: io.IOBase): 50 | f.write(util.num_to_bin(self.cond_time)) 51 | f.write(util.num_to_bin(self.ramp_in_time)) 52 | f.write(util.num_to_bin(self.ramp_out_time)) 53 | f.write(util.pose_to_bin(self.offset)) 54 | 55 | def to_rapid(self, **kwargs): 56 | raise NotImplementedError("EGM not supported for RAPID generation") 57 | 58 | _append_method_doc = "" 59 | 60 | @dataclass 61 | class EGMMoveLCommand(CommandBase): 62 | command_opcode = 50003 63 | 64 | to_point: robtarget 65 | speed: speeddata 66 | zone: zonedata 67 | 68 | def write_params(self, f: io.IOBase): 69 | to_point_b = util.robtarget_to_bin(self.to_point) 70 | speed_b = util.speeddata_to_bin(self.speed) 71 | zone_b = util.zonedata_to_bin(self.zone) 72 | 73 | f.write(to_point_b) 74 | f.write(speed_b) 75 | f.write(zone_b) 76 | 77 | def to_rapid(self, **kwargs): 78 | raise NotImplementedError("EGM not supported for RAPID generation") 79 | 80 | _append_method_doc = "" 81 | 82 | @dataclass 83 | class EGMMoveCCommand(CommandBase): 84 | command_opcode = 50004 85 | 86 | cir_point: robtarget 87 | to_point: robtarget 88 | speed: speeddata 89 | zone: zonedata 90 | 91 | def write_params(self, f: io.IOBase): 92 | cir_point_b = util.robtarget_to_bin(self.cir_point) 93 | to_point_b = util.robtarget_to_bin(self.to_point) 94 | speed_b = util.speeddata_to_bin(self.speed) 95 | zone_b = util.zonedata_to_bin(self.zone) 96 | 97 | f.write(cir_point_b) 98 | f.write(to_point_b) 99 | f.write(speed_b) 100 | f.write(zone_b) 101 | 102 | def to_rapid(self, **kwargs): 103 | raise NotImplementedError("EGM not supported for RAPID generation") 104 | 105 | _append_method_doc = "" 106 | 107 | 108 | class egm_minmax(NamedTuple): 109 | """egm_minmax structure""" 110 | min: float 111 | """min value""" 112 | max: float 113 | """max value""" 114 | 115 | def _egm_minmax_to_bin(e: egm_minmax): 116 | return util.num_to_bin(e.min) + util.num_to_bin(e.max) 117 | 118 | class EGMStreamConfig(NamedTuple): 119 | """ 120 | Configure EGM to stream feedback data only 121 | """ 122 | pass 123 | 124 | class EGMJointTargetConfig(NamedTuple): 125 | """ 126 | Activate EGM for joint target control 127 | """ 128 | J1: egm_minmax 129 | """J1 convergence criteria""" 130 | J2: egm_minmax 131 | """J2 convergence criteria""" 132 | J3: egm_minmax 133 | """J3 convergence criteria""" 134 | J4: egm_minmax 135 | """J4 convergence criteria""" 136 | J5: egm_minmax 137 | """J5 convergence criteria""" 138 | J6: egm_minmax 139 | """J6 convergence criteria""" 140 | max_pos_deviation: float 141 | """Max joint deviation in degrees""" 142 | max_speed_deviation: float 143 | """Max joint speed deviation in degrees/second""" 144 | 145 | class egmframetype(IntEnum): 146 | """Frame types for corrections and sensor measurements""" 147 | EGM_FRAME_BASE = 0 148 | """Base frame""" 149 | EGM_FRAME_TOOL = 1 150 | """Tool frame""" 151 | EGM_FRAME_WOBJ = 2 152 | """Wobj frame""" 153 | EGM_FRAME_WORLD = 3 154 | """World frame""" 155 | EGM_FRAME_JOINT = 4 156 | """Joint frame""" 157 | 158 | class EGMPoseTargetConfig(NamedTuple): 159 | """ 160 | Activate EGM for pose target control 161 | """ 162 | corr_frame: pose 163 | """The correction frame""" 164 | corr_fr_type: egmframetype 165 | """The correction frame type""" 166 | sensor_frame: pose 167 | """The sensor frame""" 168 | sensor_fr_type: egmframetype 169 | """The sensor frame type""" 170 | x: egm_minmax 171 | """x convergence criteria""" 172 | y: egm_minmax 173 | """y convergence criteria""" 174 | z: egm_minmax 175 | """z convergence criteria""" 176 | rx: egm_minmax 177 | """rx convergence criteria""" 178 | ry: egm_minmax 179 | """ry convergence criteria""" 180 | rz: egm_minmax 181 | """rz convergence criteria""" 182 | max_pos_deviation: float 183 | """Max joint deviation in degrees""" 184 | max_speed_deviation: float 185 | """Max joint speed deviation in degrees/second""" 186 | 187 | class EGMPathCorrectionConfig(NamedTuple): 188 | """ 189 | Activate EGM for path correction (``EGMMoveL``, ``EGMMoveC``) 190 | """ 191 | sensor_frame: pose 192 | """The sensor frame""" 193 | 194 | 195 | def _egm_joint_target_config_to_bin(c: EGMJointTargetConfig): 196 | return _egm_minmax_to_bin(c.J1) \ 197 | + _egm_minmax_to_bin(c.J2) \ 198 | + _egm_minmax_to_bin(c.J3) \ 199 | + _egm_minmax_to_bin(c.J4) \ 200 | + _egm_minmax_to_bin(c.J5) \ 201 | + _egm_minmax_to_bin(c.J6) \ 202 | + util.num_to_bin(c.max_pos_deviation) \ 203 | + util.num_to_bin(c.max_speed_deviation) 204 | 205 | def _egm_pose_target_config_to_bin(c: EGMPoseTargetConfig): 206 | return \ 207 | util.pose_to_bin(c.corr_frame) \ 208 | + util.num_to_bin(c.corr_fr_type.value) \ 209 | + util.pose_to_bin(c.sensor_frame) \ 210 | + util.num_to_bin(c.sensor_fr_type.value) \ 211 | + _egm_minmax_to_bin(c.x) \ 212 | + _egm_minmax_to_bin(c.y) \ 213 | + _egm_minmax_to_bin(c.z) \ 214 | + _egm_minmax_to_bin(c.rx) \ 215 | + _egm_minmax_to_bin(c.ry) \ 216 | + _egm_minmax_to_bin(c.rz) \ 217 | + util.num_to_bin(c.max_pos_deviation) \ 218 | + util.num_to_bin(c.max_speed_deviation) 219 | 220 | def _egm_path_correction_config_to_bin(c: EGMPathCorrectionConfig): 221 | return util.pose_to_bin(c.sensor_frame) 222 | 223 | def write_egm_config(f: io.IOBase, 224 | egm_config: Union[EGMStreamConfig,EGMJointTargetConfig,EGMPoseTargetConfig,EGMPathCorrectionConfig]): 225 | if egm_config is None or isinstance(egm_config,EGMStreamConfig): 226 | f.write(util.num_to_bin(0)) 227 | elif isinstance(egm_config,EGMJointTargetConfig): 228 | f.write(util.num_to_bin(1)) 229 | f.write(_egm_joint_target_config_to_bin(egm_config)) 230 | elif isinstance(egm_config,EGMPoseTargetConfig): 231 | f.write(util.num_to_bin(2)) 232 | f.write(_egm_pose_target_config_to_bin(egm_config)) 233 | elif isinstance(egm_config,EGMPathCorrectionConfig): 234 | f.write(util.num_to_bin(3)) 235 | f.write(_egm_path_correction_config_to_bin(egm_config)) 236 | else: 237 | raise Exception("Invalid EGM configuration") -------------------------------------------------------------------------------- /src/abb_motion_program_exec/commands/rapid_types.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Wason Technology LLC, Rensselaer Polytechnic Institute 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from typing import Callable, NamedTuple, Any, List 16 | from enum import IntEnum 17 | import numpy as np 18 | from . import util 19 | 20 | """ 21 | This module contains types found on the ABB Robot Controller. Documentation is taken from the 22 | ABB Robotics manual "Technical reference manual RAPID Instructions, Functions and Data types". 23 | """ 24 | 25 | 26 | class speeddata(NamedTuple): 27 | """ 28 | ABB RAPID ``speeddata`` structure. 29 | 30 | The ``abb_motion_program_exec`` module contains constants for ``v5`` through ``v5000`` and ``vmax`` as found 31 | in RAPID. 32 | """ 33 | v_tcp: float 34 | """Velocity of the tool center point (TCP) in mm/s""" 35 | v_ori: float 36 | """The reorientation velocity of the TCP expressed in degrees/s.""" 37 | v_leax: float 38 | """The velocity of linear external axes in mm/s.""" 39 | v_reax: float 40 | """The velocity of rotating external axes in degrees/s.""" 41 | 42 | def to_rapid(self): 43 | return util.nums_to_rapid_array([ 44 | self.v_tcp, 45 | self.v_ori, 46 | self.v_leax, 47 | self.v_reax 48 | ]) 49 | 50 | v5 = speeddata(5,500,5000,1000) 51 | """5 mm/s speeddata""" 52 | v10 = speeddata(10,500,5000,1000) 53 | """10 mm/s speeddata""" 54 | v20 = speeddata(20,500,5000,1000) 55 | """20 mm/s speeddata""" 56 | v30 = speeddata(30,500,5000,1000) 57 | """30 mm/s speeddata""" 58 | v40 = speeddata(40,500,5000,1000) 59 | """40 mm/s speeddata""" 60 | v50 = speeddata(50,500,5000,1000) 61 | """50 mm/s speeddata""" 62 | v60 = speeddata(60,500,5000,1000) 63 | """60 mm/s speeddata""" 64 | v80 = speeddata(80,500,5000,1000) 65 | """80 mm/s speeddata""" 66 | v100 = speeddata(100,500,5000,1000) 67 | """100 mm/s speeddata""" 68 | v150 = speeddata(150,500,5000,1000) 69 | """150 mm/s speeddata""" 70 | v200 = speeddata(200,500,5000,1000) 71 | """200 mm/s speeddata""" 72 | v300 = speeddata(300,500,5000,1000) 73 | """300 mm/s speeddata""" 74 | v400 = speeddata(400,500,5000,1000) 75 | """400 mm/s speeddata""" 76 | v500 = speeddata(500,500,5000,1000) 77 | """500 mm/s speeddata""" 78 | v600 = speeddata(600,500,5000,1000) 79 | """600 mm/s speeddata""" 80 | v800 = speeddata(800,500,5000,1000) 81 | """800 mm/s speeddata""" 82 | v1000 = speeddata(1000,500,5000,1000) 83 | """1000 mm/s speeddata""" 84 | v1500 = speeddata(1500,500,5000,1000) 85 | """1500 mm/s speeddata""" 86 | v2000 = speeddata(2000,500,5000,1000) 87 | """2000 mm/s speeddata""" 88 | v2500 = speeddata(2500,500,5000,1000) 89 | """2500 mm/s speeddata""" 90 | v3000 = speeddata(3000,500,5000,1000) 91 | """3000 mm/s speeddata""" 92 | v4000 = speeddata(4000,500,5000,1000) 93 | """4000 mm/s speeddata""" 94 | v5000 = speeddata(5000,500,5000,1000) 95 | """5000 mm/s speeddata""" 96 | v6000 = speeddata(6000,500,5000,1000) 97 | """6000 mm/s speeddata""" 98 | v7000 = speeddata(7000,500,5000,1000) 99 | """7000 mm/s speeddata""" 100 | vmax = speeddata(10000,500,5000,1000) 101 | """max speeddata""" 102 | 103 | class zonedata(NamedTuple): 104 | """ 105 | ABB RAPID ``zonedata`` structure. 106 | 107 | The ``abb_motion_program_exec`` module contains constants for ``fine`` and ``z0`` through ``z200`` as found 108 | in RAPID. 109 | """ 110 | finep: bool 111 | """Defines whether the movement is to terminate as a stop point (fine point) or as a fly-by point.""" 112 | pzone_tcp: float 113 | """The size (the radius) of the TCP zone in mm.""" 114 | pzone_ori: float 115 | """The zone size (the radius) for the tool reorientation.""" 116 | pzone_eax: float 117 | """The zone size (the radius) for external axes.""" 118 | zone_ori: float 119 | """The zone size for the tool reorientation in degrees.""" 120 | zone_leax: float 121 | """The zone size for linear external axes in mm.""" 122 | zone_reax: float 123 | """The zone size for rotating external axes in degrees.""" 124 | 125 | def to_rapid(self): 126 | return util.nums_to_rapid_array([ 127 | util.bool_to_rapid(self.finep), 128 | self.pzone_tcp, 129 | self.pzone_ori, 130 | self.pzone_eax, 131 | self.zone_ori, 132 | self.zone_leax, 133 | self.zone_reax 134 | ]) 135 | 136 | fine = zonedata(True,0,0,0,0,0,0) 137 | """Stop point, no bleding""" 138 | z0 = zonedata(False,0.3,0.3,0.3,0.03,0.3,0.03) 139 | """0.3 mm zone""" 140 | z1 = zonedata(False,1,1,1,0.1,1,0.1) 141 | """1 mm zone""" 142 | z5 = zonedata(False,5,8,8,0.8,8,0.8) 143 | """5 mm zone""" 144 | z10 = zonedata(False,10,15,15,1.5,15,1.5) 145 | """10 mm zone""" 146 | z15 = zonedata(False,15,23,23,2.3,23,2.3) 147 | """15 mm zone""" 148 | z20 = zonedata(False,20,30,30,3.0,30,3.0) 149 | """20 mm zone""" 150 | z30 = zonedata(False,30,45,45,4.5,45,4.5) 151 | """30 mm zone""" 152 | z40 = zonedata(False,40,60,60,6.0,60,6.0) 153 | """40 mm zone""" 154 | z50 = zonedata(False,50,75,75,7.5,75,7.5) 155 | """50 mm zone""" 156 | z60 = zonedata(False,60,90,90,9.0,90,9.0) 157 | """60 mm zone""" 158 | z80 = zonedata(False,80,120,120,12,120,12) 159 | """80 mm zone""" 160 | z100 = zonedata(False,100,150,150,15,150,15) 161 | """100 mm zone""" 162 | z150 = zonedata(False,150,225,225,23,225,23) 163 | """150 mm zone""" 164 | z200 = zonedata(False,200,300,300,30,300,30) 165 | """200 mm zone""" 166 | 167 | class jointtarget(NamedTuple): 168 | """ABB RAPID ``jointtarget`` structure""" 169 | robax: np.ndarray # shape=(6,) 170 | """Axis positions of the robot axes in degrees. Must have 6 elements""" 171 | extax: np.ndarray # shape=(6,) 172 | """The position of external axes. Must have 6 elements""" 173 | 174 | def to_rapid(self): 175 | return f"[{util.nums_to_rapid_array(self.robax)},{util.nums_to_rapid_array(self.extax)}]" 176 | 177 | class pose(NamedTuple): 178 | """ABB RAPID ``pose`` structure""" 179 | trans: np.ndarray # [x,y,z] 180 | """Translation in mm. Must have 3 elements""" 181 | rot: np.ndarray # [qw,qx,qy,qz] 182 | """Rotation in quaternions. [w,x,y,z] format""" 183 | 184 | def to_rapid(self): 185 | return f"[{util.nums_to_rapid_array(self.trans)},{util.nums_to_rapid_array(self.rot)}]" 186 | 187 | class confdata(NamedTuple): 188 | """ABB RAPID ``confdata`` structure. This structure has a very peculiar meaning. See the reference manual 189 | for details.""" 190 | cf1: float 191 | """cf1""" 192 | cf4: float 193 | """cf4""" 194 | cf6: float 195 | """cf6""" 196 | cfx: float 197 | """cfx""" 198 | 199 | def to_rapid(self): 200 | return util.nums_to_rapid_array([ 201 | self.cf1, 202 | self.cf4, 203 | self.cf6, 204 | self.cfx 205 | ]) 206 | 207 | class robtarget(NamedTuple): 208 | """ABB RAPID ``robtarget`` structure""" 209 | trans: np.ndarray # [x,y,z] 210 | """Translation [x,y,z] in mm""" 211 | rot: np.ndarray # [qw,qx,qy,qz] 212 | """Rotation in quaternions. [w,x,y,z] format""" 213 | robconf: confdata # 214 | """Robot configuration""" 215 | extax: np.ndarray # shape=(6,) 216 | """External axes positions. Must have 6 elements""" 217 | 218 | def to_rapid(self): 219 | trans_str = util.nums_to_rapid_array(self.trans) 220 | rot_str = util.nums_to_rapid_array(self.rot) 221 | robconf_str = self.robconf.to_rapid() 222 | extax_str = util.nums_to_rapid_array(self.extax) 223 | return f"[{trans_str},{rot_str},{robconf_str},{extax_str}]" 224 | 225 | class loaddata(NamedTuple): 226 | """ABB RAPID ``loaddata`` structure""" 227 | mass: float 228 | """Mass in kg""" 229 | cog: np.ndarray # shape=(3,) 230 | """Center of gravity [x,y,z] in mm""" 231 | aom: np.ndarray # shape=(4,) 232 | """Axes of moment (principal axis) transform [w,x,y,z]""" 233 | ix: float 234 | """x-axis moment in kgm^2""" 235 | iy: float 236 | """y-axis moment in kgm^2""" 237 | iz: float 238 | """z-axis moment in kgm^2""" 239 | 240 | def to_rapid(self): 241 | return f"[{self.mass},{util.nums_to_rapid_array(self.cog)}," \ 242 | f"{util.nums_to_rapid_array(self.aom)},{self.ix},{self.iy},{self.iz}]" 243 | 244 | class CirPathModeSwitch(IntEnum): 245 | """ABB RAPID Options for CirPathMode command""" 246 | PathFrame = 1 247 | """Path Frame""" 248 | ObjectFrame = 2 249 | """Object Frame""" 250 | CirPointOri = 3 251 | """Pass through ToPoint during MoveC""" 252 | Wrist45 = 4 253 | """Wrist45""" 254 | Wrist46 = 5 255 | """Wrist46""" 256 | Wrist56 = 6 257 | """Wrist56""" 258 | 259 | class tooldata(NamedTuple): 260 | """ABB RAPID ``tooldata`` structure""" 261 | robhold: bool 262 | """Defines whether or not the robot is holding the tool""" 263 | tframe: pose 264 | """The tool coordinate system""" 265 | tload : loaddata 266 | """The load of the tool""" 267 | 268 | def to_rapid(self): 269 | return f"[{util.bool_to_rapid(self.robhold)},{self.tframe.to_rapid()},{self.tload.to_rapid()}]" 270 | 271 | class wobjdata(NamedTuple): 272 | """ABB RAPID ``wobjdata`` structure""" 273 | robhold: bool 274 | """True if the robot is holding the work object. Typically False""" 275 | ufprog: bool 276 | """Defines whether or not a fixed user coordinate system is used""" 277 | ufmec: str 278 | """The mechanical unit which the robot movements are coordinated""" 279 | uframe: pose 280 | """User coordinate system""" 281 | oframe: pose 282 | """Object coordinate system""" 283 | 284 | def to_rapid(self): 285 | return f"[{util.bool_to_rapid(self.robhold)},{util.bool_to_rapid(self.ufprog)}," \ 286 | f"\"{self.ufmec}\",{self.uframe.to_rapid()},{self.oframe.to_rapid()}]" -------------------------------------------------------------------------------- /src/abb_motion_program_exec/commands/util.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Wason Technology LLC, Rensselaer Polytechnic Institute 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import struct 16 | import numpy as np 17 | import io 18 | from typing import TYPE_CHECKING, List 19 | 20 | if TYPE_CHECKING: 21 | from .rapid_types import * 22 | 23 | _speeddata_struct_fmt = struct.Struct("<4f") 24 | def speeddata_to_bin(z: "speeddata"): 25 | return _speeddata_struct_fmt.pack( 26 | z.v_tcp, z.v_ori, z.v_leax, z.v_reax 27 | ) 28 | 29 | _zonedata_struct_fmt = struct.Struct("<7f") 30 | def zonedata_to_bin(z: "zonedata"): 31 | return _zonedata_struct_fmt.pack( 32 | 0.0 if not z.finep else 1.0, 33 | z.pzone_tcp, z.pzone_ori, z.pzone_eax, z.zone_ori, z.zone_leax, z.zone_reax 34 | ) 35 | 36 | def fix_array(arr, l): 37 | if isinstance(arr,list): 38 | if not len(arr) == l: 39 | raise Exception(f"Invalid array, expected array length {l}") 40 | return np.array(arr,dtype=np.float64) 41 | if arr.shape == (l,): 42 | return arr 43 | if arr.shape == (l,1) or arr.shape == (1,l): 44 | return arr.flatten() 45 | raise Exception(f"Invalid array, expected array length {l}") 46 | 47 | 48 | _jointtarget_struct_fmt = struct.Struct("<12f") 49 | def jointtarget_to_bin(j: "jointtarget"): 50 | r = fix_array(j.robax,6).tolist() 51 | e = fix_array(j.extax,6).tolist() 52 | return _jointtarget_struct_fmt.pack(*r, *e) 53 | 54 | _pose_struct_fmt = struct.Struct("<7f") 55 | def pose_to_bin(p: "pose"): 56 | p1 = fix_array(p.trans,3).tolist() 57 | q = fix_array(p.rot,4).tolist() 58 | return _pose_struct_fmt.pack(*p1,*q) 59 | 60 | _confdata_struct_fmt = struct.Struct("<4f") 61 | def confdata_to_bin(c: "confdata"): 62 | return _confdata_struct_fmt.pack(c.cf1, c.cf4, c.cf6, c.cfx) 63 | 64 | _robtarget_extax_struct_fmt = struct.Struct("<6f") 65 | def robtarget_to_bin(r: "robtarget"): 66 | p1 = fix_array(r.trans,3).tolist() 67 | q = fix_array(r.rot,4).tolist() 68 | pose_bin = _pose_struct_fmt.pack(*p1,*q) 69 | robconf_bin = confdata_to_bin(r.robconf) 70 | extax = fix_array(r.extax,6).tolist() 71 | extax_bin = _robtarget_extax_struct_fmt.pack(*extax) 72 | return pose_bin + robconf_bin + extax_bin 73 | 74 | _num_struct_fmt = struct.Struct(" 0: 88 | s_bin += b" " * pad_len 89 | return num_to_bin(len(s)) + s_bin 90 | 91 | _loaddata_struct_fmt = struct.Struct("<11f") 92 | def loaddata_to_bin(l: "loaddata"): 93 | cog = fix_array(l.cog,3) 94 | aom = fix_array(l.aom,4) 95 | return _loaddata_struct_fmt.pack( 96 | l.mass, *cog, *aom, l.ix, l.iy, l.iz 97 | ) 98 | 99 | def tooldata_to_bin(td: "tooldata"): 100 | robhold_bin = num_to_bin(0.0 if not td.robhold else 1.0) 101 | tframe_bin = pose_to_bin(td.tframe) 102 | tload_bin = loaddata_to_bin(td.tload) 103 | return robhold_bin + tframe_bin + tload_bin 104 | 105 | def wobjdata_to_bin(wd: "wobjdata"): 106 | robhold_bin = num_to_bin(0.0 if not wd.robhold else 1.0) 107 | ufprog_bin = num_to_bin(0.0 if not wd.ufprog else 1.0) 108 | ufmec_bin = str_to_bin(wd.ufmec) 109 | uframe_bin = pose_to_bin(wd.uframe) 110 | oframe_bin = pose_to_bin(wd.oframe) 111 | return robhold_bin + ufprog_bin + ufmec_bin + uframe_bin + oframe_bin 112 | 113 | def read_num(f: io.IOBase): 114 | b = f.read(4) 115 | return _num_struct_fmt.unpack(b)[0] 116 | 117 | def read_nums(f: io.IOBase, n: int): 118 | return [read_num(f) for _ in range(n)] 119 | 120 | def read_str(f: io.IOBase): 121 | l = int(read_num(f)) 122 | s_ascii = f.read(l) 123 | return s_ascii.decode('ascii') 124 | 125 | def nums_to_rapid_array(nums: List[float]): 126 | return "[" + ", ".join([str(n) for n in nums]) + "]" 127 | 128 | def bool_to_rapid(b: bool): 129 | return "TRUE" if b else "FALSE" -------------------------------------------------------------------------------- /src/abb_motion_program_exec/robotraconteur/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpiRobotics/abb_motion_program_exec/3891d1b78ae0b3052be055e12c793dcb10a7cb06/src/abb_motion_program_exec/robotraconteur/__init__.py -------------------------------------------------------------------------------- /src/abb_motion_program_exec/robotraconteur/__main__.py: -------------------------------------------------------------------------------- 1 | from .abb_motion_program_exec_robotraconteur import main 2 | 3 | if __name__ == '__main__': 4 | main() -------------------------------------------------------------------------------- /src/abb_motion_program_exec/robotraconteur/abb_motion_program_exec_robotraconteur.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import numpy as np 3 | import argparse 4 | import threading 5 | from .. import abb_motion_program_exec_client as abb_client 6 | import RobotRaconteur as RR 7 | RRN=RR.RobotRaconteurNode.s 8 | import RobotRaconteurCompanion as RRC 9 | from RobotRaconteurCompanion.Util.InfoFileLoader import InfoFileLoader 10 | from RobotRaconteurCompanion.Util.AttributesUtil import AttributesUtil 11 | from RobotRaconteurCompanion.Util.TaskGenerator import SyncTaskGenerator 12 | from RobotRaconteurCompanion.Util.RobDef import register_service_types_from_resources 13 | from RobotRaconteurCompanion.Util.RobotUtil import RobotUtil 14 | from ._motion_program_conv import rr_motion_program_to_abb2 15 | 16 | import traceback 17 | import time 18 | import io 19 | import random 20 | import drekar_launch_process 21 | import yaml 22 | 23 | class MotionExecImpl: 24 | def __init__(self, mp_robot_info, base_url, username, password): 25 | 26 | self.mp_robot_info = mp_robot_info 27 | self._abb_client = abb_client.MotionProgramExecClient(base_url, username, password) 28 | 29 | self.device_info = mp_robot_info.robot_info.device_info 30 | self.robot_info = mp_robot_info.robot_info 31 | self.motion_program_robot_info = mp_robot_info 32 | self._recordings = {} 33 | 34 | self.param_changed = RR.EventHook() 35 | 36 | self._robot_util = RobotUtil(RRN) 37 | try: 38 | self._rox_robots = [] 39 | for chain_i in range(len(self.robot_info.chains)): 40 | self._rox_robots.append(self._robot_util.robot_info_to_rox_robot(self.robot_info,chain_i)) 41 | except: 42 | traceback.print_exc() 43 | raise ValueError("invalid robot_info, could not populate GeneralRoboticsToolbox.Robot") 44 | 45 | 46 | def execute_motion_program(self, program, queue): 47 | 48 | if queue: 49 | raise Exception("Motion program queue not supported") 50 | 51 | abb_program, is_multimove, tasks = rr_motion_program_to_abb2(program, self._rox_robots) 52 | 53 | gen = ExecuteMotionProgramGen(self, self._abb_client, abb_program, is_multimove, tasks) 54 | 55 | return gen 56 | 57 | def execute_motion_program_record(self, program, queue): 58 | 59 | if queue: 60 | raise Exception("Motion program queue not supported") 61 | 62 | abb_program, is_multimove, tasks = rr_motion_program_to_abb2(program, self._rox_robots) 63 | 64 | gen = ExecuteMotionProgramGen(self, self._abb_client, abb_program, is_multimove, tasks, save_recording = True) 65 | 66 | return gen 67 | 68 | def read_recording(self, recording_handle): 69 | robot_recording_np = self._recordings.pop(recording_handle) 70 | return RobotRecordingGen(robot_recording_np) 71 | 72 | def clear_recordings(self): 73 | self._recordings.clear() 74 | 75 | def get_param(self, param_name): 76 | raise RR.InvalidArgumentException("Unknown parameter") 77 | 78 | def set_param(self, param_name, value): 79 | raise RR.InvalidArgumentException("Unknown parameter") 80 | 81 | def enable_motion_program_mode(self): 82 | pass 83 | 84 | def disable_motion_program_mode(self): 85 | pass 86 | 87 | 88 | class ExecuteMotionProgramGen(SyncTaskGenerator): 89 | def __init__(self, parent, abb_client, motion_program, is_multimove, tasks, save_recording = False): 90 | super().__init__(RRN, RRN.GetStructureType("experimental.robotics.motion_program.MotionProgramStatus"), 1, -1) 91 | self._parent = parent 92 | self._abb_client = abb_client 93 | self._motion_program = motion_program 94 | self._recording_handle = 0 95 | self._save_recording = save_recording 96 | self._is_multimove = is_multimove 97 | self._tasks = tasks 98 | 99 | def RunTask(self): 100 | print("Start Motion Program!") 101 | if not self._is_multimove: 102 | robot_recording_data = self._abb_client.execute_motion_program(self._motion_program, task=self._tasks) 103 | else: 104 | robot_recording_data = self._abb_client.execute_multimove_motion_program(self._motion_program, tasks=self._tasks) 105 | if self._save_recording: 106 | recording_handle = random.randint(0,0xFFFFFFF) 107 | self._parent._recordings[recording_handle] = robot_recording_data 108 | self._recording_handle = recording_handle 109 | print("Motion Program Complete!") 110 | res = self._status_type() 111 | res.action_status = self._action_const["ActionStatusCode"]["complete"] 112 | res.recording_handle = self._recording_handle 113 | return res 114 | 115 | 116 | class RobotRecordingGen: 117 | def __init__(self, robot_recording_np, ): 118 | self.robot_rec_np = robot_recording_np 119 | self.closed = False 120 | self.aborted = False 121 | self.lock = threading.Lock() 122 | self._mp_recording_part = RRN.GetStructureType("experimental.robotics.motion_program.MotionProgramRecordingPart") 123 | 124 | def Next(self): 125 | with self.lock: 126 | if self.aborted: 127 | raise RR.OperationAbortedException("Recording aborted") 128 | 129 | if self.closed: 130 | raise RR.StopIterationException() 131 | 132 | ret = self._mp_recording_part() 133 | 134 | # All current paths expect to be within 10 MB limit 135 | ret.time = self.robot_rec_np.data[:,0].flatten().astype(np.float64) 136 | ret.command_number = self.robot_rec_np.data[:,1].flatten().astype(np.int32) 137 | # TODO: prismatic joints 138 | ret.joints = np.deg2rad(self.robot_rec_np.data[:,2:].astype(np.float64)) 139 | ret.column_headers = self.robot_rec_np.column_headers 140 | 141 | self.closed = True 142 | return ret 143 | 144 | def Abort(self): 145 | self.aborted = True 146 | 147 | def Close(self): 148 | self.closed = True 149 | 150 | def main(): 151 | 152 | parser = argparse.ArgumentParser(description="ABB Robot motion program driver service for Robot Raconteur") 153 | parser.add_argument("--mp-robot-info-file", type=argparse.FileType('r'),default=None,required=True,help="Motion program robot info file (required)") 154 | parser.add_argument("--mp-robot-base-url", type=str, default='http://127.0.0.1:80', help="robot controller ws base url (default http://127.0.0.1:80)") 155 | parser.add_argument("--mp-robot-username",type=str,default='Default User',help="robot controller username (default 'Default User')") 156 | parser.add_argument("--mp-robot-password",type=str,default='robotics',help="robot controller password (default 'robotics')") 157 | 158 | args, _ = parser.parse_known_args() 159 | 160 | RRC.RegisterStdRobDefServiceTypes(RRN) 161 | register_service_types_from_resources(RRN, __package__, ["experimental.robotics.motion_program", "experimental.abb_robot.motion_program"]) 162 | 163 | with args.mp_robot_info_file: 164 | mp_robot_info_text = args.mp_robot_info_file.read() 165 | mp_robot_info_dict = yaml.safe_load(mp_robot_info_text) 166 | # Workaround to allow loading from normal robot info file 167 | if 'robot_info' not in mp_robot_info_dict: 168 | d = { 169 | "robot_info": mp_robot_info_dict 170 | } 171 | mp_robot_info_dict = d 172 | 173 | info_loader = InfoFileLoader(RRN) 174 | mp_robot_info, mp_robot_ident_fd = info_loader.LoadInfoFileFromDict(mp_robot_info_dict, "experimental.robotics.motion_program.MotionProgramRobotInfo", "mp_robot") 175 | 176 | attributes_util = AttributesUtil(RRN) 177 | mp_robot_attributes = attributes_util.GetDefaultServiceAttributesFromDeviceInfo(mp_robot_info.robot_info.device_info) 178 | 179 | mp_exec_obj = MotionExecImpl(mp_robot_info,args.mp_robot_base_url,args.mp_robot_username,args.mp_robot_password) 180 | 181 | with RR.ServerNodeSetup("experimental.robotics.motion_program",59843,argv=sys.argv): 182 | 183 | service_ctx = RRN.RegisterService("mp_robot","experimental.robotics.motion_program.MotionProgramRobot",mp_exec_obj) 184 | service_ctx.SetServiceAttributes(mp_robot_attributes) 185 | service_ctx.AddExtraImport("experimental.abb_robot.motion_program") 186 | 187 | print("Press ctrl+c to quit") 188 | drekar_launch_process.wait_exit() 189 | 190 | if __name__ == "__main__": 191 | main() 192 | 193 | -------------------------------------------------------------------------------- /src/abb_motion_program_exec/robotraconteur/experimental.abb_robot.motion_program.robdef: -------------------------------------------------------------------------------- 1 | service experimental.abb_robot.motion_program 2 | 3 | import com.robotraconteur.geometry 4 | import experimental.robotics.motion_program 5 | 6 | using com.robotraconteur.geometry.Pose 7 | using com.robotraconteur.geometry.Point 8 | using experimental.robotics.motion_program.RobotPose 9 | 10 | enum CirPathModeSwitch 11 | PathFrame = 1, 12 | ObjectFrame = 2, 13 | CirPointOri = 3, 14 | Wrist45 = 4, 15 | Wrist46 = 5, 16 | Wrist56 = 6 17 | end 18 | 19 | enum egmframetype 20 | EGM_FRAME_BASE = 0, 21 | EGM_FRAME_TOOL = 1, 22 | EGM_FRAME_WOBJ = 2, 23 | EGM_FRAME_WORLD = 3, 24 | EGM_FRAME_JOINT = 4 25 | end 26 | 27 | struct CirPathModeCommand 28 | field CirPathModeSwitch switch 29 | field varvalue{string} extended 30 | end 31 | 32 | struct SyncMoveOnCommand 33 | field varvalue{string} extended 34 | end 35 | 36 | struct SyncMoveOffCommand 37 | field varvalue{string} extended 38 | end 39 | 40 | struct EGMRunJointCommand 41 | field double cond_time 42 | field double ramp_in_time 43 | field double ramp_out_time 44 | field varvalue{string} extended 45 | end 46 | 47 | struct EGMRunPoseCommand 48 | field double cond_time 49 | field double ramp_in_time 50 | field double ramp_out_time 51 | field Pose offset 52 | field varvalue{string} extended 53 | end 54 | 55 | struct EGMMoveLCommand 56 | field RobotPose tcp_pose 57 | field double tcp_velocity 58 | field double tcp_acceleration 59 | field double blend_radius 60 | field bool fine_point 61 | field varvalue{string} extended 62 | end 63 | 64 | struct EGMMoveCCommand 65 | field RobotPose tcp_via_pose 66 | field RobotPose tcp_pose 67 | field double tcp_velocity 68 | field double tcp_acceleration 69 | field double blend_radius 70 | field bool fine_point 71 | field varvalue{string} extended 72 | end 73 | 74 | struct egm_minmax 75 | field double min 76 | field double max 77 | end 78 | 79 | struct EGMStreamConfigCommand 80 | field varvalue{string} extended 81 | end 82 | 83 | struct EGMJointTargetConfigCommand 84 | field egm_minmax J1 85 | field egm_minmax J2 86 | field egm_minmax J3 87 | field egm_minmax J4 88 | field egm_minmax J5 89 | field egm_minmax J6 90 | field double max_position_deviation 91 | field double max_speed_deviation 92 | field varvalue{string} extended 93 | end 94 | 95 | struct EGMPoseTargetConfigCommand 96 | field Pose corr_frame 97 | field egmframetype corr_fr_type 98 | field Pose sensor_frame 99 | field egmframetype sensor_fr_type 100 | field egm_minmax x 101 | field egm_minmax y 102 | field egm_minmax z 103 | field egm_minmax rx 104 | field egm_minmax ry 105 | field egm_minmax rz 106 | field double max_position_deviation 107 | field double max_speed_deviation 108 | field varvalue{string} extended 109 | end 110 | 111 | struct EGMPathCorrectionConfigCommand 112 | field Pose sensor_frame 113 | field varvalue{string} extended 114 | end 115 | 116 | struct EGMJointTarget 117 | field uint64 seqno 118 | field uint64 state_seqno 119 | field double[] joints 120 | field double[] external_joints 121 | end 122 | 123 | struct EGMPoseTarget 124 | field uint64 seqno 125 | field uint64 state_seqno 126 | field Pose cartesian 127 | field double[] external_joints 128 | end 129 | 130 | struct EGMPathCorrection 131 | field uint64 seqno 132 | field uint64 state_seqno 133 | field Point pos 134 | field uint32 age 135 | end 136 | 137 | struct WorkObjectInfo 138 | field bool robhold 139 | field bool ufprog 140 | field string ufmec 141 | field Pose uframe 142 | field Pose oframe 143 | field varvalue{string} extended 144 | end 145 | 146 | struct SetWorkObjectCommand 147 | field WorkObjectInfo workobject_info 148 | field varvalue{string} extended 149 | end 150 | 151 | -------------------------------------------------------------------------------- /src/abb_motion_program_exec/robotraconteur/experimental.robotics.motion_program.robdef: -------------------------------------------------------------------------------- 1 | service experimental.robotics.motion_program 2 | 3 | stdver 0.10 4 | 5 | import com.robotraconteur.device 6 | import com.robotraconteur.geometry 7 | import com.robotraconteur.robotics.joints 8 | import com.robotraconteur.robotics.tool 9 | import com.robotraconteur.robotics.payload 10 | import com.robotraconteur.robotics.robot 11 | import com.robotraconteur.action 12 | import com.robotraconteur.datetime 13 | 14 | using com.robotraconteur.device.Device 15 | using com.robotraconteur.device.DeviceInfo 16 | using com.robotraconteur.robotics.joints.JointPositionUnits 17 | using com.robotraconteur.geometry.Pose 18 | using com.robotraconteur.robotics.tool.ToolInfo 19 | using com.robotraconteur.robotics.payload.PayloadInfo 20 | using com.robotraconteur.robotics.robot.RobotInfo 21 | using com.robotraconteur.action.ActionStatusCode 22 | using com.robotraconteur.datetime.TimeSpec3 23 | using com.robotraconteur.robotics.robot.RobotOperationalMode 24 | using com.robotraconteur.robotics.robot.RobotControllerState 25 | 26 | enum MotionProgramRobotCapabilities 27 | unknown = 0, 28 | queuing = 0x01, 29 | current_command_feedback = 0x02, 30 | queued_command_feedback = 0x04, 31 | motion_recording = 0x08, 32 | motion_program_preemption = 0x10, 33 | motion_program_mode_select = 0x20, 34 | preempt_number_feedback = 0x40 35 | end 36 | 37 | enum MotionProgramRobotStateFlags 38 | unknown = 0, 39 | error = 0x1, 40 | fatal_error = 0x2, 41 | estop = 0x4, 42 | estop_button1 = 0x8, 43 | estop_button2 = 0x10, 44 | estop_button3 = 0x20, 45 | estop_button4 = 0x40, 46 | estop_guard1 = 0x80, 47 | estop_guard2 = 0x100, 48 | estop_guard3 = 0x200, 49 | estop_guard4 = 0x400, 50 | estop_software = 0x800, 51 | estop_fault = 0x1000, 52 | estop_internal = 0x2000, 53 | estop_other = 0x4000, 54 | estop_released = 0x8000, 55 | enabling_switch = 0x10000, 56 | enabled = 0x20000, 57 | ready = 0x40000, 58 | homed = 0x80000, 59 | homing_required = 0x100000, 60 | communication_failure = 0x200000, 61 | motion_program_mode_enabled = 0x10000000, 62 | motion_program_running = 0x20000000 63 | end 64 | 65 | struct RobotPose 66 | field Pose tcp_pose 67 | field double[] joint_position_seed 68 | field JointPositionUnits{list} joint_units 69 | end 70 | 71 | struct MoveAbsJCommand 72 | field double[] joint_position 73 | field JointPositionUnits{list} joint_units 74 | field double tcp_velocity 75 | field double tcp_acceleration 76 | field double blend_radius 77 | field bool fine_point 78 | field varvalue{string} extended 79 | end 80 | 81 | struct MoveJCommand 82 | field RobotPose tcp_pose 83 | field double tcp_velocity 84 | field double tcp_acceleration 85 | field double blend_radius 86 | field bool fine_point 87 | field varvalue{string} extended 88 | end 89 | 90 | struct MoveLCommand 91 | field RobotPose tcp_pose 92 | field double tcp_velocity 93 | field double tcp_acceleration 94 | field double blend_radius 95 | field bool fine_point 96 | field varvalue{string} extended 97 | end 98 | 99 | struct MoveCCommand 100 | field RobotPose tcp_via_pose 101 | field RobotPose tcp_pose 102 | field double tcp_velocity 103 | field double tcp_acceleration 104 | field double blend_radius 105 | field bool fine_point 106 | field varvalue{string} extended 107 | end 108 | 109 | struct WaitTimeCommand 110 | field double time 111 | field varvalue{string} extended 112 | end 113 | 114 | struct SetToolCommand 115 | field ToolInfo tool_info 116 | field varvalue{string} extended 117 | end 118 | 119 | struct SetPayloadCommand 120 | field PayloadInfo payload_info 121 | field Pose payload_pose 122 | field varvalue{string} extended 123 | end 124 | 125 | struct FreeformCommand 126 | field string command_name 127 | field varvalue{string} command_args 128 | field bool optional 129 | field varvalue{string} extended 130 | end 131 | 132 | struct MotionProgram 133 | field varvalue{list} motion_setup_commands 134 | field varvalue{list} motion_program_commands 135 | field varvalue{string} extended 136 | end 137 | 138 | struct MotionProgramCommandInfo 139 | field string command_name 140 | field string{list} command_struct_types 141 | field string{list} freeform_command_names 142 | field string description 143 | field varvalue{string} extended 144 | end 145 | 146 | struct MotionProgramRobotInfo 147 | field RobotInfo robot_info 148 | field MotionProgramCommandInfo{list} supported_setup_commands 149 | field MotionProgramCommandInfo{list} supported_motion_commands 150 | field uint32 motion_program_robot_capabilities 151 | field varvalue{string} extended 152 | end 153 | 154 | struct MotionProgramStatus 155 | field ActionStatusCode action_status 156 | field int32 current_command 157 | field int32 queued_command 158 | field int32 current_preempt 159 | field uint32 recording_handle 160 | end 161 | 162 | struct MotionProgramRobotState 163 | field TimeSpec3 ts 164 | field uint64 seqno 165 | field RobotOperationalMode operational_mode 166 | field RobotControllerState controller_state 167 | field uint64 motion_program_robot_state_flags 168 | field int32 current_command 169 | field int32 queued_command 170 | field int32 current_preempt 171 | end 172 | 173 | struct MotionProgramRecordingPart 174 | field double[] time 175 | field int32[] command_number 176 | field double[*] joints 177 | field string{list} column_headers 178 | end 179 | 180 | object MotionProgramRobot 181 | implements Device 182 | property DeviceInfo device_info [readonly,nolock] 183 | property RobotInfo robot_info [readonly,nolock] 184 | property MotionProgramRobotInfo motion_program_robot_info [readonly,nolock] 185 | function MotionProgramStatus{generator} execute_motion_program(MotionProgram program, bool queue) 186 | function MotionProgramStatus{generator} execute_motion_program_record(MotionProgram program, bool queue) 187 | function void preempt_motion_program(MotionProgram program, uint32 preempt_number, uint32 preempt_cmdnum) 188 | function MotionProgramRecordingPart{generator} read_recording(uint32 recording_handle) 189 | wire MotionProgramRobotState motion_program_robot_state [readonly,nolock] 190 | function void enable_motion_program_mode() 191 | function void disable_motion_program_mode() 192 | function void clear_recordings() 193 | function varvalue getf_param(string param_name) 194 | function void setf_param(string param_name, varvalue value) 195 | event param_changed(string param_name) 196 | end --------------------------------------------------------------------------------