├── .gitignore ├── LICENSE ├── README.md ├── assembly-release.xml ├── examples ├── __init__.py ├── control_loop_configuration.xml ├── example_control_loop.py ├── example_plotting.py ├── plot.py ├── record.py ├── record_configuration.xml └── rtde_control_loop.urp ├── pom.xml ├── rtde ├── __init__.py ├── csv_binary_writer.py ├── csv_reader.py ├── csv_writer.py ├── rtde.py ├── rtde_config.py └── serialize.py ├── setup.cfg └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | .vscode 3 | dist 4 | build 5 | target 6 | UrRtde.egg-info 7 | venv 8 | *pyc -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2022, Universal Robots A/S 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | 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 ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RTDE client library - Python 2 | Library implements API for Universal Robots RTDE realtime interface. 3 | 4 | Full RTDE description is available on [Universal Robots support site](https://www.universal-robots.com/support/) 5 | # Project structure 6 | ## rtde 7 | RTDE core library 8 | 9 | - rtde.py: 10 | RTDE connection management object 11 | 12 | - rtde_config.py: 13 | XML configuration files parser 14 | 15 | - csv_writer.py, csv_reader.py: 16 | read and write rtde data objects to text csv files 17 | 18 | ## examples 19 | - record.py - example of recording realtime data from selected channels. 20 | - example_control_loop.py - example for controlling robot motion. Program moves robot between 2 setpoints. 21 | Copy rtde_control_loop.urp to the robot. Start python script before starting program. 22 | - example_plotting.py - example for using csv_reader, and plotting selected data. 23 | 24 | ### Running examples 25 | It's recommended to run examples in [virtual environment](https://docs.python.org/3/library/venv.html). 26 | Some require additional libraries. 27 | ``` 28 | python record.py -h 29 | python record.py --host 192.168.0.1 --frequency 10 30 | ``` 31 | # Using robot simulator in Docker 32 | RTDE can connect from host system to controller running in Docker 33 | when RTDE port 30004 is forwarded. 34 | 1. Get latest ursim docker image: docker pull universalrobots/ursim_e-series 35 | 2. Run docker container: docker run --rm -dit -p 30004:30004 -p 5900:5900 -p 6080:6080 universalrobots/ursim_e-series 36 | 3. open vnc client in browser, and confirm safet: http://localhost:6080/vnc.html?host=docker_ip&port=6080 37 | 38 | More information about ursim docker image is available on [Dockerhub](https://hub.docker.com/r/universalrobots/ursim_e-series) 39 | 40 | # Using robot simulator in VirtualBox 41 | RTDE can connect from host system to controller running in VirtualBox 42 | when RTDE port 30004 is forwarded. 43 | 1. Download simulator from [Universal Robots support site](https://www.universal-robots.com/support/) 44 | 2. Run simulator in VirtualBox 45 | 3. Open menu Devices->Network Settings 46 | 4. Open Advanced settings for NAT 47 | 5. Open Port Forwarding 48 | 6. Add new rule, setting host, and guest ports to 30004. 49 | Leave host, and guest IP fields blank. 50 | 51 | # Using rtde library 52 | Copy rtde folder python project 53 | Library is compatible with Python 2.7+, and Python 3.6+ 54 | 55 | # Build release package 56 | ``` 57 | mvn package 58 | ``` 59 | ## Using with virtual environment 60 | Create virtual environment, and install wheel package 61 | 62 | ### Linux & MacOS 63 | ``` 64 | python -m venv venv 65 | source venv/bin/activate 66 | pip install wheel 67 | ``` 68 | Install rtde package 69 | ``` 70 | pip install target/rtde--release.zip 71 | ``` 72 | 73 | ### Windows PowerShell 74 | If Python3 is not installed, then just run python3 from powershell. Microsoft store will launch the installation. 75 | 76 | Permission to run scripts in console is needed to activate virtual envrionment. 77 | ``` 78 | set-executionpolicy -Scope CurrentUser -ExecutionPolicy Unrestricted 79 | python -m venv venv 80 | venv/Scripts/Activate.ps1 81 | pip install wheel 82 | ``` 83 | Install rtde package 84 | ``` 85 | pip install target/rtde--release.zip 86 | ``` 87 | 88 | # Contributor guidelines 89 | Code is formatted with [black](https://github.com/psf/black). 90 | Run code formatter before submitting pull request. 91 | 92 | -------------------------------------------------------------------------------- /assembly-release.xml: -------------------------------------------------------------------------------- 1 | 3 | 4 | zip 5 | 6 | release 7 | false 8 | 9 | 10 | ${project.build.directory}/release 11 | /rtde-${version}/ 12 | 13 | **/* 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /examples/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniversalRobots/RTDE_Python_Client_Library/b23ae7a5d2e5046c1a87e2edfd4f4da5262852e8/examples/__init__.py -------------------------------------------------------------------------------- /examples/control_loop_configuration.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /examples/example_control_loop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2016-2022, Universal Robots A/S, 3 | # All rights reserved. 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # * Redistributions in binary form must reproduce the above copyright 9 | # notice, this list of conditions and the following disclaimer in the 10 | # documentation and/or other materials provided with the distribution. 11 | # * Neither the name of the Universal Robots A/S nor the names of its 12 | # contributors may be used to endorse or promote products derived 13 | # from this software without specific prior written permission. 14 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 18 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | 25 | import sys 26 | 27 | sys.path.append("..") 28 | import logging 29 | 30 | import rtde.rtde as rtde 31 | import rtde.rtde_config as rtde_config 32 | 33 | 34 | # logging.basicConfig(level=logging.INFO) 35 | 36 | ROBOT_HOST = "localhost" 37 | ROBOT_PORT = 30004 38 | config_filename = "control_loop_configuration.xml" 39 | 40 | keep_running = True 41 | 42 | logging.getLogger().setLevel(logging.INFO) 43 | 44 | conf = rtde_config.ConfigFile(config_filename) 45 | state_names, state_types = conf.get_recipe("state") 46 | setp_names, setp_types = conf.get_recipe("setp") 47 | watchdog_names, watchdog_types = conf.get_recipe("watchdog") 48 | 49 | con = rtde.RTDE(ROBOT_HOST, ROBOT_PORT) 50 | con.connect() 51 | 52 | # get controller version 53 | con.get_controller_version() 54 | 55 | # setup recipes 56 | con.send_output_setup(state_names, state_types) 57 | setp = con.send_input_setup(setp_names, setp_types) 58 | watchdog = con.send_input_setup(watchdog_names, watchdog_types) 59 | 60 | # Setpoints to move the robot to 61 | setp1 = [-0.12, -0.43, 0.14, 0, 3.11, 0.04] 62 | setp2 = [-0.12, -0.51, 0.21, 0, 3.11, 0.04] 63 | 64 | setp.input_double_register_0 = 0 65 | setp.input_double_register_1 = 0 66 | setp.input_double_register_2 = 0 67 | setp.input_double_register_3 = 0 68 | setp.input_double_register_4 = 0 69 | setp.input_double_register_5 = 0 70 | 71 | # The function "rtde_set_watchdog" in the "rtde_control_loop.urp" creates a 1 Hz watchdog 72 | watchdog.input_int_register_0 = 0 73 | 74 | 75 | def setp_to_list(sp): 76 | sp_list = [] 77 | for i in range(0, 6): 78 | sp_list.append(sp.__dict__["input_double_register_%i" % i]) 79 | return sp_list 80 | 81 | 82 | def list_to_setp(sp, list): 83 | for i in range(0, 6): 84 | sp.__dict__["input_double_register_%i" % i] = list[i] 85 | return sp 86 | 87 | 88 | # start data synchronization 89 | if not con.send_start(): 90 | sys.exit() 91 | 92 | # control loop 93 | move_completed = True 94 | while keep_running: 95 | # receive the current state 96 | state = con.receive() 97 | 98 | if state is None: 99 | break 100 | 101 | # do something... 102 | if move_completed and state.output_int_register_0 == 1: 103 | move_completed = False 104 | new_setp = setp1 if setp_to_list(setp) == setp2 else setp2 105 | list_to_setp(setp, new_setp) 106 | print("New pose = " + str(new_setp)) 107 | # send new setpoint 108 | con.send(setp) 109 | watchdog.input_int_register_0 = 1 110 | elif not move_completed and state.output_int_register_0 == 0: 111 | print("Move to confirmed pose = " + str(state.target_q)) 112 | move_completed = True 113 | watchdog.input_int_register_0 = 0 114 | 115 | # kick watchdog 116 | con.send(watchdog) 117 | 118 | con.send_pause() 119 | 120 | con.disconnect() 121 | -------------------------------------------------------------------------------- /examples/example_plotting.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2016-2022, Universal Robots A/S, 3 | # All rights reserved. 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # * Redistributions in binary form must reproduce the above copyright 9 | # notice, this list of conditions and the following disclaimer in the 10 | # documentation and/or other materials provided with the distribution. 11 | # * Neither the name of the Universal Robots A/S nor the names of its 12 | # contributors may be used to endorse or promote products derived 13 | # from this software without specific prior written permission. 14 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 18 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | 25 | import matplotlib.pyplot as plt 26 | 27 | import sys 28 | 29 | sys.path.append("..") 30 | import rtde.csv_reader as csv_reader 31 | 32 | with open("robot_data.csv") as csvfile: 33 | r = csv_reader.CSVReader(csvfile) 34 | 35 | # plot 36 | plt.plot(r.timestamp, r.target_q_1) 37 | plt.show() 38 | -------------------------------------------------------------------------------- /examples/plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2016-2022, Universal Robots A/S, 3 | # All rights reserved. 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # * Redistributions in binary form must reproduce the above copyright 9 | # notice, this list of conditions and the following disclaimer in the 10 | # documentation and/or other materials provided with the distribution. 11 | # * Neither the name of the Universal Robots A/S nor the names of its 12 | # contributors may be used to endorse or promote products derived 13 | # from this software without specific prior written permission. 14 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 18 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | import matplotlib.pyplot as p 25 | import numpy as np 26 | import argparse 27 | import logging 28 | import signal 29 | import sys 30 | 31 | sys.path.append("..") 32 | 33 | import rtde.csv_reader as csv_reader 34 | 35 | 36 | class Plotter(object): 37 | # load data 38 | plot_samples = None 39 | plot_data = [] 40 | number_of_plot_colors = 12 41 | color_list = [] 42 | x = None # data range 43 | 44 | def signal_handler(signal, frame): 45 | p.close("all") 46 | sys.exit(0) 47 | 48 | def __init__(self): 49 | # parse arguments 50 | 51 | parser = argparse.ArgumentParser() 52 | parser.add_argument("type", help="plot type (x,xd,q,qd,qdd,i,0:5)", nargs="+") 53 | parser.add_argument( 54 | "--file", default=["robot_data.csv"], help="data file", nargs="+" 55 | ) 56 | parser.add_argument( 57 | "--filter", 58 | help="exclude data when no program is running", 59 | action="store_true", 60 | ) 61 | 62 | args = parser.parse_args() 63 | 64 | logging.basicConfig(level=logging.INFO) 65 | 66 | plot_types = args.type 67 | 68 | self.get_plot_data(args) 69 | 70 | # prepare plots 71 | p.close("all") 72 | numberOfPlots = 7 73 | background_color = p.cm.gist_earth(np.random.rand(1))[0] 74 | 75 | self.x = range(self.plot_samples) 76 | 77 | self.color_list = p.cm.Paired(np.linspace(0, 1, self.number_of_plot_colors)) 78 | self.plot_all(plot_types, numberOfPlots, background_color) 79 | 80 | signal.signal(signal.SIGINT, self.signal_handler) 81 | 82 | def get_plot_color(self, style, cnt): 83 | if cnt < 0: 84 | cnt = 0 85 | if cnt >= self.number_of_plot_colors: 86 | cnt = self.number_of_plot_colors - 1 87 | if "r" in style: 88 | return self.color_list[cnt * 2] 89 | if "b" in style: 90 | return self.color_list[cnt * 2 + 1] 91 | return self.color_list[self.number_of_plot_colors - 1 - cnt] 92 | 93 | def makesubplot_withdata(self, subplot, y, name, style, y_range=6, color=None): 94 | if color is None: 95 | (axis,) = subplot.plot( 96 | self.x[0 : self.plot_samples], y[0 : self.plot_samples], style 97 | ) 98 | else: 99 | (axis,) = subplot.plot( 100 | self.x[0 : self.plot_samples], 101 | y[0 : self.plot_samples], 102 | style, 103 | color=color, 104 | ) 105 | axis.set_label(name) 106 | subplot.set_ylim([-y_range, y_range]) 107 | 108 | def makesubplot(self, subplot, name, style, y_range=6): 109 | plot_name = name 110 | cnt = 0 111 | for p in self.plot_data: 112 | y = p.__dict__[name] 113 | if len(self.plot_data) > 1: 114 | plot_name = name + " " + p.get_name() 115 | plot_color = self.get_plot_color(style, cnt) 116 | self.makesubplot_withdata(subplot, y, plot_name, style, y_range, plot_color) 117 | cnt = cnt + 1 118 | 119 | def addYtext(self, subplots, textArray): 120 | for pl in range(len(subplots)): 121 | subplots[pl].set_ylabel(textArray[pl]) 122 | return subplots 123 | 124 | def plot_all(self, plot_types, numberOfPlots, background_color): 125 | for plot_type in plot_types: 126 | f, subplots = p.subplots(numberOfPlots, sharex=True, sharey=False) 127 | tmp_window_title = f.canvas.get_window_title() 128 | f.set_facecolor(background_color) 129 | 130 | if plot_type == "q": 131 | f.suptitle("Q", fontsize=12) 132 | f.canvas.set_window_title(tmp_window_title + ": Q") 133 | naming = [ 134 | "base", 135 | "shoulder", 136 | "elbow", 137 | "wrist 1", 138 | "wrist 2", 139 | "wrist 3", 140 | "state", 141 | ] 142 | self.addYtext(subplots, naming) 143 | for i in range(6): 144 | name = "target_" + plot_type + "_" + str(i) 145 | self.makesubplot(subplots[i], name, "rx-") 146 | name = "actual_" + plot_type + "_" + str(i) 147 | self.makesubplot(subplots[i], name, "b+-") 148 | 149 | elif plot_type == "i": 150 | f.suptitle("I", fontsize=12) 151 | f.canvas.set_window_title(tmp_window_title + ": I") 152 | naming = [ 153 | "base", 154 | "shoulder", 155 | "elbow", 156 | "wrist 1", 157 | "wrist 2", 158 | "wrist 3", 159 | "state", 160 | ] 161 | self.addYtext(subplots, naming) 162 | for i in range(6): 163 | name = "target_current_" + str(i) 164 | target_current = self.plot_data[0].__dict__[name] 165 | self.makesubplot(subplots[i], name, "rx-") 166 | name = "actual_current_" + str(i) 167 | self.makesubplot(subplots[i], name, "b+-") 168 | name = "actual_current_window_" + str(i) 169 | current_window = self.plot_data[0].__dict__[name] 170 | self.makesubplot_withdata( 171 | subplots[i], 172 | target_current + current_window, 173 | "current max", 174 | "--", 175 | ) 176 | self.makesubplot_withdata( 177 | subplots[i], 178 | target_current - current_window, 179 | "current min", 180 | "--", 181 | ) 182 | 183 | elif plot_type == "qd": 184 | f.suptitle("QD", fontsize=12) 185 | f.canvas.set_window_title(tmp_window_title + ": QD") 186 | naming = [ 187 | "base", 188 | "shoulder", 189 | "elbow", 190 | "wrist 1", 191 | "wrist 2", 192 | "wrist 3", 193 | "state", 194 | ] 195 | self.addYtext(subplots, naming) 196 | for i in range(6): 197 | name = "target_" + plot_type + "_" + str(i) 198 | self.makesubplot(subplots[i], name, "rx-") 199 | name = "actual_" + plot_type + "_" + str(i) 200 | self.makesubplot(subplots[i], name, "b+-") 201 | 202 | elif plot_type == "qdd": 203 | f.suptitle("QDD", fontsize=12) 204 | f.canvas.set_window_title(tmp_window_title + ": QDD") 205 | naming = [ 206 | "base", 207 | "shoulder", 208 | "elbow", 209 | "wrist 1", 210 | "wrist 2", 211 | "wrist 3", 212 | "state", 213 | ] 214 | self.addYtext(subplots, naming) 215 | for i in range(6): 216 | name = "target_qdd_" + str(i) 217 | self.makesubplot(subplots[i], name, "rx-", 40) 218 | 219 | elif plot_type == "x": 220 | f.suptitle("X", fontsize=12) 221 | f.canvas.set_window_title(tmp_window_title + ": X") 222 | naming = ["X", "Y", "Z", "XA", "YA", "ZA", "state"] 223 | self.addYtext(subplots, naming) 224 | for i in range(6): 225 | name = "target_TCP_pose_" + str(i) 226 | self.makesubplot(subplots[i], name, "rx-") 227 | name = "actual_TCP_pose_" + str(i) 228 | self.makesubplot(subplots[i], name, "b+-") 229 | 230 | elif plot_type == "xd": 231 | f.suptitle("XD", fontsize=12) 232 | f.canvas.set_window_title(tmp_window_title + ": XD") 233 | naming = ["X", "Y", "Z", "XA", "YA", "ZA", "state"] 234 | self.addYtext(subplots, naming) 235 | for i in range(6): 236 | name = "target_TCP_speed_" + str(i) 237 | self.makesubplot(subplots[i], name, "rx-") 238 | name = "actual_TCP_speed_" + str(i) 239 | self.makesubplot(subplots[i], name, "b+-") 240 | 241 | elif plot_type.isdigit(): 242 | idx = int(plot_type) 243 | if idx < 0 or idx > 6: 244 | raise ValueError("Out of range") 245 | joints = ["base", "shoulder", "elbow", "wrist 1", "wrist 2", "wrist 3"] 246 | f.suptitle("joint: " + joints[idx], fontsize=12) 247 | f.canvas.set_window_title(tmp_window_title + ": joint " + joints[idx]) 248 | naming = [ 249 | "q", 250 | "qd", 251 | "qdd", 252 | "current", 253 | "joint mode", 254 | "control output", 255 | "state", 256 | ] 257 | self.addYtext(subplots, naming) 258 | name = "target_q_" + str(idx) 259 | self.makesubplot(subplots[0], name, "rx-") 260 | name = "actual_q_" + str(idx) 261 | self.makesubplot(subplots[0], name, "b+-") 262 | name = "target_qd_" + str(idx) 263 | self.makesubplot(subplots[1], name, "rx-") 264 | name = "actual_qd_" + str(idx) 265 | self.makesubplot(subplots[1], name, "b+-") 266 | name = "target_qdd_" + str(idx) 267 | self.makesubplot(subplots[2], name, "rx-", 40) 268 | name = "target_current_" + str(idx) 269 | target_current = self.plot_data[0].__dict__[name] 270 | self.makesubplot(subplots[3], name, "rx-") 271 | name = "actual_current_" + str(idx) 272 | self.makesubplot(subplots[3], name, "b+-") 273 | name = "actual_current_window_" + str(idx) 274 | current_window = self.plot_data[0].__dict__[name] 275 | self.makesubplot_withdata( 276 | subplots[3], target_current + current_window, "current max", "--" 277 | ) 278 | self.makesubplot_withdata( 279 | subplots[3], target_current - current_window, "current min", "--" 280 | ) 281 | name = "joint_mode_" + str(idx) 282 | self.makesubplot(subplots[4], name, "b+-") 283 | name = "joint_control_output_" + str(idx) 284 | self.makesubplot(subplots[5], name, "b+-") 285 | 286 | else: 287 | raise ValueError("Unrecognized plot type: " + plot_type) 288 | 289 | self.makesubplot(subplots[6], "robot_mode", "rx-", 10) 290 | self.makesubplot(subplots[6], "safety_mode", "bx-", 10) 291 | 292 | for i in range(numberOfPlots): 293 | legend = subplots[i].legend( 294 | loc="upper right", shadow=True, fontsize="x-small" 295 | ) 296 | 297 | p.show() 298 | 299 | def fill_plot_data(self, data, plot_samples, plot_data): 300 | if plot_samples is None or data.get_samples() < plot_samples: 301 | plot_samples = data.get_samples() 302 | plot_data.append(data) 303 | return (plot_samples, plot_data) 304 | 305 | def get_plot_data(self, args): 306 | for file in args.file: 307 | with open(file) as csvfile: 308 | data = csv_reader.CSVReader(csvfile, filter_running_program=args.filter) 309 | self.plot_samples, self.plot_data = self.fill_plot_data( 310 | data, self.plot_samples, self.plot_data 311 | ) 312 | 313 | 314 | if __name__ == "__main__": 315 | Plotter() 316 | -------------------------------------------------------------------------------- /examples/record.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2020-2022, Universal Robots A/S, 3 | # All rights reserved. 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # * Redistributions in binary form must reproduce the above copyright 9 | # notice, this list of conditions and the following disclaimer in the 10 | # documentation and/or other materials provided with the distribution. 11 | # * Neither the name of the Universal Robots A/S nor the names of its 12 | # contributors may be used to endorse or promote products derived 13 | # from this software without specific prior written permission. 14 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 18 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | 25 | import argparse 26 | import logging 27 | import sys 28 | 29 | sys.path.append("..") 30 | import rtde.rtde as rtde 31 | import rtde.rtde_config as rtde_config 32 | import rtde.csv_writer as csv_writer 33 | import rtde.csv_binary_writer as csv_binary_writer 34 | 35 | # parameters 36 | parser = argparse.ArgumentParser() 37 | parser.add_argument( 38 | "--host", default="localhost", help="name of host to connect to (localhost)" 39 | ) 40 | parser.add_argument("--port", type=int, default=30004, help="port number (30004)") 41 | parser.add_argument( 42 | "--samples", type=int, default=0, help="number of samples to record" 43 | ) 44 | parser.add_argument( 45 | "--frequency", type=int, default=125, help="the sampling frequency in Herz" 46 | ) 47 | parser.add_argument( 48 | "--config", 49 | default="record_configuration.xml", 50 | help="data configuration file to use (record_configuration.xml)", 51 | ) 52 | parser.add_argument( 53 | "--output", 54 | default="robot_data.csv", 55 | help="data output file to write to (robot_data.csv)", 56 | ) 57 | parser.add_argument("--verbose", help="increase output verbosity", action="store_true") 58 | parser.add_argument( 59 | "--buffered", 60 | help="Use buffered receive which doesn't skip data", 61 | action="store_true", 62 | ) 63 | parser.add_argument( 64 | "--binary", help="save the data in binary format", action="store_true" 65 | ) 66 | args = parser.parse_args() 67 | 68 | if args.verbose: 69 | logging.basicConfig(level=logging.INFO) 70 | 71 | conf = rtde_config.ConfigFile(args.config) 72 | output_names, output_types = conf.get_recipe("out") 73 | 74 | con = rtde.RTDE(args.host, args.port) 75 | con.connect() 76 | 77 | # get controller version 78 | con.get_controller_version() 79 | 80 | # setup recipes 81 | if not con.send_output_setup(output_names, output_types, frequency=args.frequency): 82 | logging.error("Unable to configure output") 83 | sys.exit() 84 | 85 | # start data synchronization 86 | if not con.send_start(): 87 | logging.error("Unable to start synchronization") 88 | sys.exit() 89 | 90 | writeModes = "wb" if args.binary else "w" 91 | with open(args.output, writeModes) as csvfile: 92 | writer = None 93 | 94 | if args.binary: 95 | writer = csv_binary_writer.CSVBinaryWriter(csvfile, output_names, output_types) 96 | else: 97 | writer = csv_writer.CSVWriter(csvfile, output_names, output_types) 98 | 99 | writer.writeheader() 100 | 101 | i = 1 102 | keep_running = True 103 | while keep_running: 104 | 105 | if i % args.frequency == 0: 106 | if args.samples > 0: 107 | sys.stdout.write("\r") 108 | sys.stdout.write("{:.2%} done.".format(float(i) / float(args.samples))) 109 | sys.stdout.flush() 110 | else: 111 | sys.stdout.write("\r") 112 | sys.stdout.write("{:3d} samples.".format(i)) 113 | sys.stdout.flush() 114 | if args.samples > 0 and i >= args.samples: 115 | keep_running = False 116 | try: 117 | if args.buffered: 118 | state = con.receive_buffered(args.binary) 119 | else: 120 | state = con.receive(args.binary) 121 | if state is not None: 122 | writer.writerow(state) 123 | i += 1 124 | 125 | except KeyboardInterrupt: 126 | keep_running = False 127 | except rtde.RTDEException: 128 | con.disconnect() 129 | sys.exit() 130 | 131 | 132 | sys.stdout.write("\rComplete! \n") 133 | 134 | con.send_pause() 135 | con.disconnect() 136 | -------------------------------------------------------------------------------- /examples/record_configuration.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 409 | 410 | 411 | -------------------------------------------------------------------------------- /examples/rtde_control_loop.urp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 83 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 283 | 284 | 285 | 286 | 325 | 326 | 327 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 405 | 446 | 487 | 528 | 569 | 610 | 611 | 612 | 613 | 614 | 615 | 616 | 617 | 618 | 628 | 629 | 630 | 631 | 632 | 633 | -------------------------------------------------------------------------------- /pom.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 4.0.0 7 | com.ur.rtde.client 8 | rtde 9 | 2.7.2 10 | pom 11 | 12 | 13 | UTF-8 14 | 15 | 16 | 17 | 18 | 19 | 20 | org.apache.maven.plugins 21 | maven-resources-plugin 22 | 2.7 23 | 24 | 25 | copy python files for release (goes to customers!) 26 | 27 | copy-resources 28 | 29 | generate-sources 30 | 31 | true 32 | ${project.basedir}/target/release 33 | 34 | 35 | ${project.basedir} 36 | 37 | rtde/** 38 | 39 | 40 | 41 | ${project.basedir} 42 | 43 | examples/** 44 | README.md 45 | LICENSE 46 | setup.py 47 | setup.cfg 48 | 49 | 50 | **/*pyc 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | org.apache.maven.plugins 62 | maven-assembly-plugin 63 | 64 | 65 | Create production / release zip 66 | package 67 | 68 | single 69 | 70 | 71 | true 72 | 73 | assembly-release.xml 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /rtde/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | from .rtde import * 25 | from .rtde_config import * 26 | -------------------------------------------------------------------------------- /rtde/csv_binary_writer.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import sys 25 | 26 | sys.path.append("..") 27 | 28 | import struct 29 | from rtde import serialize 30 | 31 | 32 | class CSVBinaryWriter(object): 33 | def __init__(self, file, names, types, delimiter=" "): 34 | if len(names) != len(types): 35 | raise ValueError("List sizes are not identical.") 36 | self.__file = file 37 | self.__names = names 38 | self.__types = types 39 | self.__delimiter = delimiter 40 | self.__header_names = [] 41 | self.__columns = 0 42 | for i in range(len(self.__names)): 43 | size = serialize.get_item_size(self.__types[i]) 44 | self.__columns += size 45 | if size > 1: 46 | for j in range(size): 47 | name = self.__names[i] + "_" + str(j) 48 | self.__header_names.append(name) 49 | else: 50 | name = self.__names[i] 51 | self.__header_names.append(name) 52 | 53 | def getType(self, vtype): 54 | if vtype == "VECTOR3D": 55 | return "DOUBLE" + self.__delimiter + "DOUBLE" + self.__delimiter + "DOUBLE" 56 | elif vtype == "VECTOR6D": 57 | return ( 58 | "DOUBLE" 59 | + self.__delimiter 60 | + "DOUBLE" 61 | + self.__delimiter 62 | + "DOUBLE" 63 | + self.__delimiter 64 | + "DOUBLE" 65 | + self.__delimiter 66 | + "DOUBLE" 67 | + self.__delimiter 68 | + "DOUBLE" 69 | ) 70 | elif vtype == "VECTOR6INT32": 71 | return ( 72 | "INT32" 73 | + self.__delimiter 74 | + "INT32" 75 | + self.__delimiter 76 | + "INT32" 77 | + self.__delimiter 78 | + "INT32" 79 | + self.__delimiter 80 | + "INT32" 81 | + self.__delimiter 82 | + "INT32" 83 | ) 84 | elif vtype == "VECTOR6UINT32": 85 | return ( 86 | "UINT32" 87 | + self.__delimiter 88 | + "UINT32" 89 | + self.__delimiter 90 | + "UINT32" 91 | + self.__delimiter 92 | + "UINT32" 93 | + self.__delimiter 94 | + "UINT32" 95 | + self.__delimiter 96 | + "UINT32" 97 | ) 98 | else: 99 | return str(vtype) 100 | 101 | def writeheader(self): 102 | # Header names 103 | headerStr = str("") 104 | for i in range(len(self.__header_names)): 105 | if i != 0: 106 | headerStr += self.__delimiter 107 | 108 | headerStr += self.__header_names[i] 109 | 110 | headerStr += "\n" 111 | self.__file.write(struct.pack(str(len(headerStr)) + "s", headerStr if sys.version_info[0] < 3 else headerStr.encode("utf-8"))) 112 | 113 | # Header types 114 | typeStr = str("") 115 | for i in range(len(self.__names)): 116 | if i != 0: 117 | typeStr += self.__delimiter 118 | 119 | typeStr += self.getType(self.__types[i]) 120 | 121 | typeStr += "\n" 122 | self.__file.write(struct.pack(str(len(typeStr)) + "s", typeStr if sys.version_info[0] < 3 else typeStr.encode("utf-8"))) 123 | 124 | def packToBinary(self, vtype, value): 125 | print(vtype) 126 | if vtype == "BOOL": 127 | print("isBOOL" + str(value)) 128 | if vtype == "UINT8": 129 | print("isUINT8" + str(value)) 130 | elif vtype == "INT32": 131 | print("isINT32" + str(value)) 132 | elif vtype == "INT64": 133 | print("isINT64" + str(value)) 134 | elif vtype == "UINT32": 135 | print("isUINT32" + str(value)) 136 | elif vtype == "UINT64": 137 | print("isUINT64" + str(value)) 138 | elif vtype == "DOUBLE": 139 | print( 140 | "isDOUBLE" + str(value) + str(type(value)) + str(sys.getsizeof(value)) 141 | ) 142 | elif vtype == "VECTOR3D": 143 | print( 144 | "isVECTOR3D" + str(value[0]) + "," + str(value[1]) + "," + str(value[2]) 145 | ) 146 | elif vtype == "VECTOR6D": 147 | print( 148 | "isVECTOR6D" 149 | + str(value[0]) 150 | + "," 151 | + str(value[1]) 152 | + "," 153 | + str(value[2]) 154 | + "," 155 | + str(value[3]) 156 | + "," 157 | + str(value[4]) 158 | + "," 159 | + str(value[5]) 160 | ) 161 | elif vtype == "VECTOR6INT32": 162 | print( 163 | "isVECTOR6INT32" 164 | + str(value[0]) 165 | + "," 166 | + str(value[1]) 167 | + "," 168 | + str(value[2]) 169 | + "," 170 | + str(value[3]) 171 | + "," 172 | + str(value[4]) 173 | + "," 174 | + str(value[5]) 175 | ) 176 | elif vtype == "VECTOR6UINT32": 177 | print( 178 | "isVECTOR6UINT32" 179 | + str(value[0]) 180 | + "," 181 | + str(value[1]) 182 | + "," 183 | + str(value[2]) 184 | + "," 185 | + str(value[3]) 186 | + "," 187 | + str(value[4]) 188 | + "," 189 | + str(value[5]) 190 | ) 191 | 192 | def writerow(self, data_object): 193 | self.__file.write(data_object) 194 | -------------------------------------------------------------------------------- /rtde/csv_reader.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import csv 25 | import numpy as np 26 | import logging 27 | 28 | from .rtde import LOGNAME 29 | 30 | _log = logging.getLogger(LOGNAME) 31 | 32 | 33 | runtime_state = "runtime_state" 34 | runtime_state_running = "2" 35 | 36 | 37 | class CSVReader(object): 38 | __samples = None 39 | __filename = None 40 | 41 | def get_header_data(self, __reader): 42 | header = next(__reader) 43 | return header 44 | 45 | def __init__(self, csvfile, delimiter=" ", filter_running_program=False): 46 | self.__filename = csvfile.name 47 | 48 | csvfile = [ 49 | csvfile for csvfile in csvfile.readlines() if csvfile.strip() 50 | ] # remove any empty lines 51 | 52 | reader = csv.reader(csvfile, delimiter=delimiter) 53 | header = self.get_header_data(reader) 54 | 55 | # read csv file 56 | data = [row for row in reader] 57 | 58 | if len(data) == 0: 59 | _log.warn("No data read from file: " + self.__filename) 60 | 61 | # filter data 62 | if filter_running_program: 63 | if runtime_state not in header: 64 | _log.warn( 65 | "Unable to filter data since runtime_state field is missing in data set" 66 | ) 67 | else: 68 | idx = header.index(runtime_state) 69 | data = [row for row in data if row[idx] == runtime_state_running] 70 | 71 | self.__samples = len(data) 72 | 73 | if self.__samples == 0: 74 | _log.warn("No data left from file: " + self.__filename + " after filtering") 75 | 76 | # transpose data 77 | data = list(zip(*data)) 78 | 79 | # create dictionary from header elements (keys) to float arrays 80 | self.__dict__.update( 81 | { 82 | header[i]: np.array(list(map(float, data[:][i]))) 83 | for i in range(len(header)) 84 | } 85 | ) 86 | 87 | def get_samples(self): 88 | return self.__samples 89 | 90 | def get_name(self): 91 | return self.__filename 92 | -------------------------------------------------------------------------------- /rtde/csv_writer.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import csv 25 | 26 | import sys 27 | 28 | sys.path.append("..") 29 | 30 | from rtde import serialize 31 | 32 | 33 | class CSVWriter(object): 34 | def __init__(self, csvfile, names, types, delimiter=" "): 35 | if len(names) != len(types): 36 | raise ValueError("List sizes are not identical.") 37 | self.__names = names 38 | self.__types = types 39 | self.__header_names = [] 40 | self.__columns = 0 41 | for i in range(len(self.__names)): 42 | size = serialize.get_item_size(self.__types[i]) 43 | self.__columns += size 44 | if size > 1: 45 | for j in range(size): 46 | name = self.__names[i] + "_" + str(j) 47 | self.__header_names.append(name) 48 | else: 49 | name = self.__names[i] 50 | self.__header_names.append(name) 51 | self.__writer = csv.writer(csvfile, delimiter=delimiter) 52 | 53 | def writeheader(self): 54 | self.__writer.writerow(self.__header_names) 55 | 56 | def writerow(self, data_object): 57 | data = [] 58 | for i in range(len(self.__names)): 59 | size = serialize.get_item_size(self.__types[i]) 60 | value = data_object.__dict__[self.__names[i]] 61 | if size > 1: 62 | data.extend(value) 63 | else: 64 | data.append(value) 65 | self.__writer.writerow(data) 66 | -------------------------------------------------------------------------------- /rtde/rtde.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import struct 25 | import socket 26 | import select 27 | import sys 28 | import logging 29 | 30 | if sys.version_info[0] < 3: 31 | import serialize 32 | else: 33 | from rtde import serialize 34 | 35 | DEFAULT_TIMEOUT = 1.0 36 | 37 | LOGNAME = "rtde" 38 | _log = logging.getLogger(LOGNAME) 39 | 40 | 41 | class Command: 42 | RTDE_REQUEST_PROTOCOL_VERSION = 86 # ascii V 43 | RTDE_GET_URCONTROL_VERSION = 118 # ascii v 44 | RTDE_TEXT_MESSAGE = 77 # ascii M 45 | RTDE_DATA_PACKAGE = 85 # ascii U 46 | RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS = 79 # ascii O 47 | RTDE_CONTROL_PACKAGE_SETUP_INPUTS = 73 # ascii I 48 | RTDE_CONTROL_PACKAGE_START = 83 # ascii S 49 | RTDE_CONTROL_PACKAGE_PAUSE = 80 # ascii P 50 | 51 | 52 | RTDE_PROTOCOL_VERSION_1 = 1 53 | RTDE_PROTOCOL_VERSION_2 = 2 54 | 55 | 56 | class ConnectionState: 57 | DISCONNECTED = 0 58 | CONNECTED = 1 59 | STARTED = 2 60 | PAUSED = 3 61 | 62 | 63 | class RTDEException(Exception): 64 | def __init__(self, msg): 65 | self.msg = msg 66 | 67 | def __str__(self): 68 | return repr(self.msg) 69 | 70 | 71 | class RTDETimeoutException(RTDEException): 72 | def __init__(self, msg): 73 | super(RTDETimeoutException, self).__init__(msg) 74 | 75 | 76 | class RTDE(object): 77 | def __init__(self, hostname, port=30004): 78 | self.hostname = hostname 79 | self.port = port 80 | self.__conn_state = ConnectionState.DISCONNECTED 81 | self.__sock = None 82 | self.__output_config = None 83 | self.__input_config = {} 84 | self.__skipped_package_count = 0 85 | self.__protocolVersion = RTDE_PROTOCOL_VERSION_1 86 | 87 | def connect(self): 88 | if self.__sock: 89 | return 90 | 91 | self.__buf = b"" # buffer data in binary format 92 | try: 93 | self.__sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 94 | self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 95 | self.__sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) 96 | self.__sock.settimeout(DEFAULT_TIMEOUT) 97 | self.__skipped_package_count = 0 98 | self.__sock.connect((self.hostname, self.port)) 99 | self.__conn_state = ConnectionState.CONNECTED 100 | except (socket.timeout, socket.error): 101 | self.__sock = None 102 | raise 103 | if not self.negotiate_protocol_version(): 104 | raise RTDEException("Unable to negotiate protocol version") 105 | 106 | def disconnect(self): 107 | if self.__sock: 108 | self.__sock.close() 109 | self.__sock = None 110 | self.__conn_state = ConnectionState.DISCONNECTED 111 | 112 | def is_connected(self): 113 | return self.__conn_state is not ConnectionState.DISCONNECTED 114 | 115 | def get_controller_version(self): 116 | cmd = Command.RTDE_GET_URCONTROL_VERSION 117 | version = self.__sendAndReceive(cmd) 118 | if version: 119 | _log.info( 120 | "Controller version: " 121 | + str(version.major) 122 | + "." 123 | + str(version.minor) 124 | + "." 125 | + str(version.bugfix) 126 | + "." 127 | + str(version.build) 128 | ) 129 | if version.major == 3 and version.minor <= 2 and version.bugfix < 19171: 130 | _log.error( 131 | "Please upgrade your controller to minimally version 3.2.19171" 132 | ) 133 | sys.exit() 134 | return version.major, version.minor, version.bugfix, version.build 135 | return None, None, None, None 136 | 137 | def negotiate_protocol_version(self): 138 | cmd = Command.RTDE_REQUEST_PROTOCOL_VERSION 139 | payload = struct.pack(">H", RTDE_PROTOCOL_VERSION_2) 140 | success = self.__sendAndReceive(cmd, payload) 141 | if success: 142 | self.__protocolVersion = RTDE_PROTOCOL_VERSION_2 143 | return success 144 | 145 | def send_input_setup(self, variables, types=[]): 146 | cmd = Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS 147 | payload = bytearray(",".join(variables), "utf-8") 148 | result = self.__sendAndReceive(cmd, payload) 149 | if len(types) != 0 and not self.__list_equals(result.types, types): 150 | _log.error( 151 | "Data type inconsistency for input setup: " 152 | + str(types) 153 | + " - " 154 | + str(result.types) 155 | ) 156 | return None 157 | result.names = variables 158 | self.__input_config[result.id] = result 159 | return serialize.DataObject.create_empty(variables, result.id) 160 | 161 | def send_output_setup(self, variables, types=[], frequency=125): 162 | cmd = Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS 163 | payload = struct.pack(">d", frequency) 164 | payload = payload + (",".join(variables).encode("utf-8")) 165 | result = self.__sendAndReceive(cmd, payload) 166 | if len(types) != 0 and not self.__list_equals(result.types, types): 167 | _log.error( 168 | "Data type inconsistency for output setup: " 169 | + str(types) 170 | + " - " 171 | + str(result.types) 172 | ) 173 | return False 174 | result.names = variables 175 | self.__output_config = result 176 | return True 177 | 178 | def send_start(self): 179 | cmd = Command.RTDE_CONTROL_PACKAGE_START 180 | success = self.__sendAndReceive(cmd) 181 | if success: 182 | _log.info("RTDE synchronization started") 183 | self.__conn_state = ConnectionState.STARTED 184 | else: 185 | _log.error("RTDE synchronization failed to start") 186 | return success 187 | 188 | def send_pause(self): 189 | cmd = Command.RTDE_CONTROL_PACKAGE_PAUSE 190 | success = self.__sendAndReceive(cmd) 191 | if success: 192 | _log.info("RTDE synchronization paused") 193 | self.__conn_state = ConnectionState.PAUSED 194 | else: 195 | _log.error("RTDE synchronization failed to pause") 196 | return success 197 | 198 | def send(self, input_data): 199 | if self.__conn_state != ConnectionState.STARTED: 200 | _log.error("Cannot send when RTDE synchronization is inactive") 201 | return 202 | if not input_data.recipe_id in self.__input_config: 203 | _log.error("Input configuration id not found: " + str(input_data.recipe_id)) 204 | return 205 | config = self.__input_config[input_data.recipe_id] 206 | return self.__sendall(Command.RTDE_DATA_PACKAGE, config.pack(input_data)) 207 | 208 | def receive(self, binary=False): 209 | """Recieve the latest data package. 210 | If muliple packages has been received, older ones are discarded 211 | and only the newest one will be returned. Will block untill a package 212 | is received or the connection is lost 213 | """ 214 | if self.__output_config is None: 215 | raise RTDEException("Output configuration not initialized") 216 | if self.__conn_state != ConnectionState.STARTED: 217 | raise RTDEException("Cannot receive when RTDE synchronization is inactive") 218 | return self.__recv(Command.RTDE_DATA_PACKAGE, binary) 219 | 220 | def receive_buffered(self, binary=False, buffer_limit=None): 221 | """Recieve the next data package. 222 | If muliple packages has been received they are buffered and will 223 | be returned on subsequent calls to this function. 224 | Returns None if no data is available. 225 | """ 226 | 227 | if self._RTDE__output_config is None: 228 | logging.error("Output configuration not initialized") 229 | return None 230 | 231 | try: 232 | while ( 233 | self.is_connected() 234 | and (buffer_limit == None or len(self.__buf) < buffer_limit) 235 | and self.__recv_to_buffer(0) 236 | ): 237 | pass 238 | except RTDEException as e: 239 | data = self.__recv_from_buffer(Command.RTDE_DATA_PACKAGE, binary) 240 | if data == None: 241 | raise e 242 | else: 243 | data = self.__recv_from_buffer(Command.RTDE_DATA_PACKAGE, binary) 244 | 245 | return data 246 | 247 | def send_message( 248 | self, message, source="Python Client", type=serialize.Message.INFO_MESSAGE 249 | ): 250 | cmd = Command.RTDE_TEXT_MESSAGE 251 | fmt = ">B%dsB%dsB" % (len(message), len(source)) 252 | payload = struct.pack(fmt, len(message), message, len(source), source, type) 253 | return self.__sendall(cmd, payload) 254 | 255 | def __on_packet(self, cmd, payload): 256 | if cmd == Command.RTDE_REQUEST_PROTOCOL_VERSION: 257 | return self.__unpack_protocol_version_package(payload) 258 | elif cmd == Command.RTDE_GET_URCONTROL_VERSION: 259 | return self.__unpack_urcontrol_version_package(payload) 260 | elif cmd == Command.RTDE_TEXT_MESSAGE: 261 | return self.__unpack_text_message(payload) 262 | elif cmd == Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: 263 | return self.__unpack_setup_outputs_package(payload) 264 | elif cmd == Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS: 265 | return self.__unpack_setup_inputs_package(payload) 266 | elif cmd == Command.RTDE_CONTROL_PACKAGE_START: 267 | return self.__unpack_start_package(payload) 268 | elif cmd == Command.RTDE_CONTROL_PACKAGE_PAUSE: 269 | return self.__unpack_pause_package(payload) 270 | elif cmd == Command.RTDE_DATA_PACKAGE: 271 | return self.__unpack_data_package(payload, self.__output_config) 272 | else: 273 | _log.error("Unknown package command: " + str(cmd)) 274 | 275 | def __sendAndReceive(self, cmd, payload=b""): 276 | if self.__sendall(cmd, payload): 277 | return self.__recv(cmd) 278 | else: 279 | return None 280 | 281 | def __sendall(self, command, payload=b""): 282 | fmt = ">HB" 283 | size = struct.calcsize(fmt) + len(payload) 284 | buf = struct.pack(fmt, size, command) + payload 285 | 286 | if self.__sock is None: 287 | _log.error("Unable to send: not connected to Robot") 288 | return False 289 | 290 | _, writable, _ = select.select([], [self.__sock], [], DEFAULT_TIMEOUT) 291 | if len(writable): 292 | self.__sock.sendall(buf) 293 | return True 294 | else: 295 | self.__trigger_disconnected() 296 | return False 297 | 298 | def has_data(self): 299 | timeout = 0 300 | readable, _, _ = select.select([self.__sock], [], [], timeout) 301 | return len(readable) != 0 302 | 303 | def __recv(self, command, binary=False): 304 | while self.is_connected(): 305 | try: 306 | self.__recv_to_buffer(DEFAULT_TIMEOUT) 307 | except RTDETimeoutException: 308 | return None 309 | 310 | # unpack_from requires a buffer of at least 3 bytes 311 | while len(self.__buf) >= 3: 312 | # Attempts to extract a packet 313 | packet_header = serialize.ControlHeader.unpack(self.__buf) 314 | 315 | if len(self.__buf) >= packet_header.size: 316 | packet, self.__buf = ( 317 | self.__buf[3 : packet_header.size], 318 | self.__buf[packet_header.size :], 319 | ) 320 | data = self.__on_packet(packet_header.command, packet) 321 | if len(self.__buf) >= 3 and command == Command.RTDE_DATA_PACKAGE: 322 | next_packet_header = serialize.ControlHeader.unpack(self.__buf) 323 | if next_packet_header.command == command: 324 | _log.debug("skipping package(1)") 325 | self.__skipped_package_count += 1 326 | continue 327 | if packet_header.command == command: 328 | if binary: 329 | return packet[1:] 330 | 331 | return data 332 | else: 333 | _log.debug("skipping package(2)") 334 | else: 335 | break 336 | raise RTDEException(" _recv() Connection lost ") 337 | 338 | def __recv_to_buffer(self, timeout): 339 | readable, _, xlist = select.select([self.__sock], [], [self.__sock], timeout) 340 | if len(readable): 341 | more = self.__sock.recv(4096) 342 | # When the controller stops while the script is running 343 | if len(more) == 0: 344 | _log.error( 345 | "received 0 bytes from Controller, probable cause: Controller has stopped" 346 | ) 347 | self.__trigger_disconnected() 348 | raise RTDEException("received 0 bytes from Controller") 349 | 350 | self.__buf = self.__buf + more 351 | return True 352 | 353 | if ( 354 | len(xlist) or len(readable) == 0 355 | ) and timeout != 0: # Effectively a timeout of timeout seconds 356 | _log.warning("no data received in last %d seconds ", timeout) 357 | raise RTDETimeoutException("no data received within timeout") 358 | 359 | return False 360 | 361 | def __recv_from_buffer(self, command, binary=False): 362 | # unpack_from requires a buffer of at least 3 bytes 363 | while len(self.__buf) >= 3: 364 | # Attempts to extract a packet 365 | packet_header = serialize.ControlHeader.unpack(self.__buf) 366 | 367 | if len(self.__buf) >= packet_header.size: 368 | packet, self.__buf = ( 369 | self.__buf[3 : packet_header.size], 370 | self.__buf[packet_header.size :], 371 | ) 372 | data = self.__on_packet(packet_header.command, packet) 373 | if packet_header.command == command: 374 | if binary: 375 | return packet[1:] 376 | 377 | return data 378 | else: 379 | _log.debug("skipping package(2)") 380 | else: 381 | return None 382 | 383 | def __trigger_disconnected(self): 384 | _log.info("RTDE disconnected") 385 | self.disconnect() # clean-up 386 | 387 | def __unpack_protocol_version_package(self, payload): 388 | if len(payload) != 1: 389 | _log.error("RTDE_REQUEST_PROTOCOL_VERSION: Wrong payload size") 390 | return None 391 | result = serialize.ReturnValue.unpack(payload) 392 | return result.success 393 | 394 | def __unpack_urcontrol_version_package(self, payload): 395 | if len(payload) != 16: 396 | _log.error("RTDE_GET_URCONTROL_VERSION: Wrong payload size") 397 | return None 398 | version = serialize.ControlVersion.unpack(payload) 399 | return version 400 | 401 | def __unpack_text_message(self, payload): 402 | if len(payload) < 1: 403 | _log.error("RTDE_TEXT_MESSAGE: No payload") 404 | return None 405 | if self.__protocolVersion == RTDE_PROTOCOL_VERSION_1: 406 | msg = serialize.MessageV1.unpack(payload) 407 | else: 408 | msg = serialize.Message.unpack(payload) 409 | 410 | if ( 411 | msg.level == serialize.Message.EXCEPTION_MESSAGE 412 | or msg.level == serialize.Message.ERROR_MESSAGE 413 | ): 414 | _log.error(msg.source + ": " + msg.message) 415 | elif msg.level == serialize.Message.WARNING_MESSAGE: 416 | _log.warning(msg.source + ": " + msg.message) 417 | elif msg.level == serialize.Message.INFO_MESSAGE: 418 | _log.info(msg.source + ": " + msg.message) 419 | 420 | def __unpack_setup_outputs_package(self, payload): 421 | if len(payload) < 1: 422 | _log.error("RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: No payload") 423 | return None 424 | output_config = serialize.DataConfig.unpack_recipe(payload) 425 | return output_config 426 | 427 | def __unpack_setup_inputs_package(self, payload): 428 | if len(payload) < 1: 429 | _log.error("RTDE_CONTROL_PACKAGE_SETUP_INPUTS: No payload") 430 | return None 431 | input_config = serialize.DataConfig.unpack_recipe(payload) 432 | return input_config 433 | 434 | def __unpack_start_package(self, payload): 435 | if len(payload) != 1: 436 | _log.error("RTDE_CONTROL_PACKAGE_START: Wrong payload size") 437 | return None 438 | result = serialize.ReturnValue.unpack(payload) 439 | return result.success 440 | 441 | def __unpack_pause_package(self, payload): 442 | if len(payload) != 1: 443 | _log.error("RTDE_CONTROL_PACKAGE_PAUSE: Wrong payload size") 444 | return None 445 | result = serialize.ReturnValue.unpack(payload) 446 | return result.success 447 | 448 | def __unpack_data_package(self, payload, output_config): 449 | if output_config is None: 450 | _log.error("RTDE_DATA_PACKAGE: Missing output configuration") 451 | return None 452 | output = output_config.unpack(payload) 453 | return output 454 | 455 | def __list_equals(self, l1, l2): 456 | if len(l1) != len(l2): 457 | return False 458 | for i in range(len((l1))): 459 | if l1[i] != l2[i]: 460 | return False 461 | return True 462 | 463 | @property 464 | def skipped_package_count(self): 465 | """The skipped package count, resets on connect""" 466 | return self.__skipped_package_count 467 | -------------------------------------------------------------------------------- /rtde/rtde_config.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import xml.etree.ElementTree as ET 25 | 26 | 27 | class Recipe(object): 28 | __slots__ = ["key", "names", "types"] 29 | 30 | @staticmethod 31 | def parse(recipe_node): 32 | rmd = Recipe() 33 | rmd.key = recipe_node.get("key") 34 | rmd.names = [f.get("name") for f in recipe_node.findall("field")] 35 | rmd.types = [f.get("type") for f in recipe_node.findall("field")] 36 | return rmd 37 | 38 | 39 | class ConfigFile(object): 40 | def __init__(self, filename): 41 | self.__filename = filename 42 | tree = ET.parse(self.__filename) 43 | root = tree.getroot() 44 | recipes = [Recipe.parse(r) for r in root.findall("recipe")] 45 | self.__dictionary = dict() 46 | for r in recipes: 47 | self.__dictionary[r.key] = r 48 | 49 | def get_recipe(self, key): 50 | r = self.__dictionary[key] 51 | return r.names, r.types 52 | -------------------------------------------------------------------------------- /rtde/serialize.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import struct 25 | 26 | 27 | class ControlHeader(object): 28 | __slots__ = [ 29 | "command", 30 | "size", 31 | ] 32 | 33 | @staticmethod 34 | def unpack(buf): 35 | rmd = ControlHeader() 36 | (rmd.size, rmd.command) = struct.unpack_from(">HB", buf) 37 | return rmd 38 | 39 | 40 | class ControlVersion(object): 41 | __slots__ = ["major", "minor", "bugfix", "build"] 42 | 43 | @staticmethod 44 | def unpack(buf): 45 | rmd = ControlVersion() 46 | (rmd.major, rmd.minor, rmd.bugfix, rmd.build) = struct.unpack_from(">IIII", buf) 47 | return rmd 48 | 49 | 50 | class ReturnValue(object): 51 | __slots__ = ["success"] 52 | 53 | @staticmethod 54 | def unpack(buf): 55 | rmd = ReturnValue() 56 | rmd.success = bool(struct.unpack_from(">B", buf)[0]) 57 | return rmd 58 | 59 | 60 | class MessageV1(object): 61 | @staticmethod 62 | def unpack(buf): 63 | rmd = Message() # use V2 message object 64 | offset = 0 65 | rmd.level = struct.unpack_from(">B", buf, offset)[0] 66 | offset = offset + 1 67 | rmd.message = str(buf[offset:]) 68 | rmd.source = "" 69 | 70 | return rmd 71 | 72 | 73 | class Message(object): 74 | __slots__ = ["level", "message", "source"] 75 | EXCEPTION_MESSAGE = 0 76 | ERROR_MESSAGE = 1 77 | WARNING_MESSAGE = 2 78 | INFO_MESSAGE = 3 79 | 80 | @staticmethod 81 | def unpack(buf): 82 | rmd = Message() 83 | offset = 0 84 | msg_length = struct.unpack_from(">B", buf, offset)[0] 85 | offset = offset + 1 86 | rmd.message = str(buf[offset : offset + msg_length]) 87 | offset = offset + msg_length 88 | 89 | src_length = struct.unpack_from(">B", buf, offset)[0] 90 | offset = offset + 1 91 | rmd.source = str(buf[offset : offset + src_length]) 92 | offset = offset + src_length 93 | rmd.level = struct.unpack_from(">B", buf, offset)[0] 94 | 95 | return rmd 96 | 97 | 98 | def get_item_size(data_type): 99 | if data_type.startswith("VECTOR6"): 100 | return 6 101 | elif data_type.startswith("VECTOR3"): 102 | return 3 103 | return 1 104 | 105 | 106 | def unpack_field(data, offset, data_type): 107 | size = get_item_size(data_type) 108 | if data_type == "VECTOR6D" or data_type == "VECTOR3D": 109 | return [float(data[offset + i]) for i in range(size)] 110 | elif data_type == "VECTOR6UINT32": 111 | return [int(data[offset + i]) for i in range(size)] 112 | elif data_type == "DOUBLE": 113 | return float(data[offset]) 114 | elif data_type == "UINT32" or data_type == "UINT64": 115 | return int(data[offset]) 116 | elif data_type == "VECTOR6INT32": 117 | return [int(data[offset + i]) for i in range(size)] 118 | elif data_type == "INT32" or data_type == "UINT8": 119 | return int(data[offset]) 120 | elif data_type == "BOOL": 121 | return bool(data[offset]) 122 | raise ValueError("unpack_field: unknown data type: " + data_type) 123 | 124 | 125 | class DataObject(object): 126 | recipe_id = None 127 | 128 | def pack(self, names, types): 129 | if len(names) != len(types): 130 | raise ValueError("List sizes are not identical.") 131 | l = [] 132 | if self.recipe_id is not None: 133 | l.append(self.recipe_id) 134 | for i in range(len(names)): 135 | if self.__dict__[names[i]] is None: 136 | raise ValueError("Uninitialized parameter: " + names[i]) 137 | if types[i].startswith("VECTOR"): 138 | l.extend(self.__dict__[names[i]]) 139 | else: 140 | l.append(self.__dict__[names[i]]) 141 | return l 142 | 143 | @staticmethod 144 | def unpack(data, names, types): 145 | if len(names) != len(types): 146 | raise ValueError("List sizes are not identical.") 147 | obj = DataObject() 148 | offset = 0 149 | obj.recipe_id = data[0] 150 | for i in range(len(names)): 151 | obj.__dict__[names[i]] = unpack_field(data[1:], offset, types[i]) 152 | offset += get_item_size(types[i]) 153 | return obj 154 | 155 | @staticmethod 156 | def create_empty(names, recipe_id): 157 | obj = DataObject() 158 | for i in range(len(names)): 159 | obj.__dict__[names[i]] = None 160 | obj.recipe_id = recipe_id 161 | return obj 162 | 163 | 164 | class DataConfig(object): 165 | __slots__ = ["id", "names", "types", "fmt"] 166 | 167 | @staticmethod 168 | def unpack_recipe(buf): 169 | rmd = DataConfig() 170 | rmd.id = struct.unpack_from(">B", buf)[0] 171 | rmd.types = buf.decode("utf-8")[1:].split(",") 172 | rmd.fmt = ">B" 173 | for i in rmd.types: 174 | if i == "INT32": 175 | rmd.fmt += "i" 176 | elif i == "UINT32": 177 | rmd.fmt += "I" 178 | elif i == "VECTOR6D": 179 | rmd.fmt += "d" * 6 180 | elif i == "VECTOR3D": 181 | rmd.fmt += "d" * 3 182 | elif i == "VECTOR6INT32": 183 | rmd.fmt += "i" * 6 184 | elif i == "VECTOR6UINT32": 185 | rmd.fmt += "I" * 6 186 | elif i == "DOUBLE": 187 | rmd.fmt += "d" 188 | elif i == "UINT64": 189 | rmd.fmt += "Q" 190 | elif i == "UINT8": 191 | rmd.fmt += "B" 192 | elif i == "BOOL": 193 | rmd.fmt += "?" 194 | elif i == "IN_USE": 195 | raise ValueError("An input parameter is already in use.") 196 | else: 197 | raise ValueError("Unknown data type: " + i) 198 | return rmd 199 | 200 | def pack(self, state): 201 | l = state.pack(self.names, self.types) 202 | return struct.pack(self.fmt, *l) 203 | 204 | def unpack(self, data): 205 | li = struct.unpack_from(self.fmt, data) 206 | return DataObject.unpack(li, self.names, self.types) 207 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [bdist_wheel] 2 | universal=1 3 | 4 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Copyright (c) 2016-2022, Universal Robots A/S, 3 | # All rights reserved. 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # * Redistributions in binary form must reproduce the above copyright 9 | # notice, this list of conditions and the following disclaimer in the 10 | # documentation and/or other materials provided with the distribution. 11 | # * Neither the name of the Universal Robots A/S nor the names of its 12 | # contributors may be used to endorse or promote products derived 13 | # from this software without specific prior written permission. 14 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 18 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | from setuptools import setup 25 | 26 | setup( 27 | name="UrRtde", 28 | packages=["rtde"], 29 | version="2.7.2", 30 | description="Real-Time Data Exchange (RTDE) python client + examples", 31 | ) 32 | --------------------------------------------------------------------------------