├── .github └── workflows │ └── python-package.yml ├── .gitignore ├── LICENSE ├── README.md ├── examples ├── example_trajectory.py ├── sim.py ├── test_moto.py ├── test_moto_rt_motion_simulation.py ├── test_rt_motion_server.py └── test_tcp_client.py ├── moto ├── __init__.py ├── control_group.py ├── io_connection.py ├── motion_connection.py ├── real_time_motion_connection.py ├── sim │ ├── __init__.py │ ├── control_group.py │ ├── motion_controller_simulator.py │ ├── motosim.py │ └── realtime_motion_server_sim.py ├── simple_message.py ├── simple_message_connection.py └── state_connection.py ├── setup.py └── test └── test_simple_message.py /.github/workflows/python-package.yml: -------------------------------------------------------------------------------- 1 | # This workflow will install Python dependencies, run tests and lint with a variety of Python versions 2 | # For more information see: https://help.github.com/actions/language-and-framework-guides/using-python-with-github-actions 3 | 4 | name: Python package 5 | 6 | on: 7 | push: 8 | branches: [ main ] 9 | pull_request: 10 | branches: [ main ] 11 | 12 | jobs: 13 | build: 14 | 15 | runs-on: ubuntu-latest 16 | strategy: 17 | matrix: 18 | python-version: [3.8] 19 | 20 | steps: 21 | - uses: actions/checkout@v2 22 | - name: Set up Python ${{ matrix.python-version }} 23 | uses: actions/setup-python@v2 24 | with: 25 | python-version: ${{ matrix.python-version }} 26 | - name: Install dependencies 27 | run: | 28 | python -m pip install --upgrade pip 29 | pip install flake8 pytest 30 | if [ -f requirements.txt ]; then pip install -r requirements.txt; fi 31 | - name: Lint with flake8 32 | run: | 33 | # stop the build if there are Python syntax errors or undefined names 34 | #flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics 35 | # exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide 36 | #flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics 37 | - name: Test with pytest 38 | run: | 39 | pytest test 40 | -------------------------------------------------------------------------------- /.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 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 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 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | .vscode 131 | .vscode/settings.json 132 | .devcontainer/Dockerfile 133 | .devcontainer/devcontainer.json 134 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 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 [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # moto 2 | A Python library for controlling Yaskawa MOTOMAN robots with the MotoROS option. 3 | 4 | ## Installation 5 | 6 | ```bash 7 | pip3 install git+https://github.com/tingelst/moto.git --upgrade 8 | ``` 9 | 10 | On the robot side, the `moto` library employs the ROS-Industrial robot driver found here: https://github.com/ros-industrial/motoman. Follow the official [tutorial](http://wiki.ros.org/motoman_driver/Tutorials/indigo/InstallServer) for installing the necessary files on the robot controller. 11 | 12 | ## Example 13 | 14 | The highest level API is defined in the `Moto` class. 15 | 16 | ```python 17 | from moto import Moto 18 | ``` 19 | 20 | Connect to the robot controller with the defined ip address `` and define the `R1` control group with six degrees of freedom. 21 | 22 | ```python 23 | m = Moto( 24 | "", 25 | [ 26 | ControlGroupDefinition( 27 | groupid="R1", 28 | groupno=0, 29 | num_joints=6, 30 | joint_names=[ 31 | "joint_1_s", 32 | "joint_2_l", 33 | "joint_3_u", 34 | "joint_4_r", 35 | "joint_5_b", 36 | "joint_6_t", 37 | ], 38 | ), 39 | ], 40 | ) 41 | ``` 42 | 43 | If your robot system has multiple control groups, e.g. a positioner with two degrees of freedom, these can be defined as follows: 44 | ```python 45 | m = Moto( 46 | "", 47 | [ 48 | ControlGroupDefinition( 49 | groupid="R1", 50 | groupno=0, 51 | num_joints=6, 52 | joint_names=[ 53 | "joint_1_s", 54 | "joint_2_l", 55 | "joint_3_u", 56 | "joint_4_r", 57 | "joint_5_b", 58 | "joint_6_t", 59 | ], 60 | ), 61 | ControlGroupDefinition( 62 | groupid="S1", 63 | groupno=1, 64 | num_joints=2, 65 | joint_names=[ 66 | "joint_1", 67 | "joint_2", 68 | ], 69 | ), 70 | ], 71 | ) 72 | ``` 73 | The system supports up to 4 control groups. 74 | 75 | Each control group can be accessed and introspected by name: 76 | ```python 77 | r1 = m.control_groups["R1"] 78 | print(r1.position) 79 | ``` 80 | 81 | ### Motion 82 | 83 | To be able to send trajectores, you must first start the trajectory mode: 84 | ```python 85 | m.motion.start_trajectory_mode() 86 | ``` 87 | 88 | The API for sending trajectories is still under development. For now, to move joint 1 you can e.g. do: 89 | ```python 90 | robot_joint_feedback = m.state.joint_feedback_ex() 91 | 92 | p0 = JointTrajPtFullEx( 93 | number_of_valid_groups=2, 94 | sequence=0, 95 | joint_traj_pt_data=[ 96 | JointTrajPtExData( 97 | groupno=0, 98 | valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY, 99 | time=0.0, 100 | pos=robot_joint_feedback.joint_feedback_data[0].pos, 101 | vel=[0.0] * 10, 102 | acc=[0.0] * 10, 103 | ), 104 | JointTrajPtExData( 105 | groupno=1, 106 | valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY, 107 | time=0.0, 108 | pos=robot_joint_feedback.joint_feedback_data[1].pos, 109 | vel=[0.0] * 10, 110 | acc=[0.0] * 10, 111 | ), 112 | ], 113 | ) 114 | 115 | p1 = JointTrajPtFullEx( 116 | number_of_valid_groups=2, 117 | sequence=1, 118 | joint_traj_pt_data=[ 119 | JointTrajPtExData( 120 | groupno=0, 121 | valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY, 122 | time=5.0, 123 | pos=np.deg2rad([10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), 124 | vel=[0.0] * 10, 125 | acc=[0.0] * 10, 126 | ), 127 | JointTrajPtExData( 128 | groupno=1, 129 | valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY, 130 | time=5.0, 131 | pos=np.deg2rad([10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), 132 | vel=[0.0] * 10, 133 | acc=[0.0] * 10, 134 | ), 135 | ], 136 | ) 137 | 138 | m.motion.send_joint_trajectory_point(p0) # Current position at time t=0.0 139 | m.motion.send_joint_trajectory_point(p1) # Desired position at time t=5.0 140 | ``` 141 | 142 | ### IO 143 | 144 | You can read and write bits: 145 | ```python 146 | m.io.read_bit(27010) 147 | m.io.write_bit(27010, 0) 148 | ``` 149 | as well as bytes: 150 | ```python 151 | m.io.read_group(1001) 152 | m.io.write_group(1001, 42) 153 | ``` 154 | 155 | As per the [documentation](https://github.com/ros-industrial/motoman/blob/591a09c5cb95378aafd02e77e45514cfac3a009d/motoman_msgs/srv/WriteSingleIO.srv#L9-L12), only the following addresses can be written to: 156 | - 27010 and up : Network Inputs (25010 and up on DX100 and FS100) 157 | - 10010 and up : Universal/General Outputs 158 | 159 | ### ROS2 and Real-time control 160 | 161 | An extension of the current robot side driver with support for real-time control, and an accompanying ROS2 Control hardware interface is under development [here](https://github.com/tingelst/motoman) and [here](https://github.com/tingelst/motoman_hardware), respectively. 162 | 163 | ## Troubleshooting 164 | This is based on experiences with the YRC1000 controller, but should be similar for other controllers as well. 165 | 166 | ### When connection I get `[Errno 113] No route to host` 167 | The IP is not correct. 168 | You can find the IP of the controller in the pendant by performing these steps: 169 | 170 | 1. Go into management mode 171 | * System info > Security > Change to 'MANAGEMENT MODE' 172 | * Default password on the YRC1000 controller is all 9's (sixteen nines) 173 | 2. Go into network services 174 | * System info > Network Services 175 | * Look up the correct IP for the LAN port you are connected to. 176 | * In our case, we use LAN2 which has a default IP of 192.168.255.200 177 | 178 | ### When connecting, nothing happens or `[Errno 101] Network is unreachable` 179 | Your computer is most likely not on the same subnetwork as the robot, and the connection times out. 180 | 181 | You have to manually set the IP and subnetmask of your computer to be the within the same subnet as the robot controller. You can find the IP of the robot controller by following the instructions above: [link](#errno-113-no-route-to-host). 182 | 183 | > **Example**: If the robot has the IP 192.168.255.200 with a subnet of 255.255.255.0, then a valid IP of the computer would be an IP in the range 192.168.255.1-255 (except for 200). And the subnetmask should be set to 255.255.255.0 184 | 185 | ### The robot doesn't move when I call `send_joint_trajectory_point()` 186 | * Is the pendant set to remote mode? 187 | * Are all emergency stop buttons reset? 188 | * Is the door to the robot cell closed and reset? 189 | 190 | A recommendation is to assert that `robot_status().mode == PendantMode.AUTO` before trying to send any trajectories. 191 | 192 | # Acknowledgements 193 | 194 | This work is supported by the Norwegian Research Council infrastructure project MANULAB: Norwegian Manufacturing Research Laboratory under grant 269898. 195 | 196 | 197 | -------------------------------------------------------------------------------- /examples/example_trajectory.py: -------------------------------------------------------------------------------- 1 | from moto.simple_message import JointTrajPtExData, JointTrajPtFullEx, ValidFields 2 | 3 | p0 = JointTrajPtExData( 4 | groupno=0, 5 | valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY, 6 | time=0.0, 7 | pos=[0.0] * 10, 8 | vel=[0.0] * 10, 9 | acc=[0.0] * 10, 10 | ) 11 | 12 | p1 = JointTrajPtExData( 13 | groupno=1, 14 | valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY, 15 | time=0.0, 16 | pos=[0.0] * 10, 17 | vel=[0.0] * 10, 18 | acc=[0.0] * 10, 19 | ) 20 | 21 | t = JointTrajPtFullEx( 22 | number_of_valid_groups=2, 23 | sequence=0, 24 | joint_traj_pt_data= 25 | [ JointTrajPtExData( 26 | groupno=0, 27 | valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY, 28 | time=0.0, 29 | pos=[0.0] * 10, 30 | vel=[0.0] * 10, 31 | acc=[0.0] * 10, 32 | ), 33 | JointTrajPtExData( 34 | groupno=1, 35 | valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY, 36 | time=0.0, 37 | pos=[0.0] * 10, 38 | vel=[0.0] * 10, 39 | acc=[0.0] * 10, 40 | ) 41 | ] 42 | ) 43 | 44 | 45 | -------------------------------------------------------------------------------- /examples/sim.py: -------------------------------------------------------------------------------- 1 | from moto.sim.motosim import ControlGroupSim, MotoSim 2 | 3 | 4 | 5 | m = MotoSim("localhost", [ControlGroupSim(0, 6, [0.0] * 6)]) 6 | m.start() 7 | 8 | 9 | -------------------------------------------------------------------------------- /examples/test_moto.py: -------------------------------------------------------------------------------- 1 | 2 | from moto import Moto, ControlGroupDefinition 3 | from moto.simple_message import ( 4 | JointFeedbackEx, 5 | JointTrajPtExData, 6 | JointTrajPtFullEx, 7 | ValidFields, 8 | ) 9 | 10 | import time 11 | import numpy as np 12 | 13 | m = Moto( 14 | "172.16.0.1", #"192.168.255.201", 15 | [ 16 | ControlGroupDefinition( 17 | groupid="robot", 18 | groupno=0, 19 | num_joints=6, 20 | joint_names=[ 21 | "joint_1_s", 22 | "joint_2_l", 23 | "joint_3_u", 24 | "joint_4_r", 25 | "joint_5_b", 26 | "joint_6_t", 27 | ], 28 | ), 29 | ControlGroupDefinition( 30 | groupid="positioner", 31 | groupno=1, 32 | num_joints=2, 33 | joint_names=["joint_1", "joint_2",], 34 | ), 35 | ], 36 | start_motion_connection=True, 37 | start_state_connection=True, 38 | ) 39 | 40 | 41 | robot = m.control_groups["robot"] 42 | positioner = m.control_groups["positioner"] 43 | 44 | time.sleep(1) 45 | 46 | 47 | robot_joint_feedback: JointFeedbackEx = m.state.joint_feedback_ex() 48 | 49 | p0 = JointTrajPtFullEx( 50 | number_of_valid_groups=2, 51 | sequence=0, 52 | joint_traj_pt_data=[ 53 | JointTrajPtExData( 54 | groupno=0, 55 | valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY, 56 | time=0.0, 57 | pos=robot_joint_feedback.joint_feedback_data[0].pos, 58 | vel=[0.0] * 10, 59 | acc=[0.0] * 10, 60 | ), 61 | JointTrajPtExData( 62 | groupno=1, 63 | valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY, 64 | time=0.0, 65 | pos=robot_joint_feedback.joint_feedback_data[1].pos, 66 | vel=[0.0] * 10, 67 | acc=[0.0] * 10, 68 | ), 69 | ], 70 | ) 71 | 72 | p1 = JointTrajPtFullEx( 73 | number_of_valid_groups=2, 74 | sequence=1, 75 | joint_traj_pt_data=[ 76 | JointTrajPtExData( 77 | groupno=0, 78 | valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY, 79 | time=2.0, 80 | pos=np.deg2rad([10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), 81 | vel=[0.0] * 10, 82 | acc=[0.0] * 10, 83 | ), 84 | JointTrajPtExData( 85 | groupno=1, 86 | valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY, 87 | time=2.5, 88 | pos=np.deg2rad([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), 89 | vel=[0.0] * 10, 90 | acc=[0.0] * 10, 91 | ), 92 | ], 93 | ) 94 | 95 | WARN = '\033[5;1;31;1;44m' # blinking red on blue 96 | HIGHLIGHT = '\033[37;1;44m' # White on blue 97 | UNDERLINE = '\033[4;37;1;44m' # Underlined white on blue 98 | CLEAR = '\033[0m' # clear formatting 99 | 100 | print(f'{WARN} Warning! {CLEAR}') 101 | print(f'{HIGHLIGHT}Will now try to move the robot to its home position. '\ 102 | + f'Make sure this operation is safe!{CLEAR}') 103 | print(f'{HIGHLIGHT}Are you ready? Type \'{UNDERLINE}sure{CLEAR}{HIGHLIGHT}\' '\ 104 | + f'to start. Any other text will abort.{CLEAR}') 105 | response = input(f'>') 106 | 107 | if response == 'sure': 108 | for i in range(5, 0, -1): 109 | print(f'{i}...', end=' ') 110 | time.sleep(1); 111 | 112 | # m.motion.start_trajectory_mode() 113 | print("Waiting for robot to be ready...", end=' ') 114 | while not m.state.robot_status().motion_possible and \ 115 | not m.state.robot_status().drives_powered: 116 | time.sleep(0.1) 117 | 118 | print("Robot ready. Sending trajectory.") 119 | m.motion.send_joint_trajectory_point(p0) # Current position at time t=0.0 120 | m.motion.send_joint_trajectory_point(p1) # Desired position at time t=5.0 121 | 122 | input("Press enter when robot has stopped.") 123 | 124 | print("Disabling trajectory mode, and turning off servos.") 125 | m.motion.stop_trajectory_mode() 126 | m.motion.stop_servos() 127 | 128 | print("Done.") 129 | 130 | else: 131 | print("Aborting... Good bye.") 132 | 133 | 134 | -------------------------------------------------------------------------------- /examples/test_moto_rt_motion_simulation.py: -------------------------------------------------------------------------------- 1 | import logging 2 | from threading import Lock 3 | 4 | from moto.sim.realtime_motion_server_sim import RealTimeMotionServerSim 5 | 6 | 7 | def main(): 8 | logging.basicConfig(level=logging.DEBUG) 9 | 10 | sim = RealTimeMotionServerSim() 11 | sim.start() 12 | 13 | while True: 14 | pass 15 | 16 | 17 | 18 | if __name__ == "__main__": 19 | main() 20 | -------------------------------------------------------------------------------- /examples/test_rt_motion_server.py: -------------------------------------------------------------------------------- 1 | from copy import copy 2 | import logging 3 | import socket 4 | import time 5 | import numpy as np 6 | 7 | from moto.simple_message import * 8 | 9 | logging.basicConfig(level=logging.DEBUG) 10 | 11 | 12 | def start_udp_server(address): 13 | server = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 14 | server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 15 | server.bind(address) 16 | return server 17 | 18 | 19 | def main(): 20 | server = start_udp_server(("192.168.255.3", 50244)) 21 | started = False 22 | 23 | t0 = time.time() 24 | p0 = None 25 | while True: 26 | 27 | try: 28 | bytes_, addr = server.recvfrom(1024) 29 | if not bytes_: 30 | logging.error("Stopping!") 31 | break 32 | 33 | if not started: 34 | server.settimeout(1.0) 35 | started = True 36 | 37 | 38 | msg = SimpleMessage.from_bytes(bytes_) 39 | state: MotoRealTimeMotionJointStateEx = msg.body 40 | 41 | if p0 is None: 42 | p0 = copy(state.joint_state_data[1].pos) 43 | 44 | print("state: {}".format(state.joint_state_data[1].vel[0])) 45 | 46 | # pd = np.deg2rad(10) 47 | # Kv = 0.1 48 | # vd = Kv * (pd - state.joint_state_data[1].pos[0]) 49 | 50 | vd = 0.1 * (np.sin(3.0 * time.time() - t0)) 51 | 52 | # vd = 0.05 53 | 54 | 55 | print("command: {}".format(vd)) 56 | 57 | command_msg: SimpleMessage = SimpleMessage( 58 | Header( 59 | MsgType.MOTO_REALTIME_MOTION_JOINT_COMMAND_EX, 60 | CommType.TOPIC, 61 | ReplyType.INVALID, 62 | ), 63 | MotoRealTimeMotionJointCommandEx( 64 | state.message_id, 65 | state.number_of_valid_groups, 66 | [ 67 | MotoRealTimeMotionJointCommandExData( 68 | 0, [vd, 0.0, 0.0, 0.0, 0.0, 0.0] 69 | ), 70 | MotoRealTimeMotionJointCommandExData(1, [vd, 0.0]), 71 | 72 | ], 73 | ), 74 | ) 75 | 76 | server.sendto(command_msg.to_bytes(), addr) 77 | except socket.timeout as e: 78 | logging.error("Timed out!") 79 | break 80 | 81 | 82 | if __name__ == "__main__": 83 | 84 | main() 85 | -------------------------------------------------------------------------------- /examples/test_tcp_client.py: -------------------------------------------------------------------------------- 1 | import socket 2 | import time 3 | 4 | from moto.simple_message import * 5 | 6 | client = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 7 | client.connect(("localhost", 50243)) 8 | 9 | req = SimpleMessage( 10 | Header(MsgType.MOTO_MOTION_CTRL, CommType.SERVICE_REQUEST, ReplyType.INVALID), 11 | MotoMotionCtrl(-1, -1, CommandType.START_REALTIME_MOTION_MODE), 12 | ) 13 | client.send(req.to_bytes()) 14 | 15 | res = SimpleMessage.from_bytes(client.recv(1024)) 16 | print(res) 17 | 18 | time.sleep(3) 19 | 20 | req = SimpleMessage( 21 | Header(MsgType.MOTO_MOTION_CTRL, CommType.SERVICE_REQUEST, ReplyType.INVALID), 22 | MotoMotionCtrl(-1, -1, CommandType.STOP_REALTIME_MOTION_MODE), 23 | ) 24 | client.send(req.to_bytes()) 25 | 26 | res = SimpleMessage.from_bytes(client.recv(1024)) 27 | print(res) 28 | -------------------------------------------------------------------------------- /moto/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Norwegian University of Science and Technology. 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 List, Mapping, Tuple, Union 16 | 17 | from moto.motion_connection import MotionConnection 18 | from moto.state_connection import StateConnection 19 | from moto.io_connection import IoConnection 20 | from moto.real_time_motion_connection import RealTimeMotionConnection 21 | from moto.control_group import ControlGroupDefinition, ControlGroup 22 | from moto.simple_message import JointTrajPtExData, JointTrajPtFullEx, JointTrajPtFull 23 | 24 | 25 | class Motion: 26 | def __init__(self, motion_connection: MotionConnection) -> None: 27 | self._motion_connection: MotionConnection = motion_connection 28 | 29 | def connect(self): 30 | self._motion_connection.start() 31 | 32 | def disconnect(self): 33 | self._motion_connection.disconnect() 34 | 35 | def check_motion_ready(self): 36 | return self._motion_connection.check_motion_ready() 37 | 38 | def stop_motion(self): 39 | return self._motion_connection.stop_motion() 40 | 41 | def start_servos(self): 42 | return self._motion_connection.start_servos() 43 | 44 | def stop_servos(self): 45 | return self._motion_connection.stop_servos() 46 | 47 | def start_trajectory_mode(self): 48 | return self._motion_connection.start_traj_mode() 49 | 50 | def stop_trajectory_mode(self): 51 | return self._motion_connection.stop_traj_mode() 52 | 53 | def select_tool(self, groupno: int, tool: int, sequence: int = -1): 54 | return self._motion_connection.select_tool(groupno, tool, sequence) 55 | 56 | def get_dh_parameters(self): 57 | return self._motion_connection.get_dh_parameters() 58 | 59 | def send_joint_trajectory_point( 60 | self, joint_trajectory_point: Union[JointTrajPtFull, JointTrajPtFullEx] 61 | ): 62 | return self._motion_connection.send_joint_trajectory_point( 63 | joint_trajectory_point 64 | ) 65 | 66 | 67 | class State: 68 | def __init__(self, state_connection: StateConnection) -> None: 69 | self._state_connection: StateConnection = state_connection 70 | 71 | def connect(self): 72 | self._state_connection.start() 73 | 74 | def joint_feedback(self, groupno: int): 75 | return self._state_connection.joint_feedback(groupno) 76 | 77 | def joint_feedback_ex(self): 78 | return self._state_connection.joint_feedback_ex() 79 | 80 | def robot_status(self): 81 | return self._state_connection.robot_status() 82 | 83 | def add_joint_feedback_msg_callback(self, callback): 84 | self._state_connection.add_joint_feedback_msg_callback(callback) 85 | 86 | def add_joint_feedback_ex_msg_callback(self, callback): 87 | self._state_connection.add_joint_feedback_ex_msg_callback(callback) 88 | 89 | 90 | class IO: 91 | def __init__(self, io_connection: IoConnection) -> None: 92 | self._io_connection: IoConnection = io_connection 93 | 94 | def connect(self): 95 | self._io_connection.start() 96 | 97 | def read_bit(self, address: int): 98 | return self._io_connection.read_io_bit(address) 99 | 100 | def write_bit(self, address: int, value: int): 101 | return self._io_connection.write_io_bit(address, value) 102 | 103 | def read_group(self, address: int): 104 | return self._io_connection.read_io_group(address) 105 | 106 | def write_group(self, address: int, value: int): 107 | return self._io_connection.write_io_group(address, value) 108 | 109 | 110 | class RealTimeMotion: 111 | def __init__(self, real_time_motion_connection: RealTimeMotionConnection) -> None: 112 | self._real_time_motion_connection: RealTimeMotionConnection = ( 113 | real_time_motion_connection 114 | ) 115 | 116 | def connect(self): 117 | self._real_time_motion_connection.start() 118 | 119 | def start_rt_mode(self): 120 | self._real_time_motion_connection.start_rt_mode() 121 | 122 | def stop_rt_mode(self): 123 | self._real_time_motion_connection.stop_rt_mode() 124 | 125 | 126 | class Moto: 127 | def __init__( 128 | self, 129 | robot_ip: str, 130 | control_group_defs: List[ControlGroupDefinition], 131 | start_motion_connection: bool = True, 132 | start_state_connection: bool = True, 133 | start_io_connection: bool = True, 134 | start_real_time_connection: bool = False, 135 | ): 136 | self._robot_ip: str = robot_ip 137 | self._control_group_defs: List[ControlGroupDefinition] = control_group_defs 138 | 139 | self._motion_connection: MotionConnection = MotionConnection(self._robot_ip) 140 | self._state_connection: StateConnection = StateConnection(self._robot_ip) 141 | self._io_connection: IoConnection = IoConnection(self._robot_ip) 142 | self._real_time_motion_connection: RealTimeMotionConnection = ( 143 | RealTimeMotionConnection(self._robot_ip) 144 | ) 145 | 146 | self._control_groups: Mapping[str, ControlGroup] = {} 147 | for control_group_def in self._control_group_defs: 148 | self._control_groups[control_group_def.groupid] = ControlGroup( 149 | control_group_def, self._motion_connection, self._state_connection, 150 | ) 151 | 152 | if start_motion_connection: 153 | self._motion_connection.start() 154 | if start_state_connection: 155 | self._state_connection.start() 156 | if start_io_connection: 157 | self._io_connection.start() 158 | 159 | @property 160 | def control_groups(self): 161 | return self._control_groups 162 | 163 | @property 164 | def motion(self): 165 | return Motion(self._motion_connection) 166 | 167 | @property 168 | def state(self): 169 | return State(self._state_connection) 170 | 171 | @property 172 | def io(self): 173 | return IO(self._io_connection) 174 | 175 | @property 176 | def rt(self): 177 | return RealTimeMotion(self._real_time_motion_connection) 178 | -------------------------------------------------------------------------------- /moto/control_group.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Norwegian University of Science and Technology. 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 List 16 | from dataclasses import dataclass 17 | 18 | from moto.motion_connection import MotionConnection 19 | from moto.state_connection import StateConnection 20 | 21 | 22 | @dataclass 23 | class ControlGroupDefinition: 24 | groupid: str 25 | groupno: int 26 | num_joints: int 27 | joint_names: List[str] 28 | 29 | def __init__(self, groupid: str, groupno: int, num_joints: int, joint_names: List[str]): 30 | self.groupid: str = groupid 31 | self.groupno: str = groupno 32 | self.num_joints: int = num_joints 33 | self.joint_names: List[str] = joint_names 34 | assert self.num_joints == len(self.joint_names) 35 | 36 | 37 | class ControlGroup: 38 | def __init__( 39 | self, 40 | control_group_def: ControlGroupDefinition, 41 | motion_connection: MotionConnection, 42 | state_connection: StateConnection, 43 | ): 44 | self._control_group_def = control_group_def 45 | self._motion_connection: MotionConnection = motion_connection 46 | self._state_connection: StateConnection = state_connection 47 | 48 | @property 49 | def groupid(self) -> str: 50 | return self._control_group_def.groupid 51 | 52 | @property 53 | def groupno(self) -> int: 54 | return self._control_group_def.groupno 55 | 56 | @property 57 | def num_joints(self) -> int: 58 | return self._control_group_def.num_joints 59 | 60 | @property 61 | def joint_names(self) -> List[str]: 62 | return self._control_group_def.joint_names 63 | 64 | @property 65 | def position(self): 66 | return self.joint_feedback.pos[: self.num_joints] 67 | 68 | @property 69 | def velocity(self): 70 | return self.joint_feedback.vel[: self.num_joints] 71 | 72 | @property 73 | def acceleration(self): 74 | return self.joint_feedback.acc[: self.num_joints] 75 | 76 | @property 77 | def joint_feedback(self): 78 | return self._state_connection.joint_feedback(self.groupno) 79 | 80 | def check_queue_count(self) -> int: 81 | return self._motion_connection.check_queue_count(self.groupno) -------------------------------------------------------------------------------- /moto/io_connection.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Norwegian University of Science and Technology. 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 moto.simple_message import ( 16 | Header, 17 | MsgType, 18 | CommType, 19 | ReplyType, 20 | MSG_TYPE_CLS, 21 | SimpleMessage, 22 | ) 23 | from moto.simple_message_connection import SimpleMessageConnection 24 | 25 | 26 | class IoConnection(SimpleMessageConnection): 27 | 28 | TCP_PORT_IO = 50242 29 | 30 | def __init__(self, ip_address): 31 | super().__init__((ip_address, self.TCP_PORT_IO)) 32 | 33 | def _send_and_recv_request(self, msg_type: MsgType, *args: int) -> SimpleMessage: 34 | request: SimpleMessage = SimpleMessage( 35 | Header(msg_type, CommType.SERVICE_REQUEST, ReplyType.INVALID), 36 | MSG_TYPE_CLS[msg_type](*args), 37 | ) 38 | response: SimpleMessage = self.send_and_recv(request) 39 | return response 40 | 41 | def start(self): 42 | self._tcp_client.connect() 43 | 44 | def read_io_bit(self, address: int): 45 | return self._send_and_recv_request(MsgType.MOTO_READ_IO_BIT, address) 46 | 47 | def write_io_bit(self, address: int, value: int): 48 | return self._send_and_recv_request(MsgType.MOTO_WRITE_IO_BIT, address, value) 49 | 50 | def read_io_group(self, address: int): 51 | return self._send_and_recv_request(MsgType.MOTO_READ_IO_GROUP, address) 52 | 53 | def write_io_group(self, address: int, value: int): 54 | return self._send_and_recv_request(MsgType.MOTO_WRITE_IO_GROUP, address, value) 55 | -------------------------------------------------------------------------------- /moto/motion_connection.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Norwegian University of Science and Technology. 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 | 16 | from typing import Union 17 | 18 | from moto.simple_message_connection import SimpleMessageConnection 19 | from moto.simple_message import ( 20 | Header, 21 | MotoGetDhParameters, 22 | MsgType, 23 | CommType, 24 | ReplyType, 25 | MotoMotionCtrl, 26 | CommandType, 27 | ResultType, 28 | SelectTool, 29 | SimpleMessage, 30 | JointTrajPtFull, 31 | JointTrajPtFullEx, 32 | SimpleMessageError, 33 | ) 34 | 35 | 36 | class MotionConnection(SimpleMessageConnection): 37 | 38 | TCP_PORT_MOTION = 50240 39 | 40 | def __init__(self, ip_address): 41 | super().__init__((ip_address, self.TCP_PORT_MOTION)) 42 | 43 | def _send_and_recv_request(self, command: CommandType, groupno=-1) -> SimpleMessage: 44 | request = SimpleMessage( 45 | Header( 46 | MsgType.MOTO_MOTION_CTRL, CommType.SERVICE_REQUEST, ReplyType.INVALID 47 | ), 48 | MotoMotionCtrl(groupno=groupno, sequence=-1, command=command), 49 | ) 50 | response: SimpleMessage = self.send_and_recv(request) 51 | return response 52 | 53 | def check_motion_ready(self): 54 | return self._send_and_recv_request(CommandType.CHECK_MOTION_READY) 55 | 56 | def motion_ready(self) -> bool: 57 | response: SimpleMessage = self.check_motion_ready() 58 | return response.body.result is ResultType.SUCCESS 59 | 60 | def check_queue_count(self, groupno: int): 61 | return self._send_and_recv_request(CommandType.CHECK_QUEUE_CNT, groupno) 62 | 63 | def stop_motion(self): 64 | return self._send_and_recv_request(CommandType.STOP_MOTION) 65 | 66 | def start_servos(self): 67 | return self._send_and_recv_request(CommandType.START_SERVOS) 68 | 69 | def stop_servos(self): 70 | return self._send_and_recv_request(CommandType.STOP_SERVOS) 71 | 72 | def reset_alarm(self): 73 | return self._send_and_recv_request(CommandType.RESET_ALARM) 74 | 75 | def start_traj_mode(self): 76 | return self._send_and_recv_request(CommandType.START_TRAJ_MODE) 77 | 78 | def stop_traj_mode(self): 79 | return self._send_and_recv_request(CommandType.STOP_TRAJ_MODE) 80 | 81 | def disconnect(self): 82 | return self._send_and_recv_request(CommandType.DISCONNECT) 83 | 84 | def select_tool(self, groupno: int, tool: int, sequence: int = -1) -> SimpleMessage: 85 | request = SimpleMessage( 86 | Header( 87 | MsgType.MOTO_SELECT_TOOL, CommType.SERVICE_REQUEST, ReplyType.INVALID 88 | ), 89 | SelectTool(groupno=groupno, tool=tool, sequence=sequence), 90 | ) 91 | response: SimpleMessage = self.send_and_recv(request) 92 | return response 93 | 94 | def get_dh_parameters(self) -> SimpleMessage: 95 | request = SimpleMessage( 96 | Header( 97 | MsgType.MOTO_GET_DH_PARAMETERS, 98 | CommType.SERVICE_REQUEST, 99 | ReplyType.INVALID, 100 | ), 101 | None, 102 | ) 103 | response: SimpleMessage = self.send_and_recv(request) 104 | return response 105 | 106 | def send_joint_trajectory_point( 107 | self, joint_trajectory_point: Union[JointTrajPtFull, JointTrajPtFullEx] 108 | ) -> SimpleMessage: 109 | if isinstance(joint_trajectory_point, JointTrajPtFull): 110 | msg_type = MsgType.JOINT_TRAJ_PT_FULL 111 | elif isinstance(joint_trajectory_point, JointTrajPtFullEx): 112 | msg_type = MsgType.MOTO_JOINT_TRAJ_PT_FULL_EX 113 | elif not self.motion_ready(): 114 | raise RuntimeError("Robot is in motion ready state. " 115 | "start_traj_mode must be executed before sending trajectory " 116 | "points.") 117 | else: 118 | raise SimpleMessageError("Not a valid joint_trajectory_point.") 119 | msg = SimpleMessage( 120 | Header( 121 | msg_type=msg_type, 122 | comm_type=CommType.SERVICE_REQUEST, 123 | reply_type=ReplyType.INVALID, 124 | ), 125 | joint_trajectory_point, 126 | ) 127 | return self.send_and_recv(msg) 128 | -------------------------------------------------------------------------------- /moto/real_time_motion_connection.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Norwegian University of Science and Technology. 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 moto.simple_message_connection import SimpleMessageConnection 16 | from moto.simple_message import ( 17 | Header, 18 | MsgType, 19 | CommType, 20 | ReplyType, 21 | MotoMotionCtrl, 22 | CommandType, 23 | SimpleMessage, 24 | ) 25 | 26 | 27 | class RealTimeMotionConnection(SimpleMessageConnection): 28 | 29 | TCP_PORT_REALTIME_MOTION = 50243 30 | 31 | def __init__(self, ip_address): 32 | super().__init__((ip_address, self.TCP_PORT_REALTIME_MOTION)) 33 | 34 | def _send_and_recv_request( 35 | self, command: CommandType, groupno=-1 36 | ) -> SimpleMessage: 37 | request = SimpleMessage( 38 | Header( 39 | MsgType.MOTO_MOTION_CTRL, CommType.SERVICE_REQUEST, ReplyType.INVALID 40 | ), 41 | MotoMotionCtrl(groupno, -1, command), 42 | ) 43 | response = self.send_and_recv(request) 44 | return response 45 | 46 | def start_rt_mode(self): 47 | return self._send_and_recv_request(CommandType.START_REALTIME_MOTION_MODE) 48 | 49 | def stop_rt_mode(self): 50 | return self._send_and_recv_request(CommandType.STOP_REALTIME_MOTION_MODE) 51 | 52 | 53 | -------------------------------------------------------------------------------- /moto/sim/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Norwegian University of Science and Technology. 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 | -------------------------------------------------------------------------------- /moto/sim/control_group.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Norwegian University of Science and Technology. 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 List, Tuple 16 | 17 | from copy import copy 18 | from threading import Lock 19 | 20 | 21 | 22 | class ControlGroup: 23 | def __init__( 24 | self, groupno: int, num_joints: int, initial_joint_positions: List[float] 25 | ) -> None: 26 | assert num_joints == len(initial_joint_positions) 27 | 28 | self._groupno: int = groupno 29 | self._num_joints: int = num_joints 30 | self._positions: List[float] = initial_joint_positions 31 | self._velocities: List[float] = [0.0] * self.num_joints 32 | 33 | self._lock: Lock = Lock() 34 | 35 | @property 36 | def groupno(self) -> int: 37 | return self._groupno 38 | 39 | @property 40 | def num_joints(self) -> int: 41 | return self._num_joints 42 | 43 | @property 44 | def state(self) -> Tuple[List[float], List[float]]: 45 | with self._lock: 46 | positions = copy(self._positions) 47 | velocities = copy(self._velocities) 48 | return positions, velocities 49 | 50 | @state.setter 51 | def state(self, state_: Tuple[List[float], List[float]]): 52 | positions, velocities = state_ 53 | assert self._num_joints == len(positions) 54 | assert self._num_joints == len(velocities) 55 | with self._lock: 56 | self._positions = copy(positions) 57 | self._velocities = copy(velocities) 58 | -------------------------------------------------------------------------------- /moto/sim/motion_controller_simulator.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2012-2014, Southwest Research Institute 4 | # Copyright (c) 2014-2015, TU Delft Robotics Institute 5 | # Copyright (c) 2020, Norwegian University of Science and Technology 6 | # All rights reserved. 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above copyright 14 | # notice, this list of conditions and the following disclaimer in the 15 | # documentation and/or other materials provided with the distribution. 16 | # * Neither the name of the Southwest Research Institute, nor the names 17 | # of its contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 24 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | 32 | from typing import List 33 | import copy 34 | import threading 35 | import queue 36 | import logging 37 | import time 38 | 39 | Vector = List[float] 40 | 41 | 42 | class JointTrajectoryPoint: 43 | def __init__(self): 44 | self.positions: Vector = [] 45 | self.velocities: Vector = [] 46 | self.accelerations: Vector = [] 47 | self.effort: Vector = [] 48 | self.time_from_start: float = 0.0 49 | 50 | 51 | class MotionControllerSimulator: 52 | def __init__( 53 | self, 54 | num_joints: int, 55 | initial_joint_state: Vector, 56 | update_rate: int = 100, 57 | buffer_size: int = 0, 58 | ): 59 | # Class lock 60 | self.lock: threading.Lock = threading.Lock() 61 | 62 | # Motion loop update rate (higher update rates result in smoother simulated motion) 63 | self.update_rate: int = update_rate 64 | logging.debug("Setting motion update rate (hz): %f", self.update_rate) 65 | 66 | # Initialize joint position 67 | self.joint_positions: Vector = initial_joint_state 68 | logging.debug("Setting initial joint state: %s", str(initial_joint_state)) 69 | 70 | # Initialize motion buffer (contains joint position lists) 71 | self.motion_buffer = queue.Queue(buffer_size) 72 | logging.debug("Setting motion buffer size: %i", buffer_size) 73 | 74 | # Shutdown signal 75 | self.sig_shutdown: bool = False 76 | 77 | # Stop signal 78 | self.sig_stop: bool = False 79 | 80 | # Motion thread 81 | self.motion_thread: threading.Thread = threading.Thread( 82 | target=self._motion_worker 83 | ) 84 | self.motion_thread.daemon = True 85 | self.motion_thread.start() 86 | 87 | def add_motion_waypoint(self, point: JointTrajectoryPoint): 88 | self.motion_buffer.put(point) 89 | 90 | def get_joint_positions(self) -> Vector: 91 | with self.lock: 92 | return self.joint_positions[:] 93 | 94 | def is_in_motion(self) -> bool: 95 | return not self.motion_buffer.empty() 96 | 97 | def shutdown(self): 98 | self.sig_shutdown = True 99 | logging.debug("Motion_Controller shutdown signaled") 100 | 101 | def stop(self): 102 | logging.debug("Motion_Controller stop signaled") 103 | with self.lock: 104 | self._clear_buffer() 105 | self.sig_stop = True 106 | 107 | def interpolate( 108 | self, 109 | last_pt: JointTrajectoryPoint, 110 | current_pt: JointTrajectoryPoint, 111 | alpha: float, 112 | ): 113 | intermediate_pt = JointTrajectoryPoint() 114 | for last_joint, current_joint in zip(last_pt.positions, current_pt.positions): 115 | intermediate_pt.positions.append( 116 | last_joint + alpha * (current_joint - last_joint) 117 | ) 118 | intermediate_pt.time_from_start = last_pt.time_from_start + alpha * ( 119 | current_pt.time_from_start - last_pt.time_from_start 120 | ) 121 | return intermediate_pt 122 | 123 | def accelerate( 124 | self, 125 | last_pt: JointTrajectoryPoint, 126 | current_pt: JointTrajectoryPoint, 127 | current_time: float, 128 | delta_time: float, 129 | ): 130 | intermediate_pt = JointTrajectoryPoint() 131 | for last_joint, current_joint, last_vel, current_vel in zip( 132 | last_pt.positions, 133 | current_pt.positions, 134 | last_pt.velocities, 135 | current_pt.velocities, 136 | ): 137 | delta_x = current_joint - last_joint 138 | dv = current_vel + last_vel 139 | a1 = 6 * delta_x / pow(delta_time, 2) - 2 * (dv + last_vel) / delta_time 140 | a2 = -12 * delta_x / pow(delta_time, 3) + 6 * dv / pow(delta_time, 2) 141 | current_pos = ( 142 | last_joint 143 | + last_vel * current_time 144 | + a1 * pow(current_time, 2) / 2 145 | + a2 * pow(current_time, 3) / 6 146 | ) 147 | intermediate_pt.positions.append(current_pos) 148 | intermediate_pt.time_from_start = last_pt.time_from_start + current_time 149 | 150 | return intermediate_pt 151 | 152 | def _clear_buffer(self): 153 | with self.motion_buffer.mutex: 154 | self.motion_buffer.queue.clear() 155 | 156 | def _move_to(self, point, dur: float): 157 | time.sleep(dur) 158 | 159 | with self.lock: 160 | if not self.sig_stop: 161 | self.joint_positions = point.positions[:] 162 | else: 163 | logging.debug("Stopping motion immediately, clearing stop signal") 164 | self.sig_stop = False 165 | 166 | def _motion_worker(self): 167 | logging.debug("Starting motion worker in motion controller simulator") 168 | if self.update_rate != 0.0: 169 | update_duration = 1.0 / self.update_rate 170 | last_goal_point = JointTrajectoryPoint() 171 | 172 | with self.lock: 173 | last_goal_point.positions = self.joint_positions[:] 174 | 175 | while not self.sig_shutdown: 176 | try: 177 | current_goal_point = self.motion_buffer.get() 178 | 179 | # If the current time from start is less than the last, then it's a new trajectory 180 | if current_goal_point.time_from_start < last_goal_point.time_from_start: 181 | move_duration = current_goal_point.time_from_start 182 | # Else it's an existing trajectory and subtract the two 183 | else: 184 | # If current move duration is greater than update_duration, move arm to interpolated joint position 185 | # Provide an exception to this rule: if update rate is <=0, do not add interpolated points 186 | move_duration = ( 187 | current_goal_point.time_from_start 188 | - last_goal_point.time_from_start 189 | ) 190 | if self.update_rate > 0: 191 | starting_goal_point = copy.deepcopy(last_goal_point) 192 | full_duration = move_duration 193 | while update_duration < move_duration: 194 | if ( 195 | not starting_goal_point.velocities 196 | or not current_goal_point.velocities 197 | ): 198 | intermediate_goal_point = self.interpolate( 199 | last_goal_point, 200 | current_goal_point, 201 | update_duration / move_duration, 202 | ) 203 | else: 204 | intermediate_goal_point = self.accelerate( 205 | starting_goal_point, 206 | current_goal_point, 207 | full_duration - move_duration + update_duration, 208 | full_duration, 209 | ) 210 | self._move_to( 211 | intermediate_goal_point, update_duration 212 | ) # TODO should this use min(update_duration, 0.5*move_duration) to smooth timing? 213 | last_goal_point = copy.deepcopy(intermediate_goal_point) 214 | move_duration = ( 215 | current_goal_point.time_from_start 216 | - intermediate_goal_point.time_from_start 217 | ) 218 | 219 | self._move_to(current_goal_point, move_duration) 220 | last_goal_point = copy.deepcopy(current_goal_point) 221 | 222 | except Exception as e: 223 | logging.error("Unexpected exception: %s", e) 224 | 225 | logging.debug("Shutting down motion controller") -------------------------------------------------------------------------------- /moto/sim/motosim.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Norwegian University of Science and Technology. 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 List, Tuple, Any, Mapping 16 | 17 | import socket 18 | from threading import Thread, Lock 19 | import time 20 | import copy 21 | 22 | Vector = List[float] 23 | Address = Tuple[str, int] 24 | 25 | from moto.simple_message import ( 26 | JointFeedback, 27 | JointTrajPtFull, 28 | Header, 29 | MsgType, 30 | CommType, 31 | ReplyType, 32 | ResultType, 33 | SimpleMessage, 34 | MotoMotionCtrl, 35 | MotoMotionReply, 36 | ) 37 | 38 | from moto.sim.motion_controller_simulator import ( 39 | MotionControllerSimulator, 40 | JointTrajectoryPoint, 41 | ) 42 | 43 | 44 | def open_tcp_connection(address: Address) -> Tuple[socket.socket, Any]: 45 | server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 46 | server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 47 | server.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) 48 | server.bind(address) 49 | server.listen() 50 | return server.accept() 51 | 52 | 53 | class ControlGroupSim: 54 | def __init__( 55 | self, groupno, num_joints, initial_joint_state, update_rate=100.0 56 | ) -> None: 57 | self._groupno = groupno 58 | self._num_joints = num_joints 59 | self._initial_joint_state = initial_joint_state 60 | self._update_rate = update_rate 61 | self._motion_controller_simulator = MotionControllerSimulator( 62 | self._num_joints, self._initial_joint_state, self._update_rate 63 | ) 64 | 65 | @property 66 | def groupno(self): 67 | return self._groupno 68 | 69 | @property 70 | def num_joints(self): 71 | return self._num_joints 72 | 73 | def add_motion_waypoint(self, waypoint: JointTrajectoryPoint): 74 | self._motion_controller_simulator.add_motion_waypoint(waypoint) 75 | 76 | @property 77 | def position(self): 78 | return self._motion_controller_simulator.get_joint_positions() 79 | 80 | 81 | class MotionServer: 82 | 83 | MAX_MOTION_CONNECTIONS: int = 1 84 | TCP_PORT_MOTION: int = 50240 85 | 86 | def __init__(self, ip_address: str, control_groups: List[ControlGroupSim]): 87 | self._ip_address: str = ip_address 88 | self._conn = None 89 | 90 | self._control_groups: Mapping[str, ControlGroupSim] = {} 91 | for control_group in control_groups: 92 | self._control_groups[control_group.groupno] = control_group 93 | 94 | self._lock: Lock = Lock() 95 | self._sig_stop: bool = False 96 | 97 | self._worker_thread = Thread(target=self._worker) 98 | self._worker_thread.daemon = True 99 | 100 | def start(self) -> None: 101 | self._worker_thread.start() 102 | 103 | def stop(self) -> None: 104 | self._sig_stop = True 105 | 106 | def _worker(self) -> None: 107 | print("[motion_server]: Waiting for connection") 108 | conn, addr = open_tcp_connection((self._ip_address, self.TCP_PORT_MOTION)) 109 | print("[motion_server]: Got connection from {}".format(addr)) 110 | self._conn = conn 111 | while not self._sig_stop: 112 | msg = SimpleMessage.from_bytes(self._conn.recv(1024)) 113 | if msg.header.msg_type == MsgType.MOTO_MOTION_CTRL: 114 | reply = SimpleMessage( 115 | Header( 116 | MsgType.MOTO_MOTION_REPLY, 117 | CommType.SERVICE_REPLY, 118 | ReplyType.SUCCESS, 119 | ), 120 | MotoMotionReply( 121 | -1, 122 | -1, 123 | msg.header.msg_type, 124 | ResultType.SUCCESS, 125 | msg.body.command.value, 126 | ), 127 | ) 128 | self._conn.send(reply.to_bytes()) 129 | 130 | elif msg.header.msg_type == MsgType.JOINT_TRAJ_PT_FULL: 131 | groupno = msg.body.groupno 132 | pt = JointTrajectoryPoint() 133 | pt.time_from_start = copy.deepcopy(msg.body.time) 134 | pt.positions = copy.deepcopy( 135 | msg.body.pos[: self._control_groups[groupno].num_joints] 136 | ) 137 | self._control_groups[msg.body.groupno].add_motion_waypoint(pt) 138 | 139 | 140 | class StateServer: 141 | 142 | MAX_STATE_CONNECTIONS: int = 4 143 | TCP_PORT_STATE: int = 50241 144 | 145 | def __init__( 146 | self, ip_address: str, control_groups: List[ControlGroupSim], rate: float = 25 147 | ): 148 | self._ip_address: str = ip_address 149 | self._conn = None 150 | 151 | self._control_groups = control_groups 152 | self._rate = rate 153 | 154 | self._position = [0.0] * 10 155 | 156 | self._lock: Lock = Lock() 157 | self._sig_stop: bool = False 158 | 159 | self._worker_thread = Thread(target=self._worker) 160 | self._worker_thread.daemon = True 161 | 162 | def start(self) -> None: 163 | self._worker_thread.start() 164 | 165 | def stop(self) -> None: 166 | self._sig_stop = True 167 | 168 | def _worker(self) -> None: 169 | print("[state_server]: Waiting for connection") 170 | conn, addr = open_tcp_connection((self._ip_address, self.TCP_PORT_STATE)) 171 | print("[state_server]: Got connection from {}".format(addr)) 172 | self._conn = conn 173 | while not self._sig_stop: 174 | for control_group in self._control_groups: 175 | msg = SimpleMessage( 176 | Header(MsgType.JOINT_FEEDBACK, CommType.TOPIC, ReplyType.INVALID), 177 | JointFeedback( 178 | control_group.groupno, 179 | int("0011", 2), 180 | 0.0, 181 | list(control_group.position) 182 | + [0.0] * (10 - control_group.num_joints), 183 | [0.0] * 10, 184 | [0.0] * 10, 185 | ), 186 | ) 187 | self._conn.sendall(msg.to_bytes()) 188 | time.sleep(1.0 / self._rate) 189 | 190 | 191 | class IoServer: 192 | 193 | MAX_IO_CONNECTIONS: int = 1 194 | TCP_PORT_IO: int = 50242 195 | 196 | def __init__(self, ip_address: str) -> None: 197 | self._ip_address: str = ip_address 198 | self._conn = None 199 | 200 | self._lock: Lock = Lock() 201 | self._sig_stop: bool = False 202 | 203 | self._worker_thread = Thread(target=self._worker) 204 | self._worker_thread.daemon = True 205 | 206 | def start(self) -> None: 207 | self._worker_thread.start() 208 | 209 | def stop(self) -> None: 210 | self._sig_stop = True 211 | 212 | def _worker(self) -> None: 213 | print("[io_server]: Waiting for connection") 214 | conn, addr = open_tcp_connection((self._ip_address, self.TCP_PORT_IO)) 215 | print("[io_server]: Got connection from {}".format(addr)) 216 | self._conn = conn 217 | while not self._sig_stop: 218 | pass 219 | 220 | 221 | class MotoSim: 222 | def __init__(self, ip_address: str, control_groups: List[ControlGroupSim]): 223 | self._ip_address: str = ip_address 224 | self._control_groups: List[ControlGroupSim] = control_groups 225 | 226 | self._motion_server: MotionServer = MotionServer( 227 | self._ip_address, self._control_groups 228 | ) 229 | 230 | self._state_server: StateServer = StateServer( 231 | self._ip_address, self._control_groups 232 | ) 233 | 234 | self._io_server: IoServer = IoServer(self._ip_address) 235 | 236 | self._sig_stop: bool = False 237 | 238 | def start(self) -> None: 239 | self._motion_server.start() 240 | self._state_server.start() 241 | self._io_server.start() 242 | 243 | def stop(self) -> None: 244 | self._motion_server.stop() 245 | self._state_server.stop() 246 | self._io_server.stop() 247 | -------------------------------------------------------------------------------- /moto/sim/realtime_motion_server_sim.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Norwegian University of Science and Technology. 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 List, NamedTuple, Tuple, Any 16 | 17 | import logging 18 | import socket 19 | import time 20 | from threading import Thread, Lock 21 | 22 | from moto.sim.control_group import ControlGroup 23 | 24 | from moto.simple_message import * 25 | 26 | Address = Tuple[str, int] 27 | 28 | logging.basicConfig(level=logging.DEBUG) 29 | 30 | 31 | def open_tcp_connection(address: Address) -> Tuple[socket.socket, Any]: 32 | server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 33 | server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 34 | server.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) 35 | server.bind(address) 36 | server.listen() 37 | return server.accept() 38 | 39 | 40 | class RealTimeMotionServerSim: 41 | 42 | BUFSIZE = 1024 43 | TCP_PORT_REALTIME_MOTION = 50243 44 | UDP_PORT_REALTIME_MOTION = 50244 45 | 46 | def __init__( 47 | self, 48 | ip_address: str = "localhost", 49 | rt_server_ip_address: str = "localhost", 50 | mode: MotoRealTimeMotionMode = MotoRealTimeMotionMode.JOINT_VELOCITY, 51 | control_groups: List[ControlGroup] = [ControlGroup(0, 6, [0.0] * 6)], 52 | update_rate: int = 250, 53 | ) -> None: 54 | 55 | self._ip_address: str = ip_address 56 | self._conn: socket.socket = None 57 | 58 | self._num_control_groups = len(control_groups) 59 | assert self._num_control_groups <= 4, "Max 4 control groups can be used at once" 60 | self._control_groups: List[ControlGroup] = control_groups 61 | 62 | self._mode = mode 63 | 64 | self._lock: Lock = Lock() 65 | self._sig_stop: bool = False 66 | 67 | self._worker_thread = Thread(target=self._worker) 68 | self._worker_thread.daemon = True 69 | 70 | self._rt_server_ip_address: str = rt_server_ip_address 71 | self._rt_worker_thread = Thread(target=self._rt_worker) 72 | self._rt_worker_thread.daemon = True 73 | 74 | self._sig_stop_rt: bool = False 75 | 76 | self._update_duration: float = 1.0 / update_rate 77 | 78 | def start(self) -> None: 79 | self._worker_thread.start() 80 | 81 | def stop(self) -> None: 82 | self._sig_stop = True 83 | 84 | def _worker(self) -> None: 85 | while not self._sig_stop: 86 | logging.info("Waiting for connection") 87 | conn, addr = open_tcp_connection( 88 | (self._ip_address, self.TCP_PORT_REALTIME_MOTION) 89 | ) 90 | logging.info("Got connection from {}".format(addr)) 91 | self._conn = conn 92 | 93 | while True: 94 | 95 | try: 96 | bytes_ = self._conn.recv(self.BUFSIZE) 97 | if not bytes_: 98 | break 99 | 100 | req = SimpleMessage.from_bytes(bytes_) 101 | if req.header.msg_type == MsgType.MOTO_MOTION_CTRL: 102 | if req.body.command == CommandType.START_REALTIME_MOTION_MODE: 103 | self._rt_worker_thread.start() 104 | 105 | if req.body.command == CommandType.STOP_REALTIME_MOTION_MODE: 106 | self._sig_stop_rt = True 107 | 108 | res = SimpleMessage( 109 | Header( 110 | MsgType.MOTO_MOTION_REPLY, 111 | CommType.SERVICE_REQUEST, 112 | ReplyType.SUCCESS, 113 | ), 114 | MotoMotionReply( 115 | -1, -1, req.body.command, ResultType.SUCCESS, 0, 116 | ), 117 | ) 118 | 119 | self._conn.send(res.to_bytes()) 120 | except Exception: 121 | pass 122 | 123 | def _rt_worker(self) -> None: 124 | logging.debug("Starting RT motion thread") 125 | 126 | message_id: int = 0 127 | 128 | rt_server_addr: Tuple[str, int] = ( 129 | self._rt_server_ip_address, 130 | self.UDP_PORT_REALTIME_MOTION, 131 | ) 132 | udp_client: socket.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 133 | 134 | while not self._sig_stop_rt: 135 | 136 | state_msg = SimpleMessage( 137 | Header( 138 | MsgType.MOTO_REALTIME_MOTION_JOINT_STATE_EX, 139 | CommType.TOPIC, 140 | ReplyType.INVALID, 141 | ), 142 | MotoRealTimeMotionJointStateEx( 143 | message_id, 144 | self._mode, 145 | self._num_control_groups, 146 | [ 147 | MotoRealTimeMotionJointStateExData( 148 | control_group.groupno, *control_group.state 149 | ) 150 | for control_group in self._control_groups 151 | ], 152 | ), 153 | ) 154 | 155 | udp_client.sendto(state_msg.to_bytes(), rt_server_addr) 156 | bytes_, _ = udp_client.recvfrom(self.BUFSIZE) 157 | command_msg = SimpleMessage.from_bytes(bytes_) 158 | command: MotoRealTimeMotionJointCommandEx = command_msg.body 159 | 160 | assert command_msg.body.message_id == state_msg.body.message_id 161 | 162 | for control_group, joint_command_data in zip( 163 | self._control_groups, command.joint_command_data 164 | ): 165 | assert control_group.groupno == joint_command_data.groupno 166 | 167 | pos, vel = control_group.state 168 | 169 | if self._mode == MotoRealTimeMotionMode.IDLE: 170 | pass 171 | 172 | elif self._mode == MotoRealTimeMotionMode.JOINT_POSITION: 173 | pos_cmd = joint_command_data.command[: control_group.num_joints] 174 | pos = pos_cmd 175 | for k in range(control_group.num_joints): 176 | vel[k] = (pos[k] - pos_cmd[k]) / self._update_duration 177 | 178 | elif self._mode == MotoRealTimeMotionMode.JOINT_VELOCITY: 179 | for k in range(control_group.num_joints): 180 | vel_cmd = joint_command_data.command[k] 181 | pos[k] += vel_cmd * self._update_duration 182 | vel[k] = vel_cmd 183 | 184 | control_group.state = (pos, vel) 185 | 186 | message_id += 1 187 | 188 | time.sleep(self._update_duration) 189 | 190 | self._sig_stop_rt = False 191 | 192 | -------------------------------------------------------------------------------- /moto/simple_message.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Norwegian University of Science and Technology. 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 List, Tuple, Union, ClassVar 16 | from dataclasses import dataclass 17 | from enum import Enum, IntEnum, IntFlag 18 | 19 | import struct 20 | from struct import Struct 21 | 22 | ROS_MAX_JOINT: int = 10 23 | MOT_MAX_GR: int = 4 24 | 25 | 26 | class SimpleMessageError(Exception): 27 | pass 28 | 29 | 30 | @dataclass 31 | class Prefix: 32 | struct_: ClassVar[Struct] = Struct("i") 33 | size: ClassVar[int] = struct_.size 34 | 35 | length: int 36 | 37 | @classmethod 38 | def from_bytes(cls, bytes_: bytes): 39 | return cls(*cls.struct_.unpack(bytes_)) 40 | 41 | def to_bytes(self) -> bytes: 42 | return self.struct_.pack(self.length) 43 | 44 | 45 | class MsgType(Enum): 46 | INVALID = -1 47 | GET_VERSION = 2 48 | ROBOT_STATUS = 13 49 | 50 | JOINT_TRAJ_PT_FULL = 14 51 | JOINT_FEEDBACK = 15 52 | 53 | MOTO_MOTION_CTRL = 2001 54 | MOTO_MOTION_REPLY = 2002 55 | 56 | MOTO_READ_IO_BIT = 2003 57 | MOTO_READ_IO_BIT_REPLY = 2004 58 | MOTO_WRITE_IO_BIT = 2005 59 | MOTO_WRITE_IO_BIT_REPLY = 2006 60 | MOTO_READ_IO_GROUP = 2007 61 | MOTO_READ_IO_GROUP_REPLY = 2008 62 | MOTO_WRITE_IO_GROUP = 2009 63 | MOTO_WRITE_IO_GROUP_REPLY = 2010 64 | MOTO_IOCTRL_REPLY = 2011 65 | 66 | MOTO_JOINT_TRAJ_PT_FULL_EX = 2016 67 | MOTO_JOINT_FEEDBACK_EX = 2017 68 | MOTO_SELECT_TOOL = 2018 69 | 70 | MOTO_GET_DH_PARAMETERS = 2020 71 | 72 | MOTO_REALTIME_MOTION_JOINT_STATE_EX = 2030 73 | MOTO_REALTIME_MOTION_JOINT_COMMAND_EX = 2031 74 | 75 | 76 | class CommType(Enum): 77 | INVALID = 0 78 | TOPIC = 1 79 | SERVICE_REQUEST = 2 80 | SERVICE_REPLY = 3 81 | 82 | 83 | class ReplyType(Enum): 84 | INVALID = 0 85 | SUCCESS = 1 86 | FAILURE = 2 87 | 88 | 89 | class CommandType(Enum): 90 | CHECK_MOTION_READY = 200101 91 | CHECK_QUEUE_CNT = 200102 92 | STOP_MOTION = 200111 93 | # Starts the servo motors 94 | START_SERVOS = 200112 95 | # Stops the servo motors and motion 96 | STOP_SERVOS = 200113 97 | # Clears the error in the current controller 98 | RESET_ALARM = 200114 99 | START_TRAJ_MODE = 200121 100 | STOP_TRAJ_MODE = 200122 101 | DISCONNECT = 200130 102 | START_REALTIME_MOTION_MODE = 200140 103 | STOP_REALTIME_MOTION_MODE = 200141 104 | 105 | 106 | class ResultType(Enum): 107 | SUCCESS = 0 108 | TRUE = 0 109 | BUSY = 1 110 | FAILURE = 2 111 | FALSE = 2 112 | INVALID = 3 113 | ALARM = 4 114 | NOT_READY = 5 115 | MP_FAILURE = 6 116 | 117 | 118 | class InvalidSubCode(IntEnum): 119 | UNSPECIFIED = 3000 120 | MSGSIZE = 3001 121 | MSGHEADER = 3002 122 | MSGTYPE = 3003 123 | GROUPNO = 3004 124 | SEQUENCE = 3005 125 | COMMAND = 3006 126 | DATA = 3010 127 | DATA_START_POS = 3011 128 | DATA_POSITION = 3011 129 | DATA_SPEED = 3012 130 | DATA_ACCEL = 3013 131 | DATA_INSUFFICIENT = 3014 132 | DATA_TIME = 3015 133 | DATA_TOOLNO = 3016 134 | 135 | 136 | class NotReadySubcode(IntEnum): 137 | UNSPECIFIED = 5000 138 | ALARM = 5001 139 | ERROR = 5002 140 | ESTOP = 5003 141 | NOT_PLAY = 5004 142 | NOT_REMOTE = 5005 143 | SERVO_OFF = 5006 144 | HOLD = 5007 145 | NOT_STARTED = 5008 146 | WAITING_ROS = 5009 147 | SKILLSEND = 5010 148 | PFL_ACTIVE = 5011 149 | 150 | 151 | SubCode = Union[int, InvalidSubCode, NotReadySubcode] 152 | 153 | 154 | @dataclass 155 | class Header: 156 | struct_: ClassVar[Struct] = Struct("3i") 157 | size: ClassVar[int] = struct_.size 158 | 159 | msg_type: MsgType 160 | comm_type: CommType 161 | reply_type: ReplyType 162 | 163 | def __init__( 164 | self, 165 | msg_type: Union[int, MsgType], 166 | comm_type: Union[int, CommType], 167 | reply_type: Union[int, ReplyType], 168 | ): 169 | try: 170 | self.msg_type = MsgType(msg_type) 171 | self.comm_type = CommType(comm_type) 172 | self.reply_type = ReplyType(reply_type) 173 | except ValueError as e: 174 | # If any of the msg, command, or reply types isn't a type described in 175 | # Motoplus-ROS Incremental Motion interface - Engineering Design Specifications. 176 | # Then we assign it to be of type Invalid 177 | self.msg_type = MsgType(-1) 178 | 179 | 180 | @classmethod 181 | def from_bytes(cls, bytes_: bytes): 182 | return cls(*cls.struct_.unpack(bytes_)) 183 | 184 | def to_bytes(self) -> bytes: 185 | return self.struct_.pack( 186 | self.msg_type.value, self.comm_type.value, self.reply_type.value 187 | ) 188 | 189 | 190 | class ValidFields(IntFlag): 191 | """Bit-mask indicating which 'optional' fields are filled with data.""" 192 | 193 | TIME = 1 194 | POSITION = 2 195 | VELOCITY = 4 196 | ACCELERATION = 8 197 | 198 | 199 | class Ternary(Enum): 200 | UNKNOWN = -1 201 | FALSE = 0 202 | TRUE = 1 203 | 204 | def __bool__(self): 205 | if self is Ternary.TRUE: 206 | return True 207 | else: 208 | return False 209 | 210 | 211 | class PendantMode(Enum): 212 | """Controller / Pendant mode.""" 213 | 214 | UNKNOWN = -1 215 | MANUAL = 1 216 | AUTO = 2 217 | 218 | @dataclass 219 | class Invalid: 220 | data: bytes 221 | 222 | def __init__(self, data) -> None: 223 | self.data = data 224 | 225 | @classmethod 226 | def from_bytes(cls, bytes_: bytes): 227 | return cls(bytes_) 228 | 229 | 230 | @dataclass 231 | class RobotStatus: 232 | struct_: ClassVar[Struct] = Struct("7i") 233 | size = struct_.size 234 | 235 | # Servo Power: -1=Unknown, 1=ON, 0=OFF 236 | drives_powered: Ternary 237 | # Controller E-Stop state: -1=Unknown, 1=True(ON), 0=False(OFF) 238 | e_stopped: Ternary 239 | # Alarm code 240 | error_code: int 241 | # Is there an alarm: -1=Unknown, 1=True, 0=False 242 | in_error: Ternary 243 | # Is currently executing a motion command: -1=Unknown, 1=True, 0=False 244 | in_motion: Ternary 245 | # Controller/Pendant mode: -1=Unknown, 1=Manual(TEACH), 2=Auto(PLAY) 246 | mode: PendantMode 247 | # Is the controller ready to receive motion: -1=Unknown, 1=ENABLED, 0=DISABLED 248 | motion_possible: Ternary 249 | 250 | def __init__( 251 | self, 252 | drives_powered: Union[int, Ternary], 253 | e_stopped: Union[int, Ternary], 254 | error_code: int, 255 | in_error: Union[int, Ternary], 256 | in_motion: Union[int, Ternary], 257 | mode: Union[int, PendantMode], 258 | motion_possible: Union[int, Ternary], 259 | ) -> None: 260 | self.drives_powered: Ternary = Ternary(drives_powered) 261 | self.e_stopped: Ternary = Ternary(e_stopped) 262 | self.error_code: int = error_code 263 | self.in_error: Ternary = Ternary(in_error) 264 | self.in_motion: Ternary = Ternary(in_motion) 265 | self.mode: PendantMode = PendantMode(mode) 266 | self.motion_possible: Ternary = Ternary(motion_possible) 267 | 268 | @classmethod 269 | def from_bytes(cls, bytes_: bytes): 270 | return cls(*cls.struct_.unpack(bytes_[: cls.size])) 271 | 272 | def to_bytes(self) -> bytes: 273 | packed = self.struct_.pack( 274 | self.drives_powered.value, 275 | self.e_stopped.value, 276 | self.error_code, 277 | self.in_error.value, 278 | self.in_motion.value, 279 | self.mode.value, 280 | self.motion_possible.value, 281 | ) 282 | return packed 283 | 284 | 285 | @dataclass 286 | class JointTrajPtFull: 287 | struct_: ClassVar[Struct] = Struct("iiif10f10f10f") 288 | size: ClassVar[int] = struct_.size 289 | 290 | # Robot/group ID; 0 = 1st robot 291 | groupno: int 292 | # Index of point in trajectory; 0 = Initial trajectory point, 293 | # which should match the robot current position. 294 | sequence: int 295 | # Bit-mask indicating which 'optional' fields are filled with data. 296 | # 1=time, 2=position, 4=velocity, 8=acceleration 297 | valid_fields: ValidFields 298 | # Timestamp associated with this trajectory point; Units: in seconds 299 | time: float 300 | # Desired joint positions in radian. Base to Tool joint order 301 | pos: List[float] 302 | # Desired joint velocities in radian/sec. 303 | vel: List[float] 304 | # Desired joint accelerations in radian/sec^2. 305 | acc: List[float] 306 | 307 | def __init__( 308 | self, 309 | groupno: int, 310 | sequence: int, 311 | valid_fields: Union[int, ValidFields], 312 | time: float, 313 | pos: List[float], 314 | vel: List[float], 315 | acc: List[float], 316 | ) -> None: 317 | self.groupno: int = groupno 318 | self.sequence: int = sequence 319 | self.valid_fields: ValidFields = ValidFields(valid_fields) 320 | self.time: float = time 321 | self.pos: List[float] = pos 322 | self.vel: List[float] = vel 323 | self.acc: List[float] = acc 324 | 325 | @classmethod 326 | def from_bytes(cls, bytes_: bytes): 327 | unpacked: Tuple = cls.struct_.unpack(bytes_[: cls.size]) 328 | groupno = unpacked[0] 329 | sequence = unpacked[1] 330 | valid_fields = unpacked[2] 331 | time = unpacked[3] 332 | pos = unpacked[4:14] 333 | vel = unpacked[14:24] 334 | acc = unpacked[24:34] 335 | return cls(groupno, sequence, valid_fields, time, pos, vel, acc) 336 | 337 | def to_bytes(self) -> bytes: 338 | packed: bytes = self.struct_.pack( 339 | self.groupno, 340 | self.sequence, 341 | self.valid_fields.value, 342 | self.time, 343 | *self.pos, 344 | *self.vel, 345 | *self.acc, 346 | ) 347 | return packed 348 | 349 | 350 | @dataclass 351 | class JointFeedback: 352 | struct_: ClassVar[Struct] = Struct("iif10f10f10f") 353 | size: ClassVar[int] = struct_.size 354 | 355 | # Robot/group ID; 0 = 1st robot 356 | groupno: int 357 | # Bit-mask indicating which “optional” fields are filled with data. 358 | # 1=time, 2=position, 4=velocity, 8=acceleration 359 | valid_fields: ValidFields 360 | # Timestamp associated with this trajectory point; Units: in seconds 361 | time: float 362 | # Feedback joint positions in radian. Base to Tool joint order 363 | pos: List[float] 364 | # Feedback joint velocities in radian/sec. 365 | vel: List[float] 366 | # Feedback joint accelerations in radian/sec^2. 367 | acc: List[float] 368 | 369 | def __init__( 370 | self, 371 | groupno: int, 372 | valid_fields: Union[int, ValidFields], 373 | time: float, 374 | pos: List[float], 375 | vel: List[float], 376 | acc: List[float], 377 | ): 378 | self.groupno: int = groupno 379 | self.valid_fields: ValidFields = ValidFields(valid_fields) 380 | self.time: float = time 381 | self.pos: List[float] = pos 382 | self.vel: List[float] = vel 383 | self.acc: List[float] = acc 384 | 385 | @classmethod 386 | def from_bytes(cls, bytes_: bytes): 387 | unpacked = cls.struct_.unpack(bytes_[: cls.size]) 388 | groupno = unpacked[0] 389 | valid_fields = unpacked[1] 390 | time = unpacked[2] 391 | pos = unpacked[3:13] 392 | vel = unpacked[13:23] 393 | acc = unpacked[23:33] 394 | return cls(groupno, valid_fields, time, pos, vel, acc) 395 | 396 | def to_bytes(self) -> bytes: 397 | packed = self.struct_.pack( 398 | self.groupno, self.valid_fields, self.time, *self.pos, *self.vel, *self.acc 399 | ) 400 | return packed 401 | 402 | 403 | @dataclass 404 | class MotoMotionCtrl: 405 | struct_: ClassVar[Struct] = Struct("3i10f") 406 | size: ClassVar[int] = struct_.size 407 | 408 | # Robot/group ID; 0 = 1st robot 409 | groupno: int 410 | # Optional message tracking number that will be echoed back in the response. 411 | sequence: int 412 | # Desired command 413 | command: CommandType 414 | # Command data - for future use 415 | data: List[float] 416 | 417 | def __init__( 418 | self, 419 | groupno: int, 420 | sequence: int, 421 | command: CommandType, 422 | data: List[float] = [0.0] * ROS_MAX_JOINT, 423 | ): 424 | self.groupno: int = groupno 425 | self.sequence: int = sequence 426 | self.command: CommandType = CommandType(command) 427 | self.data: List[float] = data 428 | 429 | @classmethod 430 | def from_bytes(cls, bytes_: bytes): 431 | unpacked = cls.struct_.unpack(bytes_) 432 | groupno = unpacked[0] 433 | sequence = unpacked[1] 434 | command = CommandType(unpacked[2]) 435 | data = unpacked[3:13] 436 | return cls(groupno, sequence, command, data) 437 | 438 | def to_bytes(self) -> bytes: 439 | packed = self.struct_.pack( 440 | self.groupno, self.sequence, self.command.value, *self.data 441 | ) 442 | return packed 443 | 444 | 445 | @dataclass 446 | class MotoMotionReply: 447 | struct_: ClassVar[Struct] = Struct("5i10f") 448 | size = struct_.size 449 | 450 | # Robot/group ID; 0 = 1st robot 451 | groupno: int 452 | # Optional message tracking number that will be echoed back in the response. 453 | sequence: int 454 | # Reference to the received message command or type 455 | command: CommandType 456 | # High level result code 457 | result: ResultType 458 | # More detailed result code (optional) 459 | subcode: SubCode 460 | # Reply data - for future use 461 | data: List[float] 462 | 463 | def __init__( 464 | self, 465 | groupno: int, 466 | sequence: int, 467 | command: Union[int, CommandType, MsgType], 468 | result: Union[int, ResultType], 469 | subcode: SubCode, 470 | data: List[float] = [0.0] * ROS_MAX_JOINT, 471 | ): 472 | self.groupno: int = groupno 473 | self.sequence: int = sequence 474 | try: 475 | self.command: CommandType = CommandType(command) 476 | except Exception: 477 | try: 478 | self.command: MsgType = MsgType(command) 479 | except Exception: 480 | self.command: int = command 481 | try: 482 | self.result: ResultType = ResultType(result) 483 | except Exception: 484 | self.result = result 485 | try: 486 | self.subcode: InvalidSubCode = InvalidSubCode(subcode) 487 | except Exception: 488 | try: 489 | self.subcode: NotReadySubcode = NotReadySubcode(subcode) 490 | except Exception: 491 | self.subcode = subcode 492 | self.data: List[float] = data 493 | 494 | @classmethod 495 | def from_bytes(cls, bytes_: bytes): 496 | unpacked = cls.struct_.unpack(bytes_[: cls.size]) 497 | groupno = unpacked[0] 498 | sequence = unpacked[1] 499 | command = unpacked[2] 500 | result = unpacked[3] 501 | subcode = unpacked[4] 502 | data = unpacked[5:15] 503 | return cls(groupno, sequence, command, result, subcode, data) 504 | 505 | def to_bytes(self) -> bytes: 506 | packed = self.struct_.pack( 507 | self.groupno, 508 | self.sequence, 509 | self.command.value, 510 | self.result.value, 511 | self.subcode, 512 | *self.data, 513 | ) 514 | return packed 515 | 516 | 517 | @dataclass 518 | class JointTrajPtExData: 519 | struct_: ClassVar[Struct] = Struct("iif10f10f10f") 520 | size: ClassVar[int] = struct_.size 521 | 522 | groupno: int # Robot/group ID; 0 = 1st robot 523 | valid_fields: ValidFields 524 | time: float # Timestamp associated with this trajectory point; Units: in seconds 525 | # Desired joint positions in radian. Base to Tool joint order 526 | pos: List[float] 527 | vel: List[float] # Desired joint velocities in radian/sec. 528 | acc: List[float] # Desired joint accelerations in radian/sec^2. 529 | 530 | def __init__( 531 | self, 532 | groupno: int, 533 | valid_fields: Union[int, ValidFields], 534 | time: float, 535 | pos: List[float], 536 | vel: List[float], 537 | acc: List[float], 538 | ) -> None: 539 | self.groupno: int = groupno 540 | self.valid_fields: ValidFields = ValidFields(valid_fields) 541 | self.time: float = time 542 | self.pos: List[float] = pos 543 | self.vel: List[float] = vel 544 | self.acc: List[float] = acc 545 | 546 | @classmethod 547 | def from_bytes(cls, bytes_: bytes): 548 | unpacked = cls.struct_.unpack(bytes_[: cls.size]) 549 | groupno = unpacked[0] 550 | valid_fields = unpacked[1] 551 | time = unpacked[2] 552 | pos = unpacked[3:13] 553 | vel = unpacked[13:23] 554 | acc = unpacked[23:33] 555 | return cls(groupno, valid_fields, time, pos, vel, acc) 556 | 557 | def to_bytes(self) -> bytes: 558 | packed = self.struct_.pack( 559 | self.groupno, 560 | self.valid_fields.value, 561 | self.time, 562 | *self.pos, 563 | *self.vel, 564 | *self.acc, 565 | ) 566 | return packed 567 | 568 | 569 | @dataclass 570 | class JointTrajPtFullEx: 571 | number_of_valid_groups: int 572 | sequence: int 573 | joint_traj_pt_data: List[JointTrajPtExData] 574 | 575 | @property 576 | def size(self): 577 | return 8 + JointTrajPtExData.size * self.number_of_valid_groups 578 | 579 | @classmethod 580 | def from_bytes(cls, bytes_: bytes): 581 | number_of_valid_groups, sequence = struct.unpack("ii", bytes_[:8]) 582 | bytes_ = bytes_[8:] 583 | joint_traj_pt_data = [] 584 | for _ in range(number_of_valid_groups): 585 | joint_traj_pt_data.append( 586 | JointTrajPtExData.from_bytes(bytes_[: JointTrajPtExData.size]) 587 | ) 588 | bytes_ = bytes_[JointTrajPtExData.size :] 589 | return cls(number_of_valid_groups, sequence, joint_traj_pt_data) 590 | 591 | def to_bytes(self) -> bytes: 592 | packed: bytes = struct.pack("ii", self.number_of_valid_groups, self.sequence) 593 | for pt in self.joint_traj_pt_data: 594 | packed += pt.to_bytes() 595 | return packed 596 | 597 | 598 | @dataclass 599 | class JointFeedbackEx: 600 | number_of_valid_groups: int 601 | joint_feedback_data: List[JointFeedback] 602 | 603 | @property 604 | def size(self): 605 | return 4 + JointFeedback.size * self.number_of_valid_groups 606 | 607 | @classmethod 608 | def from_bytes(cls, bytes_: bytes): 609 | number_of_valid_groups = struct.unpack("i", bytes_[:4])[0] 610 | bytes_ = bytes_[4:] 611 | joint_traj_pt_data = [] 612 | for _ in range(number_of_valid_groups): 613 | joint_traj_pt_data.append( 614 | JointFeedback.from_bytes(bytes_[: JointFeedback.size]) 615 | ) 616 | bytes_ = bytes_[JointFeedback.size :] 617 | 618 | return cls(number_of_valid_groups, joint_traj_pt_data) 619 | 620 | def to_bytes(self) -> bytes: 621 | packed: bytes = struct.pack("i", self.number_of_valid_groups) 622 | for pt in self.joint_feedback_data: 623 | packed += pt.to_bytes() 624 | return packed 625 | 626 | 627 | @dataclass 628 | class SelectTool: 629 | struct_: ClassVar[Struct] = Struct("iii") 630 | size = struct_.size 631 | 632 | groupno: int 633 | tool: int 634 | sequence: int 635 | 636 | @classmethod 637 | def from_bytes(cls, bytes_: bytes): 638 | return cls(*cls.struct_.unpack(bytes_[:, cls.size])) 639 | 640 | def to_bytes(self) -> bytes: 641 | return self.struct_.pack(self.groupno, self.tool, self.sequence) 642 | 643 | 644 | class IoResultCodes(IntEnum): 645 | OK = 0 646 | # The ioAddress cannot be read on this controller 647 | READ_ADDRESS_INVALID = 1001 648 | # The ioAddress cannot be written to on this controller 649 | WRITE_ADDRESS_INVALID = 1002 650 | # The value supplied is not a valid value for the addressed IO element 651 | WRITE_VALUE_INVALID = 1003 652 | # mpReadIO return -1 653 | READ_API_ERROR = 1004 654 | # mpWriteIO returned -1 655 | WRITE_API_ERROR = 1005 656 | 657 | 658 | @dataclass 659 | class MotoReadIO: 660 | struct_: ClassVar[Struct] = Struct("I") 661 | size = struct_.size 662 | address: int 663 | 664 | @classmethod 665 | def from_bytes(cls, bytes_: bytes): 666 | unpacked = cls.struct_.unpack(bytes_[: cls.size]) 667 | address = unpacked[0] 668 | return cls(address) 669 | 670 | def to_bytes(self) -> bytes: 671 | return self.struct_.pack(self.address) 672 | 673 | 674 | @dataclass 675 | class MotoReadIOReply: 676 | struct_: ClassVar[Struct] = Struct("II") 677 | size = struct_.size 678 | value: int 679 | result_code: IoResultCodes 680 | 681 | def __init__(self, value: int, result_code: Union[int, IoResultCodes]): 682 | self.value = value 683 | try: 684 | self.result_code: IoResultCodes = IoResultCodes(result_code) 685 | except Exception: 686 | self.result_code: int = result_code 687 | 688 | @classmethod 689 | def from_bytes(cls, bytes_: bytes): 690 | unpacked = cls.struct_.unpack(bytes_[: cls.size]) 691 | value = unpacked[0] 692 | result_code = unpacked[1] 693 | return cls(value, result_code) 694 | 695 | def to_bytes(self) -> bytes: 696 | return self.struct_.pack(self.value, self.result_code) 697 | 698 | 699 | @dataclass 700 | class MotoWriteIO: 701 | struct_: ClassVar[Struct] = Struct("II") 702 | size = struct_.size 703 | address: int 704 | value: int 705 | 706 | @classmethod 707 | def from_bytes(cls, bytes_: bytes): 708 | unpacked = cls.struct_.unpack(bytes_[: cls.size]) 709 | address = unpacked[0] 710 | value = unpacked[1] 711 | return cls(address, value) 712 | 713 | def to_bytes(self) -> bytes: 714 | return self.struct_.pack(self.address, self.value) 715 | 716 | 717 | @dataclass 718 | class MotoWriteIOReply: 719 | struct_: ClassVar[Struct] = Struct("I") 720 | size = struct_.size 721 | result_code: int 722 | 723 | def __init__(self, result_code: Union[int, IoResultCodes]): 724 | try: 725 | self.result_code: IoResultCodes = IoResultCodes(result_code) 726 | except Exception: 727 | self.result_code: int = result_code 728 | 729 | @classmethod 730 | def from_bytes(cls, bytes_: bytes): 731 | unpacked = cls.struct_.unpack(bytes_[: cls.size]) 732 | result_code = unpacked[0] 733 | return cls(result_code) 734 | 735 | def to_bytes(self) -> bytes: 736 | return self.struct_.pack(self.result_code) 737 | 738 | 739 | @dataclass 740 | class MotoIoCtrlReply: 741 | struct_: ClassVar[Struct] = Struct("Ii") 742 | size = struct_.size 743 | 744 | # High level result code 745 | result: ResultType 746 | # More detailed result code (optional) 747 | subcode: SubCode 748 | 749 | @classmethod 750 | def from_bytes(cls, bytes_: bytes): 751 | unpacked = cls.struct_.unpack(bytes_[: cls.size]) 752 | result = unpacked[0] 753 | subcode = unpacked[1] 754 | return cls(result, subcode) 755 | 756 | def to_bytes(self) -> bytes: 757 | return self.struct_.pack(self.result.value, self.subcode) 758 | 759 | 760 | @dataclass 761 | class DhLink: 762 | struct_: ClassVar[Struct] = Struct("4f") 763 | size = struct_.size 764 | 765 | theta: float 766 | d: float 767 | a: float 768 | alpha: float 769 | 770 | @classmethod 771 | def from_bytes(cls, bytes_: bytes): 772 | return cls(*cls.struct_.unpack(bytes_[: cls.size])) 773 | 774 | def to_bytes(self) -> bytes: 775 | return self.struct_.pack(self.theta, self.d, self.a, self.alpha) 776 | 777 | 778 | @dataclass 779 | class DhParameters: 780 | struct_: ClassVar[Struct] = Struct("32f") 781 | size = struct_.size 782 | 783 | link: List[DhLink] 784 | 785 | @classmethod 786 | def from_bytes(cls, bytes_: bytes): 787 | link = [] 788 | for _ in range(8): 789 | link.append(DhLink.from_bytes(bytes_)) 790 | bytes_ = bytes_[DhLink.size :] 791 | return cls(link) 792 | 793 | def to_bytes(self) -> bytes: 794 | bytes_: bytes = b"" 795 | for link in self.link: 796 | bytes_ += link.to_bytes() 797 | return bytes_ 798 | 799 | 800 | @dataclass 801 | class MotoGetDhParameters: 802 | struct_: ClassVar[Struct] = Struct("128f") 803 | size = struct_.size 804 | 805 | dh_parameters: List[DhParameters] 806 | 807 | @classmethod 808 | def from_bytes(cls, bytes_: bytes): 809 | dh_parameters = [] 810 | for _ in range(MOT_MAX_GR): 811 | dh_parameters.append(DhParameters.from_bytes(bytes_)) 812 | bytes_ = bytes_[DhParameters.size :] 813 | return cls(dh_parameters) 814 | 815 | def to_bytes(self) -> bytes: 816 | bytes_: bytes = b"" 817 | for dh_parameters in self.dh_parameters: 818 | bytes_ += dh_parameters.to_bytes() 819 | return bytes_ 820 | 821 | 822 | class MotoRealTimeMotionMode(Enum): 823 | IDLE = 0 824 | JOINT_POSITION = 1 825 | JOINT_VELOCITY = 2 826 | 827 | 828 | @dataclass 829 | class MotoRealTimeMotionJointStateExData: 830 | struct_: ClassVar[Struct] = Struct("i10f10f") 831 | size: ClassVar[int] = struct_.size 832 | 833 | # Robot/group ID; 0 = 1st robot 834 | groupno: int 835 | # Feedback joint positions in radian. Base to Tool joint order 836 | pos: List[float] 837 | # Feedback joint velocities in radian/sec. 838 | vel: List[float] 839 | 840 | def __init__(self, groupno: int, pos: List[float], vel: List[float]) -> None: 841 | self.groupno = groupno 842 | num_joints = len(pos) 843 | assert num_joints == len(vel) 844 | padding = [0.0] * (ROS_MAX_JOINT - num_joints) 845 | self.pos = pos + padding 846 | self.vel = vel + padding 847 | 848 | @classmethod 849 | def from_bytes(cls, bytes_: bytes): 850 | unpacked = cls.struct_.unpack(bytes_[: cls.size]) 851 | groupno = unpacked[0] 852 | pos = list(unpacked[1:11]) 853 | vel = list(unpacked[11:21]) 854 | return cls(groupno, pos, vel) 855 | 856 | def to_bytes(self) -> bytes: 857 | return self.struct_.pack(self.groupno, *self.pos, *self.vel) 858 | 859 | 860 | @dataclass 861 | class MotoRealTimeMotionJointStateEx: 862 | # Message id that the external controller must echo back in the command 863 | message_id: int 864 | # Control mode (idle, joint position, or joint velocity) 865 | mode: MotoRealTimeMotionMode 866 | number_of_valid_groups: int # Max 4 groups 867 | joint_state_data: List[MotoRealTimeMotionJointStateExData] 868 | 869 | @property 870 | def size(self): 871 | return ( 872 | 12 + MotoRealTimeMotionJointStateExData.size * self.number_of_valid_groups 873 | ) 874 | 875 | @classmethod 876 | def from_bytes(cls, bytes_: bytes): 877 | message_id, mode, number_of_valid_groups = struct.unpack("3i", bytes_[:12]) 878 | bytes_ = bytes_[12:] 879 | joint_state_data = [] 880 | for _ in range(number_of_valid_groups): 881 | joint_state_data.append( 882 | MotoRealTimeMotionJointStateExData.from_bytes( 883 | bytes_[: MotoRealTimeMotionJointStateExData.size] 884 | ) 885 | ) 886 | bytes_ = bytes_[MotoRealTimeMotionJointStateExData.size :] 887 | 888 | return cls( 889 | message_id, 890 | MotoRealTimeMotionMode(mode), 891 | number_of_valid_groups, 892 | joint_state_data, 893 | ) 894 | 895 | def to_bytes(self) -> bytes: 896 | packed: bytes = struct.pack( 897 | "3i", self.message_id, self.mode.value, self.number_of_valid_groups 898 | ) 899 | for group_joint_state_data in self.joint_state_data: 900 | packed += group_joint_state_data.to_bytes() 901 | return packed 902 | 903 | 904 | @dataclass 905 | class MotoRealTimeMotionJointCommandExData: 906 | struct_: ClassVar[Struct] = Struct("i10f") 907 | size: ClassVar[int] = struct_.size 908 | 909 | # Robot/group ID; 0 = 1st robot 910 | groupno: int 911 | # Commanded joint positions or velocities. Depends on control mode. 912 | command: List[float] 913 | 914 | def __init__(self, groupno: int, command: List[float]) -> None: 915 | self.groupno = groupno 916 | padding = [0.0] * (ROS_MAX_JOINT - len(command)) 917 | self.command = list(command) + padding 918 | 919 | @classmethod 920 | def from_bytes(cls, bytes_: bytes): 921 | unpacked = cls.struct_.unpack(bytes_[: cls.size]) 922 | groupno = unpacked[0] 923 | command = unpacked[1:] 924 | return cls(groupno, command) 925 | 926 | def to_bytes(self) -> bytes: 927 | return self.struct_.pack(self.groupno, *self.command) 928 | 929 | 930 | @dataclass 931 | class MotoRealTimeMotionJointCommandEx: 932 | # Message id from the state message that the external controller must echo back in the command 933 | message_id: int 934 | number_of_valid_groups: int 935 | joint_command_data: List[MotoRealTimeMotionJointCommandExData] 936 | 937 | @property 938 | def size(self): 939 | return ( 940 | 8 + MotoRealTimeMotionJointCommandExData.size * self.number_of_valid_groups 941 | ) 942 | 943 | @classmethod 944 | def from_bytes(cls, bytes_: bytes): 945 | message_id, number_of_valid_groups = struct.unpack("2i", bytes_[:8]) 946 | bytes_ = bytes_[8:] 947 | joint_command_data = [] 948 | for _ in range(number_of_valid_groups): 949 | joint_command_data.append( 950 | MotoRealTimeMotionJointCommandExData.from_bytes( 951 | bytes_[: MotoRealTimeMotionJointCommandExData.size] 952 | ) 953 | ) 954 | bytes_ = bytes_[MotoRealTimeMotionJointCommandExData.size :] 955 | 956 | return cls(message_id, number_of_valid_groups, joint_command_data,) 957 | 958 | def to_bytes(self) -> bytes: 959 | packed: bytes = struct.pack("2i", self.message_id, self.number_of_valid_groups) 960 | for group_joint_command_data in self.joint_command_data: 961 | packed += group_joint_command_data.to_bytes() 962 | return packed 963 | 964 | 965 | SimpleMessageBody = Union[ 966 | RobotStatus, 967 | JointTrajPtFull, 968 | JointFeedback, 969 | MotoMotionCtrl, 970 | MotoMotionReply, 971 | JointTrajPtFullEx, 972 | JointFeedbackEx, 973 | SelectTool, 974 | MotoReadIO, 975 | MotoReadIOReply, 976 | MotoWriteIO, 977 | MotoWriteIOReply, 978 | MotoIoCtrlReply, 979 | MotoGetDhParameters, 980 | MotoRealTimeMotionJointStateEx, 981 | MotoRealTimeMotionJointCommandEx, 982 | ] 983 | 984 | 985 | MSG_TYPE_CLS = { 986 | MsgType.INVALID: Invalid, 987 | MsgType.ROBOT_STATUS: RobotStatus, 988 | MsgType.JOINT_TRAJ_PT_FULL: JointTrajPtFull, 989 | MsgType.MOTO_JOINT_TRAJ_PT_FULL_EX: JointTrajPtFullEx, 990 | MsgType.JOINT_FEEDBACK: JointFeedback, 991 | MsgType.MOTO_MOTION_CTRL: MotoMotionCtrl, 992 | MsgType.MOTO_MOTION_REPLY: MotoMotionReply, 993 | MsgType.MOTO_JOINT_FEEDBACK_EX: JointFeedbackEx, 994 | MsgType.MOTO_READ_IO_BIT: MotoReadIO, 995 | MsgType.MOTO_READ_IO_BIT_REPLY: MotoReadIOReply, 996 | MsgType.MOTO_WRITE_IO_BIT: MotoWriteIO, 997 | MsgType.MOTO_WRITE_IO_BIT_REPLY: MotoWriteIOReply, 998 | MsgType.MOTO_READ_IO_GROUP: MotoReadIO, 999 | MsgType.MOTO_READ_IO_GROUP_REPLY: MotoReadIOReply, 1000 | MsgType.MOTO_WRITE_IO_GROUP: MotoWriteIO, 1001 | MsgType.MOTO_WRITE_IO_GROUP_REPLY: MotoWriteIOReply, 1002 | MsgType.MOTO_IOCTRL_REPLY: MotoIoCtrlReply, 1003 | MsgType.MOTO_SELECT_TOOL: SelectTool, 1004 | MsgType.MOTO_GET_DH_PARAMETERS: MotoGetDhParameters, 1005 | MsgType.MOTO_REALTIME_MOTION_JOINT_STATE_EX: MotoRealTimeMotionJointStateEx, 1006 | MsgType.MOTO_REALTIME_MOTION_JOINT_COMMAND_EX: MotoRealTimeMotionJointCommandEx, 1007 | } 1008 | 1009 | 1010 | @dataclass 1011 | class SimpleMessage: 1012 | header: Header 1013 | body: SimpleMessageBody 1014 | 1015 | @classmethod 1016 | def from_bytes(cls, bytes_: bytes): 1017 | header = Header.from_bytes(bytes_[4:16]) 1018 | if header.msg_type is Invalid: 1019 | body = MSG_TYPE_CLS[header.msg_type].from_bytes(bytes_) 1020 | else: 1021 | body = MSG_TYPE_CLS[header.msg_type].from_bytes(bytes_[16:]) 1022 | return SimpleMessage(header, body) 1023 | 1024 | def to_bytes(self) -> bytes: 1025 | if self.body is not None: 1026 | return ( 1027 | Prefix(self.header.size + self.body.size).to_bytes() 1028 | + self.header.to_bytes() 1029 | + self.body.to_bytes() 1030 | ) 1031 | else: 1032 | return Prefix(self.header.size).to_bytes() + self.header.to_bytes() 1033 | 1034 | -------------------------------------------------------------------------------- /moto/simple_message_connection.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Norwegian University of Science and Technology. 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 Tuple 16 | import socket 17 | 18 | from moto.simple_message import SimpleMessage 19 | 20 | 21 | Address = Tuple[str, int] 22 | 23 | 24 | class TcpClient: 25 | def __init__(self, address: Address): 26 | self._address: Address = address 27 | self._socket: socket.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 28 | self._socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) 29 | 30 | def connect(self) -> None: 31 | try: 32 | self._socket.connect(self._address) 33 | # TODO: Write a custom exception? RobotConnectionException? 34 | except ConnectionRefusedError as e: 35 | # This can happen if: 36 | # 1. If the alarms are not reset. Then the controller refuses connection. 37 | # 2. If the user has multiple network interfaces and the one connecting 38 | # to the robot is not configured to the same subnet as the robot. 39 | error = ( 40 | f'{e.strerror}. Did you remember to reset the alarms? ' 41 | 'And is your computer on the correct subnet?' 42 | ) 43 | e.strerror = error 44 | raise 45 | except OSError as e: 46 | # This can happen if: 47 | # 1. The network interface is not set to the same subnet as the robot. 48 | if e.errno == 113: 49 | error = f'{e.strerror}. Is the robot IP correct?' 50 | elif e.errno == 101: 51 | error = f'{e.strerror}. Is your computer on the correct subnet?' 52 | e.strerror = error 53 | raise 54 | 55 | 56 | def send(self, data: bytes) -> None: 57 | self._socket.sendall(data) 58 | 59 | def recv(self, bufsize: int = 1024) -> bytes: 60 | return self._socket.recv(bufsize) 61 | 62 | 63 | class SimpleMessageConnection: 64 | def __init__(self, addr: Address) -> None: 65 | self._tcp_client = TcpClient(addr) 66 | 67 | def start(self) -> None: 68 | self._tcp_client.connect() 69 | 70 | def send(self, msg: SimpleMessage) -> None: 71 | self._tcp_client.send(msg.to_bytes()) 72 | 73 | def recv(self) -> SimpleMessage: 74 | return SimpleMessage.from_bytes(self._tcp_client.recv()) 75 | 76 | def send_and_recv(self, msg: SimpleMessage) -> SimpleMessage: 77 | self.send(msg) 78 | return self.recv() 79 | -------------------------------------------------------------------------------- /moto/state_connection.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Norwegian University of Science and Technology. 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 List, Callable 16 | from copy import deepcopy 17 | from threading import Thread, Lock, Event 18 | 19 | from moto.simple_message_connection import SimpleMessageConnection 20 | from moto.simple_message import ( 21 | JointFeedback, 22 | JointFeedbackEx, 23 | MsgType, 24 | RobotStatus, 25 | SimpleMessage, 26 | MOT_MAX_GR, 27 | ) 28 | 29 | 30 | class StateConnection(SimpleMessageConnection): 31 | 32 | TCP_PORT_STATE = 50241 33 | 34 | def __init__(self, ip_address: str): 35 | super().__init__((ip_address, self.TCP_PORT_STATE)) 36 | 37 | self._joint_feedback: List[JointFeedback] = [ 38 | None 39 | ] * MOT_MAX_GR # Max controllable groups 40 | 41 | self._joint_feedback_ex: JointFeedbackEx = None 42 | self._robot_status: RobotStatus = None 43 | self._initial_response: Event = Event() 44 | self._stop: Event = Event() 45 | self._lock: Lock = Lock() 46 | 47 | self._joint_feedback_callbacks: List[Callable] = [] 48 | self._joint_feedback_ex_callbacks: List[Callable] = [] 49 | 50 | self._worker_thread: Thread = Thread(target=self._worker) 51 | self._worker_thread.daemon = True 52 | 53 | def joint_feedback(self, groupno: int) -> JointFeedback: 54 | with self._lock: 55 | return deepcopy(self._joint_feedback[groupno]) 56 | 57 | def joint_feedback_ex(self) -> JointFeedbackEx: 58 | with self._lock: 59 | return deepcopy(self._joint_feedback_ex) 60 | 61 | def robot_status(self) -> RobotStatus: 62 | with self._lock: 63 | return deepcopy(self._robot_status) 64 | 65 | def add_joint_feedback_msg_callback(self, callback: Callable): 66 | self._joint_feedback_callbacks.append(callback) 67 | 68 | def add_joint_feedback_ex_msg_callback(self, callback: Callable): 69 | self._joint_feedback_ex_callbacks.append(callback) 70 | 71 | def start(self, timeout: float = 5.0) -> None: 72 | self._tcp_client.connect() 73 | self._worker_thread.start() 74 | if not self._initial_response.wait(timeout): 75 | self._stop.set() 76 | raise TimeoutError( 77 | "Did not receive at least one of each message before timeout" 78 | " occured. Try increasing the timeout period. " 79 | + f"Timeout currently set to {timeout}" 80 | ) 81 | 82 | def stop(self) -> None: 83 | pass 84 | 85 | def _worker(self) -> None: 86 | while True and not self._stop.is_set(): 87 | msg: SimpleMessage = self.recv() 88 | if msg.header.msg_type == MsgType.JOINT_FEEDBACK: 89 | with self._lock: 90 | self._joint_feedback[msg.body.groupno] = deepcopy(msg.body) 91 | for callback in self._joint_feedback_callbacks: 92 | callback(deepcopy(msg.body)) 93 | 94 | elif msg.header.msg_type == MsgType.MOTO_JOINT_FEEDBACK_EX: 95 | with self._lock: 96 | self._joint_feedback_ex = deepcopy(msg.body) 97 | for callback in self._joint_feedback_ex_callbacks: 98 | callback(deepcopy(msg.body)) 99 | 100 | elif msg.header.msg_type == MsgType.ROBOT_STATUS: 101 | with self._lock: 102 | self._robot_status = deepcopy(msg.body) 103 | 104 | if not self._initial_response.is_set() and ( 105 | isinstance(self.robot_status(), RobotStatus) 106 | and any(isinstance(elem, JointFeedback) for elem in self._joint_feedback) 107 | and isinstance(self.joint_feedback_ex(), JointFeedbackEx) 108 | ): 109 | self._initial_response.set() 110 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | from setuptools import setup, find_packages 5 | 6 | directory = os.path.abspath(os.path.dirname(__file__)) 7 | with open(os.path.join(directory, "README.md"), encoding="utf-8") as f: 8 | long_description = f.read() 9 | 10 | setup( 11 | name="moto", 12 | version="0.1.0", 13 | description="A library for controlling Yaskawa MOTOMAN robots using Python", 14 | author="Lars Tingelstad", 15 | author_email="lars.tingelstad@ntnu.no", 16 | license="Apache License 2.0", 17 | long_description=long_description, 18 | long_description_content_type="text/markdown", 19 | packages=find_packages(), 20 | classifiers=[ 21 | "Programming Language :: Python :: 3", 22 | "License :: OSI Approved :: Apache Software License", 23 | ], 24 | python_requires=">=3.8", 25 | ) 26 | -------------------------------------------------------------------------------- /test/test_simple_message.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from moto.simple_message import ( 4 | JointFeedback, 5 | JointTrajPtFull, 6 | MotoMotionCtrl, 7 | MotoMotionReply, 8 | PendantMode, 9 | ResultType, 10 | RobotStatus, 11 | SubCode, 12 | CommandType, 13 | Ternary, 14 | ValidFields, 15 | ) 16 | 17 | 18 | class TestRobotStatus(unittest.TestCase): 19 | def test_to_and_from_bytes(self): 20 | robot_status = RobotStatus( 21 | drives_powered=Ternary.TRUE, 22 | e_stopped=Ternary.TRUE, 23 | error_code=42, 24 | in_error=Ternary.TRUE, 25 | in_motion=Ternary.TRUE, 26 | mode=PendantMode.MANUAL, 27 | motion_possible=Ternary.TRUE, 28 | ) 29 | 30 | robot_status_from_bytes = RobotStatus.from_bytes(robot_status.to_bytes()) 31 | 32 | self.assertEqual( 33 | robot_status.drives_powered, robot_status_from_bytes.drives_powered 34 | ) 35 | self.assertEqual(robot_status.e_stopped, robot_status_from_bytes.e_stopped) 36 | self.assertEqual(robot_status.error_code, robot_status_from_bytes.error_code) 37 | self.assertEqual(robot_status.in_error, robot_status_from_bytes.in_error) 38 | self.assertEqual(robot_status.in_motion, robot_status_from_bytes.in_motion) 39 | self.assertEqual(robot_status.mode, robot_status_from_bytes.mode) 40 | self.assertEqual( 41 | robot_status.motion_possible, robot_status_from_bytes.motion_possible 42 | ) 43 | 44 | 45 | class TestJointTrajPtFull(unittest.TestCase): 46 | def test_to_and_from_bytes(self): 47 | joint_traj_pt_full = JointTrajPtFull( 48 | groupno=0, 49 | sequence=42, 50 | valid_fields=ValidFields.TIME 51 | | ValidFields.POSITION 52 | | ValidFields.VELOCITY 53 | | ValidFields.ACCELERATION, 54 | time=1.5, 55 | pos=[1.0, 2.0, 3.0123, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0], 56 | vel=[0.0] * 10, 57 | acc=[0.0] * 10, 58 | ) 59 | 60 | joint_traj_pt_full_from_bytes = JointTrajPtFull.from_bytes( 61 | joint_traj_pt_full.to_bytes() 62 | ) 63 | 64 | self.assertEqual( 65 | joint_traj_pt_full.groupno, joint_traj_pt_full_from_bytes.groupno 66 | ) 67 | self.assertEqual( 68 | joint_traj_pt_full.sequence, joint_traj_pt_full_from_bytes.sequence 69 | ) 70 | self.assertEqual( 71 | joint_traj_pt_full.valid_fields, joint_traj_pt_full_from_bytes.valid_fields 72 | ) 73 | self.assertAlmostEqual( 74 | joint_traj_pt_full.time, joint_traj_pt_full_from_bytes.time 75 | ) 76 | for p, p_from_bytes in zip( 77 | joint_traj_pt_full.pos, joint_traj_pt_full_from_bytes.pos 78 | ): 79 | self.assertAlmostEqual(p, p_from_bytes) 80 | 81 | for v, v_from_bytes in zip( 82 | joint_traj_pt_full.vel, joint_traj_pt_full_from_bytes.vel 83 | ): 84 | self.assertAlmostEqual(v, v_from_bytes) 85 | 86 | for a, a_from_bytes in zip( 87 | joint_traj_pt_full.acc, joint_traj_pt_full_from_bytes.acc 88 | ): 89 | self.assertAlmostEqual(a, a_from_bytes) 90 | 91 | 92 | class TestJointFeedback(unittest.TestCase): 93 | def test_to_and_from_bytes(self): 94 | joint_feedback = JointFeedback( 95 | groupno=0, 96 | valid_fields=ValidFields.TIME 97 | | ValidFields.POSITION 98 | | ValidFields.VELOCITY 99 | | ValidFields.ACCELERATION, 100 | time=42.1, 101 | pos=[0.0] * 10, 102 | vel=[0.0] * 10, 103 | acc=[0.0] * 10, 104 | ) 105 | 106 | joint_feedback_from_bytes = JointFeedback.from_bytes(joint_feedback.to_bytes()) 107 | 108 | self.assertEqual(joint_feedback.groupno, joint_feedback_from_bytes.groupno) 109 | self.assertEqual( 110 | joint_feedback.valid_fields, joint_feedback_from_bytes.valid_fields 111 | ) 112 | self.assertAlmostEqual( 113 | joint_feedback.time, joint_feedback_from_bytes.time, places=4 114 | ) 115 | 116 | for p, p_from_bytes in zip(joint_feedback.pos, joint_feedback_from_bytes.pos): 117 | self.assertAlmostEqual(p, p_from_bytes) 118 | 119 | for v, v_from_bytes in zip(joint_feedback.vel, joint_feedback_from_bytes.vel): 120 | self.assertAlmostEqual(v, v_from_bytes) 121 | 122 | for a, a_from_bytes in zip(joint_feedback.acc, joint_feedback_from_bytes.acc): 123 | self.assertAlmostEqual(a, a_from_bytes) 124 | 125 | 126 | class TestMotoMotionCtrl(unittest.TestCase): 127 | def test_to_and_from_bytes(self): 128 | moto_motion_ctrl = MotoMotionCtrl( 129 | groupno=1, sequence=42, command=CommandType.START_SERVOS, data=[0.0] * 10 130 | ) 131 | 132 | moto_motion_ctrl_from_bytes = MotoMotionCtrl.from_bytes( 133 | moto_motion_ctrl.to_bytes() 134 | ) 135 | 136 | self.assertEqual(moto_motion_ctrl.groupno, moto_motion_ctrl_from_bytes.groupno) 137 | self.assertEqual( 138 | moto_motion_ctrl.sequence, moto_motion_ctrl_from_bytes.sequence 139 | ) 140 | self.assertEqual(moto_motion_ctrl.command, moto_motion_ctrl_from_bytes.command) 141 | for d, d_from_bytes in zip(moto_motion_ctrl.data, moto_motion_ctrl.data): 142 | self.assertAlmostEqual(d, d_from_bytes) 143 | 144 | 145 | class TestMotoMotionReply(unittest.TestCase): 146 | def test_to_and_from_bytes(self): 147 | moto_motion_reply = MotoMotionReply( 148 | groupno=1, 149 | sequence=42, 150 | command=CommandType.START_SERVOS, 151 | result=ResultType.ALARM, 152 | subcode=SubCode.NOT_READY_HOLD, 153 | data=[0.0] * 10, 154 | ) 155 | 156 | moto_motion_reply_from_bytes = MotoMotionReply.from_bytes( 157 | moto_motion_reply.to_bytes() 158 | ) 159 | 160 | self.assertEqual( 161 | moto_motion_reply.groupno, moto_motion_reply_from_bytes.groupno 162 | ) 163 | self.assertEqual( 164 | moto_motion_reply.sequence, moto_motion_reply_from_bytes.sequence 165 | ) 166 | self.assertEqual( 167 | moto_motion_reply.command, moto_motion_reply_from_bytes.command 168 | ) 169 | self.assertEqual(moto_motion_reply.result, moto_motion_reply_from_bytes.result) 170 | self.assertEqual( 171 | moto_motion_reply.subcode, moto_motion_reply_from_bytes.subcode 172 | ) 173 | for d, d_from_bytes in zip(moto_motion_reply.data, moto_motion_reply.data): 174 | self.assertAlmostEqual(d, d_from_bytes) 175 | 176 | 177 | if __name__ == "__main__": 178 | unittest.main() 179 | --------------------------------------------------------------------------------