├── .gitignore ├── COPYING ├── README.md ├── debian ├── compat ├── control ├── python-urx.pyinstall ├── python3-urx.install └── rules ├── docs ├── Makefile ├── conf.py ├── index.rst ├── robot.rst └── urrobot.rst ├── examples ├── get_realtime_data.py ├── get_robot.py ├── joystick_control.py ├── matrices.py ├── simple.py ├── spnav_control.py ├── test_all.py └── test_movep.py ├── make_deb.py ├── make_windows_installer.bat ├── release.py ├── setup.py ├── tools ├── fakerobot.py ├── find_packet.py ├── get_rob.py ├── grabber.py └── packet.bin └── urx ├── __init__.py ├── robot.py ├── robotiq_two_finger_gripper.py ├── urrobot.py ├── urrtmon.py ├── urscript.py └── ursecmon.py /.gitignore: -------------------------------------------------------------------------------- 1 | docs/_* 2 | build 3 | .idea/ 4 | .history/ 5 | *.pyc 6 | -------------------------------------------------------------------------------- /COPYING: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | urx is a python library to control the robots from [Universal Robots](https://www.universal-robots.com/). It is published under the LGPL license and comes with absolutely no guarantee. 2 | 3 | It is meant as an easy to use module for pick and place operations, although it has been used for welding and other sensor based applications that do not require high control frequency. 4 | 5 | Both the 'secondary port' interface and the real-time/matlab interface of the UR controller are used. urx can optionally use the [python-math3d](https://github.com/mortlind/pymath3d)(GPL) library to receive and send transformation matrices to the robot urx is known to work with all release robots from Universal Robot. 6 | 7 | urx was primarily developed by [Olivier Roulet-Dubonnet](https://github.com/oroulet) for [Sintef Raufoss Manufacturing](http://www.sintef.no/manufacturing/). 8 | 9 | 10 | 11 | # Install 12 | 13 | The easiest is probably to use pip: 14 | ``` 15 | pip install urx 16 | ``` 17 | 18 | 19 | # Example use: 20 | 21 | ```python 22 | import urx 23 | 24 | rob = urx.Robot("192.168.0.100") 25 | rob.set_tcp((0, 0, 0.1, 0, 0, 0)) 26 | rob.set_payload(2, (0, 0, 0.1)) 27 | sleep(0.2) #leave some time to robot to process the setup commands 28 | rob.movej((1, 2, 3, 4, 5, 6), a, v) 29 | rob.movel((x, y, z, rx, ry, rz), a, v) 30 | print "Current tool pose is: ", rob.getl() 31 | rob.movel((0.1, 0, 0, 0, 0, 0), a, v, relative=true) # move relative to current pose 32 | rob.translate((0.1, 0, 0), a, v) #move tool and keep orientation 33 | rob.stopj(a) 34 | 35 | rob.movel(x, y, z, rx, ry, rz), wait=False) 36 | while True : 37 | sleep(0.1) #sleep first since the robot may not have processed the command yet 38 | if rob.is_program_running(): 39 | break 40 | 41 | rob.movel(x, y, z, rx, ry, rz), wait=False) 42 | while rob.getForce() < 50: 43 | sleep(0.01) 44 | if not rob.is_program_running(): 45 | break 46 | rob.stopl() 47 | 48 | try: 49 | rob.movel((0,0,0.1,0,0,0), relative=True) 50 | except RobotError, ex: 51 | print("Robot could not execute move (emergency stop for example), do something", ex) 52 | ``` 53 | 54 | # Development using Transform objects from math3d library: 55 | 56 | ```python 57 | from urx import Robot 58 | import math3d as m3d 59 | 60 | robot = Robot("192.168.1.1") 61 | mytcp = m3d.Transform() # create a matrix for our tool tcp 62 | mytcp.pos.z = 0.18 63 | mytcp.orient.rotate_zb(pi/3) 64 | robot.set_tcp(mytcp) 65 | time.sleep(0.2) 66 | 67 | # get current pose, transform it and move robot to new pose 68 | trans = robot.get_pose() # get current transformation matrix (tool to base) 69 | trans.pos.z += 0.3 70 | trans.orient.rotate_yb(pi/2) 71 | robot.set_pose(trans, acc=0.5, vel=0.2) # apply the new pose 72 | 73 | 74 | #or only work with orientation part 75 | o = robot.get_orientation() 76 | o.rotate_yb(pi) 77 | robot.set_orientation(o) 78 | ``` 79 | 80 | # Other interactive methods/properties 81 | 82 | ```python 83 | 84 | from urx import Robot 85 | rob = Robot("192.168.1.1") 86 | rob.x # returns current x 87 | rob.rx # returns 0 (could return x component of axis vector, but it is not very usefull 88 | rob.rx -= 0.1 # rotate tool around X axis 89 | rob.z_t += 0.01 # move robot in tool z axis for +1cm 90 | 91 | csys = rob.new_csys_from_xpy() # generate a new csys from 3 points: X, origin, Y 92 | rob.set_csys(csys) 93 | ``` 94 | 95 | 96 | # Robotiq Gripper 97 | 98 | urx can also control a Robotiq gripper attached to the UR robot. The robotiq class was primarily developed by [Mark Silliman](https://github.com/markwsilliman). 99 | 100 | ## Example use: 101 | 102 | ```python 103 | import sys 104 | import urx 105 | from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper 106 | 107 | if __name__ == '__main__': 108 | rob = urx.Robot("192.168.0.100") 109 | robotiqgrip = Robotiq_Two_Finger_Gripper() 110 | 111 | if(len(sys.argv) != 2): 112 | print "false" 113 | sys.exit() 114 | 115 | if(sys.argv[1] == "close") : 116 | robotiqgrip.close_gripper() 117 | if(sys.argv[1] == "open") : 118 | robotiqgrip.open_gripper() 119 | 120 | rob.send_program(robotiqgrip.ret_program_to_run()) 121 | 122 | rob.close() 123 | print "true" 124 | sys.exit() 125 | ``` 126 | -------------------------------------------------------------------------------- /debian/compat: -------------------------------------------------------------------------------- 1 | 9 2 | -------------------------------------------------------------------------------- /debian/control: -------------------------------------------------------------------------------- 1 | Source: python-urx 2 | Section: python 3 | Priority: extra 4 | Maintainer: Olivier Roulet-Dubonnet 5 | Build-Depends: debhelper (>= 8.0.0) 6 | Standards-Version: 3.9.3 7 | Homepage: http://launchpad.net/XXX 8 | X-Python-Version: >= 2.6 9 | X-Python3-Version: >= 3.1 10 | 11 | Package: python-urx 12 | Architecture: all 13 | Depends: ${python:Depends}, ${misc:Depends} 14 | Recommends: python-math3d 15 | Description: Python library to control a robot from Universal Robots 16 | 17 | Package: python3-urx 18 | Architecture: all 19 | Depends: ${python3:Depends}, ${misc:Depends} 20 | Recommends: python3-math3d 21 | Description: Python library to control a robot from Universal Robots 22 | -------------------------------------------------------------------------------- /debian/python-urx.pyinstall: -------------------------------------------------------------------------------- 1 | urx/*.py 2 | -------------------------------------------------------------------------------- /debian/python3-urx.install: -------------------------------------------------------------------------------- 1 | urx/*.py /usr/lib/python3/dist-packages/urx 2 | -------------------------------------------------------------------------------- /debian/rules: -------------------------------------------------------------------------------- 1 | #!/usr/bin/make -f 2 | # -*- makefile -*- 3 | # Sample debian/rules that uses debhelper. 4 | # This file was originally written by Joey Hess and Craig Small. 5 | # As a special exception, when this file is copied by dh-make into a 6 | # dh-make output file, you may use that output file without restriction. 7 | # This special exception was added by Craig Small in version 0.37 of dh-make. 8 | 9 | # Uncomment this to turn on verbose mode. 10 | export DH_VERBOSE=1 11 | 12 | %: 13 | dh $@ --with python2,python3 14 | 15 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | PAPER = 8 | BUILDDIR = _build 9 | 10 | # User-friendly check for sphinx-build 11 | ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1) 12 | $(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/) 13 | endif 14 | 15 | # Internal variables. 16 | PAPEROPT_a4 = -D latex_paper_size=a4 17 | PAPEROPT_letter = -D latex_paper_size=letter 18 | ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . 19 | # the i18n builder cannot share the environment and doctrees with the others 20 | I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . 21 | 22 | .PHONY: help clean html dirhtml singlehtml pickle json htmlhelp qthelp devhelp epub latex latexpdf text man changes linkcheck doctest coverage gettext 23 | 24 | help: 25 | @echo "Please use \`make ' where is one of" 26 | @echo " html to make standalone HTML files" 27 | @echo " dirhtml to make HTML files named index.html in directories" 28 | @echo " singlehtml to make a single large HTML file" 29 | @echo " pickle to make pickle files" 30 | @echo " json to make JSON files" 31 | @echo " htmlhelp to make HTML files and a HTML help project" 32 | @echo " qthelp to make HTML files and a qthelp project" 33 | @echo " applehelp to make an Apple Help Book" 34 | @echo " devhelp to make HTML files and a Devhelp project" 35 | @echo " epub to make an epub" 36 | @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" 37 | @echo " latexpdf to make LaTeX files and run them through pdflatex" 38 | @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" 39 | @echo " text to make text files" 40 | @echo " man to make manual pages" 41 | @echo " texinfo to make Texinfo files" 42 | @echo " info to make Texinfo files and run them through makeinfo" 43 | @echo " gettext to make PO message catalogs" 44 | @echo " changes to make an overview of all changed/added/deprecated items" 45 | @echo " xml to make Docutils-native XML files" 46 | @echo " pseudoxml to make pseudoxml-XML files for display purposes" 47 | @echo " linkcheck to check all external links for integrity" 48 | @echo " doctest to run all doctests embedded in the documentation (if enabled)" 49 | @echo " coverage to run coverage check of the documentation (if enabled)" 50 | 51 | clean: 52 | rm -rf $(BUILDDIR)/* 53 | 54 | html: 55 | $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html 56 | @echo 57 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." 58 | 59 | dirhtml: 60 | $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml 61 | @echo 62 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." 63 | 64 | singlehtml: 65 | $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml 66 | @echo 67 | @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." 68 | 69 | pickle: 70 | $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle 71 | @echo 72 | @echo "Build finished; now you can process the pickle files." 73 | 74 | json: 75 | $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json 76 | @echo 77 | @echo "Build finished; now you can process the JSON files." 78 | 79 | htmlhelp: 80 | $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp 81 | @echo 82 | @echo "Build finished; now you can run HTML Help Workshop with the" \ 83 | ".hhp project file in $(BUILDDIR)/htmlhelp." 84 | 85 | qthelp: 86 | $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp 87 | @echo 88 | @echo "Build finished; now you can run "qcollectiongenerator" with the" \ 89 | ".qhcp project file in $(BUILDDIR)/qthelp, like this:" 90 | @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/PythonURx.qhcp" 91 | @echo "To view the help file:" 92 | @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/PythonURx.qhc" 93 | 94 | applehelp: 95 | $(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp 96 | @echo 97 | @echo "Build finished. The help book is in $(BUILDDIR)/applehelp." 98 | @echo "N.B. You won't be able to view it unless you put it in" \ 99 | "~/Library/Documentation/Help or install it in your application" \ 100 | "bundle." 101 | 102 | devhelp: 103 | $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp 104 | @echo 105 | @echo "Build finished." 106 | @echo "To view the help file:" 107 | @echo "# mkdir -p $$HOME/.local/share/devhelp/PythonURx" 108 | @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/PythonURx" 109 | @echo "# devhelp" 110 | 111 | epub: 112 | $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub 113 | @echo 114 | @echo "Build finished. The epub file is in $(BUILDDIR)/epub." 115 | 116 | latex: 117 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 118 | @echo 119 | @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." 120 | @echo "Run \`make' in that directory to run these through (pdf)latex" \ 121 | "(use \`make latexpdf' here to do that automatically)." 122 | 123 | latexpdf: 124 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 125 | @echo "Running LaTeX files through pdflatex..." 126 | $(MAKE) -C $(BUILDDIR)/latex all-pdf 127 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 128 | 129 | latexpdfja: 130 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 131 | @echo "Running LaTeX files through platex and dvipdfmx..." 132 | $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja 133 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 134 | 135 | text: 136 | $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text 137 | @echo 138 | @echo "Build finished. The text files are in $(BUILDDIR)/text." 139 | 140 | man: 141 | $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man 142 | @echo 143 | @echo "Build finished. The manual pages are in $(BUILDDIR)/man." 144 | 145 | texinfo: 146 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 147 | @echo 148 | @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." 149 | @echo "Run \`make' in that directory to run these through makeinfo" \ 150 | "(use \`make info' here to do that automatically)." 151 | 152 | info: 153 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 154 | @echo "Running Texinfo files through makeinfo..." 155 | make -C $(BUILDDIR)/texinfo info 156 | @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." 157 | 158 | gettext: 159 | $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale 160 | @echo 161 | @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." 162 | 163 | changes: 164 | $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes 165 | @echo 166 | @echo "The overview file is in $(BUILDDIR)/changes." 167 | 168 | linkcheck: 169 | $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck 170 | @echo 171 | @echo "Link check complete; look for any errors in the above output " \ 172 | "or in $(BUILDDIR)/linkcheck/output.txt." 173 | 174 | doctest: 175 | $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest 176 | @echo "Testing of doctests in the sources finished, look at the " \ 177 | "results in $(BUILDDIR)/doctest/output.txt." 178 | 179 | coverage: 180 | $(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage 181 | @echo "Testing of coverage in the sources finished, look at the " \ 182 | "results in $(BUILDDIR)/coverage/python.txt." 183 | 184 | xml: 185 | $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml 186 | @echo 187 | @echo "Build finished. The XML files are in $(BUILDDIR)/xml." 188 | 189 | pseudoxml: 190 | $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml 191 | @echo 192 | @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." 193 | -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # Python URx documentation build configuration file, created by 4 | # sphinx-quickstart on Mon May 11 21:37:43 2015. 5 | # 6 | # This file is execfile()d with the current directory set to its 7 | # containing dir. 8 | # 9 | # Note that not all possible configuration values are present in this 10 | # autogenerated file. 11 | # 12 | # All configuration values have a default; values that are commented out 13 | # serve to show the default. 14 | 15 | import sys 16 | import os 17 | import shlex 18 | 19 | # If extensions (or modules to document with autodoc) are in another directory, 20 | # add these directories to sys.path here. If the directory is relative to the 21 | # documentation root, use os.path.abspath to make it absolute, like shown here. 22 | #sys.path.insert(0, os.path.abspath('.')) 23 | 24 | # -- General configuration ------------------------------------------------ 25 | 26 | # If your documentation needs a minimal Sphinx version, state it here. 27 | #needs_sphinx = '1.0' 28 | 29 | # Add any Sphinx extension module names here, as strings. They can be 30 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 31 | # ones. 32 | extensions = [ 33 | 'sphinx.ext.autodoc', 34 | 'sphinx.ext.coverage', 35 | 'sphinx.ext.viewcode', 36 | ] 37 | 38 | # Add any paths that contain templates here, relative to this directory. 39 | templates_path = ['_templates'] 40 | 41 | # The suffix(es) of source filenames. 42 | # You can specify multiple suffix as a list of string: 43 | # source_suffix = ['.rst', '.md'] 44 | source_suffix = '.rst' 45 | 46 | # The encoding of source files. 47 | #source_encoding = 'utf-8-sig' 48 | 49 | # The master toctree document. 50 | master_doc = 'index' 51 | 52 | # General information about the project. 53 | project = u'Python URx' 54 | copyright = u'2015, Olivier Roulet-Dubonnet' 55 | author = u'Olivier Roulet-Dubonnet' 56 | 57 | # The version info for the project you're documenting, acts as replacement for 58 | # |version| and |release|, also used in various other places throughout the 59 | # built documents. 60 | # 61 | # The short X.Y version. 62 | version = '1.0' 63 | # The full version, including alpha/beta/rc tags. 64 | release = '1.0' 65 | 66 | # The language for content autogenerated by Sphinx. Refer to documentation 67 | # for a list of supported languages. 68 | # 69 | # This is also used if you do content translation via gettext catalogs. 70 | # Usually you set "language" from the command line for these cases. 71 | language = None 72 | 73 | # There are two options for replacing |today|: either, you set today to some 74 | # non-false value, then it is used: 75 | #today = '' 76 | # Else, today_fmt is used as the format for a strftime call. 77 | #today_fmt = '%B %d, %Y' 78 | 79 | # List of patterns, relative to source directory, that match files and 80 | # directories to ignore when looking for source files. 81 | exclude_patterns = ['_build'] 82 | 83 | # The reST default role (used for this markup: `text`) to use for all 84 | # documents. 85 | #default_role = None 86 | 87 | # If true, '()' will be appended to :func: etc. cross-reference text. 88 | #add_function_parentheses = True 89 | 90 | # If true, the current module name will be prepended to all description 91 | # unit titles (such as .. function::). 92 | #add_module_names = True 93 | 94 | # If true, sectionauthor and moduleauthor directives will be shown in the 95 | # output. They are ignored by default. 96 | #show_authors = False 97 | 98 | # The name of the Pygments (syntax highlighting) style to use. 99 | pygments_style = 'sphinx' 100 | 101 | # A list of ignored prefixes for module index sorting. 102 | #modindex_common_prefix = [] 103 | 104 | # If true, keep warnings as "system message" paragraphs in the built documents. 105 | #keep_warnings = False 106 | 107 | # If true, `todo` and `todoList` produce output, else they produce nothing. 108 | todo_include_todos = False 109 | 110 | 111 | # -- Options for HTML output ---------------------------------------------- 112 | 113 | # The theme to use for HTML and HTML Help pages. See the documentation for 114 | # a list of builtin themes. 115 | html_theme = 'alabaster' 116 | 117 | # Theme options are theme-specific and customize the look and feel of a theme 118 | # further. For a list of options available for each theme, see the 119 | # documentation. 120 | #html_theme_options = {} 121 | 122 | # Add any paths that contain custom themes here, relative to this directory. 123 | #html_theme_path = [] 124 | 125 | # The name for this set of Sphinx documents. If None, it defaults to 126 | # " v documentation". 127 | #html_title = None 128 | 129 | # A shorter title for the navigation bar. Default is the same as html_title. 130 | #html_short_title = None 131 | 132 | # The name of an image file (relative to this directory) to place at the top 133 | # of the sidebar. 134 | #html_logo = None 135 | 136 | # The name of an image file (within the static path) to use as favicon of the 137 | # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 138 | # pixels large. 139 | #html_favicon = None 140 | 141 | # Add any paths that contain custom static files (such as style sheets) here, 142 | # relative to this directory. They are copied after the builtin static files, 143 | # so a file named "default.css" will overwrite the builtin "default.css". 144 | html_static_path = ['_static'] 145 | 146 | # Add any extra paths that contain custom files (such as robots.txt or 147 | # .htaccess) here, relative to this directory. These files are copied 148 | # directly to the root of the documentation. 149 | #html_extra_path = [] 150 | 151 | # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, 152 | # using the given strftime format. 153 | #html_last_updated_fmt = '%b %d, %Y' 154 | 155 | # If true, SmartyPants will be used to convert quotes and dashes to 156 | # typographically correct entities. 157 | #html_use_smartypants = True 158 | 159 | # Custom sidebar templates, maps document names to template names. 160 | #html_sidebars = {} 161 | 162 | # Additional templates that should be rendered to pages, maps page names to 163 | # template names. 164 | #html_additional_pages = {} 165 | 166 | # If false, no module index is generated. 167 | #html_domain_indices = True 168 | 169 | # If false, no index is generated. 170 | #html_use_index = True 171 | 172 | # If true, the index is split into individual pages for each letter. 173 | #html_split_index = False 174 | 175 | # If true, links to the reST sources are added to the pages. 176 | #html_show_sourcelink = True 177 | 178 | # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. 179 | #html_show_sphinx = True 180 | 181 | # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. 182 | #html_show_copyright = True 183 | 184 | # If true, an OpenSearch description file will be output, and all pages will 185 | # contain a tag referring to it. The value of this option must be the 186 | # base URL from which the finished HTML is served. 187 | #html_use_opensearch = '' 188 | 189 | # This is the file name suffix for HTML files (e.g. ".xhtml"). 190 | #html_file_suffix = None 191 | 192 | # Language to be used for generating the HTML full-text search index. 193 | # Sphinx supports the following languages: 194 | # 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja' 195 | # 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr' 196 | #html_search_language = 'en' 197 | 198 | # A dictionary with options for the search language support, empty by default. 199 | # Now only 'ja' uses this config value 200 | #html_search_options = {'type': 'default'} 201 | 202 | # The name of a javascript file (relative to the configuration directory) that 203 | # implements a search results scorer. If empty, the default will be used. 204 | #html_search_scorer = 'scorer.js' 205 | 206 | # Output file base name for HTML help builder. 207 | htmlhelp_basename = 'PythonURxdoc' 208 | 209 | # -- Options for LaTeX output --------------------------------------------- 210 | 211 | latex_elements = { 212 | # The paper size ('letterpaper' or 'a4paper'). 213 | #'papersize': 'letterpaper', 214 | 215 | # The font size ('10pt', '11pt' or '12pt'). 216 | #'pointsize': '10pt', 217 | 218 | # Additional stuff for the LaTeX preamble. 219 | #'preamble': '', 220 | 221 | # Latex figure (float) alignment 222 | #'figure_align': 'htbp', 223 | } 224 | 225 | # Grouping the document tree into LaTeX files. List of tuples 226 | # (source start file, target name, title, 227 | # author, documentclass [howto, manual, or own class]). 228 | latex_documents = [ 229 | (master_doc, 'PythonURx.tex', u'Python URx Documentation', 230 | u'Olivier Roulet-Dubonnet', 'manual'), 231 | ] 232 | 233 | # The name of an image file (relative to this directory) to place at the top of 234 | # the title page. 235 | #latex_logo = None 236 | 237 | # For "manual" documents, if this is true, then toplevel headings are parts, 238 | # not chapters. 239 | #latex_use_parts = False 240 | 241 | # If true, show page references after internal links. 242 | #latex_show_pagerefs = False 243 | 244 | # If true, show URL addresses after external links. 245 | #latex_show_urls = False 246 | 247 | # Documents to append as an appendix to all manuals. 248 | #latex_appendices = [] 249 | 250 | # If false, no module index is generated. 251 | #latex_domain_indices = True 252 | 253 | 254 | # -- Options for manual page output --------------------------------------- 255 | 256 | # One entry per manual page. List of tuples 257 | # (source start file, name, description, authors, manual section). 258 | man_pages = [ 259 | (master_doc, 'pythonurx', u'Python URx Documentation', 260 | [author], 1) 261 | ] 262 | 263 | # If true, show URL addresses after external links. 264 | #man_show_urls = False 265 | 266 | 267 | # -- Options for Texinfo output ------------------------------------------- 268 | 269 | # Grouping the document tree into Texinfo files. List of tuples 270 | # (source start file, target name, title, author, 271 | # dir menu entry, description, category) 272 | texinfo_documents = [ 273 | (master_doc, 'PythonURx', u'Python URx Documentation', 274 | author, 'PythonURx', 'One line description of project.', 275 | 'Miscellaneous'), 276 | ] 277 | 278 | # Documents to append as an appendix to all manuals. 279 | #texinfo_appendices = [] 280 | 281 | # If false, no module index is generated. 282 | #texinfo_domain_indices = True 283 | 284 | # How to display URL addresses: 'footnote', 'no', or 'inline'. 285 | #texinfo_show_urls = 'footnote' 286 | 287 | # If true, do not generate a @detailmenu in the "Top" node's menu. 288 | #texinfo_no_detailmenu = False 289 | -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | .. Python URx documentation master file, created by 2 | sphinx-quickstart on Mon May 11 21:37:43 2015. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Python URx Documentation 7 | ====================================== 8 | 9 | Contents: 10 | 11 | .. toctree:: 12 | :maxdepth: 2 13 | 14 | urrobot 15 | robot 16 | 17 | 18 | 19 | Indices and tables 20 | ================== 21 | 22 | * :ref:`genindex` 23 | * :ref:`modindex` 24 | * :ref:`search` 25 | 26 | -------------------------------------------------------------------------------- /docs/robot.rst: -------------------------------------------------------------------------------- 1 | 2 | Robot Object 3 | ========================================= 4 | 5 | .. autoclass:: urx.robot.Robot 6 | :members: 7 | :undoc-members: 8 | 9 | .. autoattribute: 10 | 11 | -------------------------------------------------------------------------------- /docs/urrobot.rst: -------------------------------------------------------------------------------- 1 | 2 | URRobot Object 3 | ========================================= 4 | 5 | .. autoclass:: urx.robot.URRobot 6 | :members: 7 | :undoc-members: 8 | 9 | .. autoattribute: 10 | 11 | -------------------------------------------------------------------------------- /examples/get_realtime_data.py: -------------------------------------------------------------------------------- 1 | import urx 2 | import time 3 | import logging 4 | 5 | r = urx.Robot("192.168.111.134", use_rt=True, urFirm=5.1) 6 | 7 | if __name__ == "__main__": 8 | logging.basicConfig(level=logging.INFO) 9 | while 1: 10 | try: 11 | j_temp = r.get_joint_temperature() 12 | j_voltage = r.get_joint_voltage() 13 | j_current = r.get_joint_current() 14 | main_voltage = r.get_main_voltage() 15 | robot_voltage = r.get_robot_voltage() 16 | robot_current = r.get_robot_current() 17 | 18 | print("JOINT TEMPERATURE") 19 | print(j_temp) 20 | 21 | print("JOINT VOLTAGE") 22 | print(j_voltage) 23 | 24 | print("JOINT CURRENT") 25 | print(j_current) 26 | 27 | print("MAIN VOLTAGE") 28 | print(main_voltage) 29 | 30 | print("ROBOT VOLTAGE") 31 | print(robot_voltage) 32 | 33 | print("ROBOT CURRENT") 34 | print(robot_current) 35 | 36 | print("##########\t##########\t##########\t##########") 37 | 38 | time.sleep(1) 39 | 40 | except: 41 | pass 42 | 43 | r.close() -------------------------------------------------------------------------------- /examples/get_robot.py: -------------------------------------------------------------------------------- 1 | import urx 2 | from IPython import embed 3 | import logging 4 | 5 | if __name__ == "__main__": 6 | try: 7 | logging.basicConfig(level=logging.INFO) 8 | #robot = urx.Robot("192.168.1.6") 9 | robot = urx.Robot("192.168.1.100") 10 | #robot = urx.Robot("localhost") 11 | r = robot 12 | print("Robot object is available as robot or r") 13 | embed() 14 | finally: 15 | robot.close() 16 | -------------------------------------------------------------------------------- /examples/joystick_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | example program to control a universal robot with a joystick 3 | All joysticks are differens, so you will need to modify the script to work with your joystick 4 | """ 5 | 6 | import time 7 | 8 | import pygame 9 | 10 | import math3d as m3d 11 | from math import pi 12 | 13 | import urx 14 | 15 | class Cmd(object): 16 | def __init__(self): 17 | self.reset() 18 | 19 | def reset(self): 20 | self.axis0 = 0 21 | self.axis1 = 0 22 | self.axis2 = 0 23 | self.axis3 = 0 24 | self.axis4 = 0 25 | self.axis5 = 0 26 | self.btn0 = 0 27 | self.btn1 = 0 28 | self.btn2 = 0 29 | self.btn3 = 0 30 | self.btn4 = 0 31 | self.btn5 = 0 32 | self.btn6 = 0 33 | self.btn7 = 0 34 | self.btn8 = 0 35 | self.btn9 = 0 36 | 37 | 38 | class Service(object): 39 | def __init__(self, robot, linear_velocity=0.1, rotational_velocity=0.1, acceleration=0.1): 40 | self.joystick = None 41 | self.robot = robot 42 | #max velocity and acceleration to be send to robot 43 | self.linear_velocity = linear_velocity 44 | self.rotational_velocity = rotational_velocity 45 | self.acceleration = acceleration 46 | #one button send the robot to a preprogram position defined by this variable in join space 47 | self.init_pose = [-2.0782002408411593, -1.6628931459654561, 2.067930303382134, -1.9172217394630149, 1.5489023943220621, 0.6783171005488982] 48 | 49 | self.cmd = Cmd() 50 | 51 | def init_joystick(self): 52 | pygame.init() 53 | self.joystick = pygame.joystick.Joystick(0) 54 | self.joystick.init() 55 | print('Initialized Joystick : %s' % self.joystick.get_name()) 56 | 57 | def loop(self): 58 | print("Starting loop") 59 | air = False 60 | while True: 61 | self.cmd.reset() 62 | pygame.event.pump()#Seems we need polling in pygame... 63 | 64 | #get joystick state 65 | for i in range(0, self.joystick.get_numaxes()): 66 | val = self.joystick.get_axis(i) 67 | if i in (2, 5) and val != 0: 68 | val += 1 69 | if abs(val) < 0.2: 70 | val = 0 71 | tmp = "self.cmd.axis" + str(i) + " = " + str(val) 72 | if val != 0: 73 | print(tmp) 74 | exec(tmp) 75 | 76 | #get button state 77 | for i in range(0, self.joystick.get_numbuttons()): 78 | if self.joystick.get_button(i) != 0: 79 | tmp = "self.cmd.btn" + str(i) + " = 1" 80 | print(tmp) 81 | exec(tmp) 82 | 83 | if self.cmd.btn0: 84 | #toggle IO 85 | air = not air 86 | self.robot.set_digital_out(2, air) 87 | 88 | if self.cmd.btn9: 89 | print("moving to init pose") 90 | self.robot.movej(self.init_pose, acc=1, vel=0.1) 91 | 92 | #initalize speed array to 0 93 | speeds = [0, 0, 0, 0, 0, 0] 94 | 95 | #get linear speed from joystick 96 | speeds[0] = -1 * self.cmd.axis0 * self.linear_velocity 97 | speeds[1] = self.cmd.axis1 * self.linear_velocity 98 | if self.cmd.btn4 and not self.cmd.axis2: 99 | speeds[2] = -self.linear_velocity 100 | if self.cmd.axis2 and not self.cmd.btn4: 101 | speeds[2] = self.cmd.axis2 * self.linear_velocity 102 | 103 | #get rotational speed from joystick 104 | speeds[4] = -1 * self.cmd.axis3 * self.rotational_velocity 105 | speeds[3] = -1 * self.cmd.axis4 * self.rotational_velocity 106 | if self.cmd.btn5 and not self.cmd.axis5: 107 | speeds[5] = self.rotational_velocity 108 | if self.cmd.axis5 and not self.cmd.btn5: 109 | speeds[5] = self.cmd.axis5 * -self.rotational_velocity 110 | 111 | #for some reasons everything is inversed 112 | speeds = [-i for i in speeds] 113 | #Now sending to robot. tol by default and base csys if btn2 is on 114 | if speeds != [0 for _ in speeds]: 115 | print("Sending ", speeds) 116 | if self.cmd.btn7: 117 | self.robot.speedl_tool(speeds, acc=self.acceleration, min_time=2) 118 | else: 119 | self.robot.speedl(speeds, acc=self.acceleration, min_time=2) 120 | 121 | 122 | def close(self): 123 | if self.joystick: 124 | self.joystick.quit() 125 | 126 | 127 | if __name__ == "__main__": 128 | robot = urx.Robot("192.168.1.100") 129 | r = robot 130 | 131 | 132 | #start joystick service with given max speed and acceleration 133 | service = Service(robot, linear_velocity=0.1, rotational_velocity=0.1, acceleration=0.1) 134 | service.init_joystick() 135 | try: 136 | service.loop() 137 | finally: 138 | robot.close() 139 | service.close() 140 | -------------------------------------------------------------------------------- /examples/matrices.py: -------------------------------------------------------------------------------- 1 | from math import pi 2 | 3 | import urx 4 | import logging 5 | 6 | if __name__ == "__main__": 7 | rob = urx.Robot("192.168.1.100") 8 | #rob = urx.Robot("localhost") 9 | rob.set_tcp((0,0,0,0,0,0)) 10 | rob.set_payload(0.5, (0,0,0)) 11 | try: 12 | l = 0.05 13 | v = 0.05 14 | a = 0.3 15 | j = rob.getj() 16 | print("Initial joint configuration is ", j) 17 | t = rob.get_pose() 18 | print("Transformation from base to tcp is: ", t) 19 | print("Translating in x") 20 | rob.translate((l, 0, 0), acc=a, vel=v) 21 | pose = rob.getl() 22 | print("robot tcp is at: ", pose) 23 | print("moving in z") 24 | pose[2] += l 25 | rob.movel(pose, acc=a, vel=v) 26 | 27 | 28 | print("Translate in -x and rotate") 29 | t.orient.rotate_zb(pi/4) 30 | t.pos[0] -= l 31 | rob.set_pose(t, vel=v, acc=a) 32 | print("Sending robot back to original position") 33 | rob.movej(j, acc=0.8, vel=0.2) 34 | 35 | 36 | finally: 37 | rob.close() 38 | 39 | -------------------------------------------------------------------------------- /examples/simple.py: -------------------------------------------------------------------------------- 1 | 2 | import urx 3 | import logging 4 | 5 | if __name__ == "__main__": 6 | logging.basicConfig(level=logging.WARN) 7 | 8 | rob = urx.Robot("192.168.1.100") 9 | #rob = urx.Robot("localhost") 10 | rob.set_tcp((0,0,0,0,0,0)) 11 | rob.set_payload(0.5, (0,0,0)) 12 | try: 13 | l = 0.05 14 | v = 0.05 15 | a = 0.3 16 | pose = rob.getl() 17 | print("robot tcp is at: ", pose) 18 | print("absolute move in base coordinate ") 19 | pose[2] += l 20 | rob.movel(pose, acc=a, vel=v) 21 | print("relative move in base coordinate ") 22 | rob.translate((0, 0, -l), acc=a, vel=v) 23 | print("relative move back and forth in tool coordinate") 24 | rob.translate_tool((0, 0, -l), acc=a, vel=v) 25 | rob.translate_tool((0, 0, l), acc=a, vel=v) 26 | finally: 27 | rob.close() 28 | 29 | -------------------------------------------------------------------------------- /examples/spnav_control.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | 3 | import spnav 4 | import time 5 | import math3d as m3d 6 | from math import pi 7 | 8 | import urx 9 | 10 | class Cmd(object): 11 | def __init__(self): 12 | self.reset() 13 | 14 | def reset(self): 15 | self.x = 0 16 | self.y = 0 17 | self.z = 0 18 | self.rx = 0 19 | self.ry = 0 20 | self.rz = 0 21 | self.btn0 = 0 22 | self.btn1 = 0 23 | 24 | def get_speeds(self): 25 | return [self.x, self.y, self.z, self.rx, self.ry, self.rz] 26 | 27 | 28 | 29 | class Service(object): 30 | def __init__(self, robot): 31 | self.robot = robot 32 | self.lin_coef = 5000 33 | self.rot_coef = 5000 34 | 35 | def loop(self): 36 | ts = 0 37 | btn0_state = 0 38 | btn_event = None 39 | cmd = Cmd() 40 | while True: 41 | time.sleep(0.01) 42 | cmd.reset() 43 | #spnav.spnav_remove_events(spnav.SPNAV_EVENT_ANY) # seems broken 44 | event = spnav.spnav_poll_event() 45 | if event: 46 | if type(event) is spnav.SpnavButtonEvent: 47 | btn_event = event 48 | if event.bnum == 0: 49 | btn0_state = event.press 50 | elif type(event) is spnav.SpnavMotionEvent: 51 | if abs(event.translation[0]) > 30: 52 | cmd.y = event.translation[0] / self.lin_coef 53 | if abs(event.translation[1]) > 30: 54 | cmd.z = -1 * event.translation[1] / self.lin_coef 55 | if abs(event.translation[2]) > 30: 56 | cmd.x = event.translation[2] / self.lin_coef 57 | if abs(event.rotation[0]) > 20: 58 | cmd.ry = event.rotation[0] / self.lin_coef 59 | if abs(event.rotation[1]) > 20: 60 | cmd.rz = -1 * event.rotation[1] / self.lin_coef 61 | if abs(event.rotation[2]) > 20: 62 | cmd.rx = event.rotation[2] / self.lin_coef 63 | 64 | if (time.time() - ts) > 0.12: 65 | ts = time.time() 66 | speeds = cmd.get_speeds() 67 | if btn0_state: 68 | self.robot.speedl_tool(speeds, acc=0.10, min_time=2) 69 | else: 70 | self.robot.speedl(speeds, acc=0.10, min_time=2) 71 | btn_event = None 72 | speeds = cmd.get_speeds() 73 | #if speeds != [0 for _ in speeds]: 74 | print(event) 75 | print("Sending", speeds) 76 | 77 | 78 | if __name__ == '__main__': 79 | spnav.spnav_open() 80 | robot = urx.Robot("192.168.0.90") 81 | #robot = urx.Robot("localhost") 82 | robot.set_tcp((0, 0, 0.27, 0, 0, 0)) 83 | trx = m3d.Transform() 84 | trx.orient.rotate_zb(pi/4) 85 | robot.set_csys("mycsys", trx) 86 | service = Service(robot) 87 | try: 88 | service.loop() 89 | finally: 90 | robot.close() 91 | spnav.spnav_close() 92 | 93 | 94 | 95 | -------------------------------------------------------------------------------- /examples/test_all.py: -------------------------------------------------------------------------------- 1 | """ 2 | Testing script that runs many of the urx methods, while attempting to keep robot pose around its starting pose 3 | """ 4 | 5 | from math import pi 6 | import time 7 | import sys 8 | 9 | import urx 10 | import logging 11 | 12 | if sys.version_info[0] < 3: # support python v2 13 | input = raw_input 14 | 15 | def wait(): 16 | if do_wait: 17 | print("Click enter to continue") 18 | input() 19 | 20 | if __name__ == "__main__": 21 | logging.basicConfig(level=logging.INFO) 22 | 23 | do_wait = True 24 | if len(sys.argv) > 1: 25 | do_wait = False 26 | 27 | rob = urx.Robot("192.168.1.100") 28 | #rob = urx.Robot("localhost") 29 | rob.set_tcp((0, 0, 0, 0, 0, 0)) 30 | rob.set_payload(0.5, (0, 0, 0)) 31 | try: 32 | l = 0.05 33 | v = 0.05 34 | a = 0.3 35 | r = 0.01 36 | print("Digital out 0 and 1 are: ", rob.get_digital_out(0), rob.get_digital_out(1)) 37 | print("Analog inputs are: ", rob.get_analog_inputs()) 38 | initj = rob.getj() 39 | print("Initial joint configuration is ", initj) 40 | t = rob.get_pose() 41 | print("Transformation from base to tcp is: ", t) 42 | 43 | print("Translating in x") 44 | wait() 45 | rob.translate((l, 0, 0), acc=a, vel=v) 46 | pose = rob.getl() 47 | print("robot tcp is at: ", pose) 48 | 49 | print("moving in z") 50 | wait() 51 | pose[2] += l 52 | rob.movel(pose, acc=a, vel=v, wait=False) 53 | print("Waiting 2s for end move") 54 | time.sleep(2) 55 | 56 | print("Moving through several points with a radius") 57 | wait() 58 | pose[0] -= l 59 | p1 = pose[:] 60 | pose[2] -= l 61 | p2 = pose[:] 62 | rob.movels([p1, p2], vel=v, acc=a, radius=r) 63 | 64 | print("rotate tcp around around base z ") 65 | wait() 66 | t.orient.rotate_zb(pi / 8) 67 | rob.set_pose(t, vel=v, acc=a) 68 | 69 | print("moving in tool z") 70 | wait() 71 | rob.translate_tool((0, 0, l), vel=v, acc=a) 72 | 73 | print("moving in tool -z using speed command") 74 | wait() 75 | rob.speedl_tool((0, 0, -v, 0, 0, 0), acc=a, min_time=3) 76 | print("Waiting 2 seconds2") 77 | time.sleep(2) 78 | print("stop robot") 79 | rob.stopj() 80 | 81 | print("Test movec") 82 | wait() 83 | pose = rob.get_pose() 84 | via = pose.copy() 85 | via.pos[0] += l 86 | to = via.copy() 87 | to.pos[1] += l 88 | rob.movec(via, to, acc=a, vel=v) 89 | 90 | print("Sending robot back to original position") 91 | rob.movej(initj, acc=0.8, vel=0.2) 92 | 93 | finally: 94 | rob.close() 95 | -------------------------------------------------------------------------------- /examples/test_movep.py: -------------------------------------------------------------------------------- 1 | import time 2 | import urx 3 | import logging 4 | 5 | 6 | if __name__ == "__main__": 7 | rob = urx.Robot("192.168.1.6") 8 | try: 9 | l = 0.1 10 | v = 0.07 11 | a = 0.1 12 | r = 0.05 13 | pose = rob.getl() 14 | pose[2] += l 15 | rob.movep(pose, acc=a, vel=v, radius=r, wait=False) 16 | while True: 17 | p = rob.getl(wait=True) 18 | if p[2] > pose[2] - 0.05: 19 | break 20 | 21 | pose[1] += l 22 | rob.movep(pose, acc=a, vel=v, radius=r, wait=False) 23 | while True: 24 | p = rob.getl(wait=True) 25 | if p[1] > pose[1] - 0.05: 26 | break 27 | 28 | pose[2] -= l 29 | rob.movep(pose, acc=a, vel=v, radius=r, wait=False) 30 | while True: 31 | p = rob.getl(wait=True) 32 | if p[2] < pose[2] + 0.05: 33 | break 34 | 35 | pose[1] -= l 36 | rob.movep(pose, acc=a, vel=v, radius=0, wait=True) 37 | 38 | finally: 39 | rob.close() 40 | 41 | -------------------------------------------------------------------------------- /make_deb.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | """ 3 | hackish file to crreate deb from setup.py 4 | """ 5 | 6 | import subprocess 7 | from email.utils import formatdate 8 | 9 | import urx 10 | 11 | DEBVERSION = urx.__version__ 12 | 13 | branch = subprocess.check_output("git rev-parse --abbrev-ref HEAD", shell=True) 14 | branch = branch.decode() 15 | branch = branch.strip() 16 | branch = str(branch).replace("'","") 17 | rev = subprocess.check_output("git log -1 --format=\'%ad--%h\' --date=short", shell=True) 18 | rev = rev.decode() 19 | rev = rev.strip() 20 | rev = rev.replace("'","") 21 | #rev = rev.replace(" ", "T", 1) 22 | #ev = rev.replace(" ", "Z", 1) 23 | 24 | vcsstring = "git-" + branch + "-" + rev 25 | 26 | def get_changelog(progname, version, changelog, date): 27 | """ 28 | return a dummy changelog acceptable by debian script engine 29 | """ 30 | return """%s (%s) unstable; urgency=low 31 | 32 | %s 33 | 34 | -- Olivier R-D %s """ % (progname, version, changelog, date) 35 | 36 | 37 | 38 | def check_deb(name): 39 | print("checking if %s is installed" % name) 40 | subprocess.check_call("dpkg -s %s > /dev/null" % name, shell=True) 41 | 42 | if __name__ == "__main__": 43 | check_deb("build-essential") 44 | f = open("debian/changelog", "w") 45 | f.write(get_changelog("python-urx", DEBVERSION + vcsstring, "Updated to last changes in repository", formatdate())) 46 | f.close() 47 | 48 | #now build package 49 | #subprocess.check_call("dpkg-buildpackage -rfakeroot -uc -us -b", shell=True) 50 | subprocess.check_call("fakeroot dh binary --with python3,python2", shell=True) 51 | subprocess.check_call("dh clean", shell=True) 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /make_windows_installer.bat: -------------------------------------------------------------------------------- 1 | python setup.py bdist_wininst 2 | pause 3 | -------------------------------------------------------------------------------- /release.py: -------------------------------------------------------------------------------- 1 | import re 2 | import os 3 | 4 | 5 | def bump_version(): 6 | with open("setup.py") as f: 7 | s = f.read() 8 | m = re.search(r'version="(.*)\.(.*)\.(.*)",', s) 9 | v1, v2, v3 = m.groups() 10 | oldv = "{0}.{1}.{2}".format(v1, v2, v3) 11 | newv = "{0}.{1}.{2}".format(v1, v2, str(int(v3) + 1)) 12 | print("Current version is: {0}, write new version, ctrl-c to exit".format(oldv)) 13 | ans = input(newv) 14 | if ans: 15 | newv = ans 16 | s = s.replace(oldv, newv) 17 | with open("setup.py", "w") as f: 18 | f.write(s) 19 | return newv 20 | 21 | 22 | def release(): 23 | v = bump_version() 24 | ans = input("version bumped, commiting?(Y/n)") 25 | if ans in ("", "y", "yes"): 26 | os.system("git add setup.py") 27 | os.system("git commit -m 'new release'") 28 | os.system("git tag {0}".format(v)) 29 | ans = input("change committed, push to server?(Y/n)") 30 | if ans in ("", "y", "yes"): 31 | os.system("git push") 32 | os.system("git push --tags") 33 | ans = input("upload to pip?(Y/n)") 34 | if ans in ("", "y", "yes"): 35 | os.system("rm -rf dist/*") 36 | os.system("python setup.py sdist") 37 | os.system("twine upload dist/*") 38 | 39 | 40 | if __name__ == "__main__": 41 | release() 42 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name="urx", 5 | version="0.11.0", 6 | description="Python library to control an UR robot", 7 | author="Olivier Roulet-Dubonnet", 8 | author_email="olivier.roulet@gmail.com", 9 | url='https://github.com/oroulet/python-urx', 10 | packages=["urx"], 11 | provides=["urx"], 12 | install_requires=["numpy", "math3d"], 13 | license="GNU Lesser General Public License v3", 14 | classifiers=[ 15 | "Programming Language :: Python", 16 | "Programming Language :: Python :: 3", 17 | "Development Status :: 4 - Beta", 18 | "Intended Audience :: Developers", 19 | "Operating System :: OS Independent", 20 | "Topic :: System :: Hardware :: Hardware Drivers", 21 | "Topic :: Software Development :: Libraries :: Python Modules", 22 | ]) 23 | -------------------------------------------------------------------------------- /tools/fakerobot.py: -------------------------------------------------------------------------------- 1 | import socket 2 | import threading 3 | import socketserver 4 | import time 5 | 6 | class RequestHandler(socketserver.BaseRequestHandler): 7 | #def __init__(self, *args, **kwargs): 8 | #print(self, *args, **kwargs) 9 | #print("Got connection from {}".format( self.client_address[0]) ) 10 | #socketserver.BaseRequestHandler.__init__(self, *args, **kwargs) 11 | 12 | def handle(self): 13 | while True: 14 | data = str(self.request.recv(1024), 'ascii') 15 | cur_thread = threading.current_thread() 16 | print("{} received {} from {}".format(cur_thread.name, data, self.client_address) ) 17 | if data == "": 18 | return 19 | 20 | #when this methods returns, the connection to the client closes 21 | 22 | def setup(self): 23 | print("Got new connection from {}".format( self.client_address) ) 24 | self.server.handlers.append(self) 25 | 26 | def finish(self): 27 | print("Connection from {} lost".format( self.client_address) ) 28 | self.server.handlers.remove(self) 29 | 30 | class Server(socketserver.ThreadingMixIn, socketserver.TCPServer): 31 | def init(self): 32 | """ 33 | __init__ should not be overriden 34 | """ 35 | self.handlers = [] 36 | 37 | def close(self): 38 | for handler in self.handlers: 39 | handler.request.shutdown(socket.SHUT_RDWR) 40 | handler.request.close() 41 | self.shutdown() 42 | 43 | 44 | class FakeRobot(object): 45 | 46 | def run(self): 47 | host = "localhost" 48 | port = 30002 49 | server = Server((host, port), RequestHandler) 50 | server.init() 51 | server_thread = threading.Thread(target=server.serve_forever) 52 | server_thread.daemon = True 53 | server_thread.start() 54 | print("Fake Universal robot running at ", host, port) 55 | try: 56 | f = open("packet.bin", "rb") 57 | packet = f.read() 58 | f.close() 59 | while True: 60 | time.sleep(0.09) #The real robot published data 10 times a second 61 | for handler in server.handlers: 62 | handler.request.sendall(packet) 63 | finally: 64 | print("Shutting down server") 65 | server.close() 66 | 67 | 68 | if __name__ == "__main__": 69 | r = FakeRobot() 70 | r.run() 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /tools/find_packet.py: -------------------------------------------------------------------------------- 1 | from urx import ursecmon 2 | 3 | if __name__ == "__main__": 4 | f = open("packets.bin", "rb") 5 | s = open("packet.bin", "wb") 6 | data = f.read(99999) 7 | parser = ursecmon.ParserUtils() 8 | p, rest = parser.find_first_packet(data) 9 | print(len(p)) 10 | p, rest = parser.find_first_packet(rest) 11 | print(len(p)) 12 | s.write(p) 13 | p, rest = parser.find_first_packet(rest) 14 | print(len(p)) 15 | -------------------------------------------------------------------------------- /tools/get_rob.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import logging 4 | from math import pi 5 | from IPython import embed 6 | 7 | from urx import Robot 8 | import math3d 9 | 10 | if __name__ == "__main__": 11 | if len(sys.argv) > 1: 12 | host = sys.argv[1] 13 | else: 14 | host = 'localhost' 15 | try: 16 | robot = Robot(host) 17 | r = robot 18 | embed() 19 | finally: 20 | if "robot" in dir(): 21 | robot.close() 22 | 23 | -------------------------------------------------------------------------------- /tools/grabber.py: -------------------------------------------------------------------------------- 1 | import socket 2 | import sys 3 | 4 | 5 | if __name__ == "__main__": 6 | host, port = "localhost", 30002 7 | host, port = "192.168.1.8", 30002 8 | if len(sys.argv) > 1: 9 | host = sys.argv[1] 10 | 11 | sock = socket.create_connection((host, port)) 12 | f = open("packets.bin", "wb") 13 | 14 | try: 15 | # Connect to server and send data 16 | for i in range(0, 20): 17 | data = sock.recv(1024) 18 | f.write(data) 19 | print("Got packet: ", i) 20 | finally: 21 | f.close() 22 | sock.close() 23 | 24 | -------------------------------------------------------------------------------- /tools/packet.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SintefManufacturing/python-urx/e277ef1a7fb057f9e642a21cd95ade2b4e5a3468/tools/packet.bin -------------------------------------------------------------------------------- /urx/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Python library to control an UR robot through its TCP/IP interface 3 | """ 4 | from urx.urrobot import RobotException, URRobot # noqa 5 | 6 | __version__ = "0.11.0" 7 | 8 | try: 9 | from urx.robot import Robot 10 | except ImportError as ex: 11 | print("Exception while importing math3d base robot, disabling use of matrices", ex) 12 | Robot = URRobot 13 | -------------------------------------------------------------------------------- /urx/robot.py: -------------------------------------------------------------------------------- 1 | """ 2 | Python library to control an UR robot through its TCP/IP interface 3 | DOC LINK 4 | http://support.universal-robots.com/URRobot/RemoteAccess 5 | """ 6 | 7 | 8 | import math3d as m3d 9 | import numpy as np 10 | 11 | from urx.urrobot import URRobot 12 | 13 | __author__ = "Olivier Roulet-Dubonnet" 14 | __copyright__ = "Copyright 2011-2016, Sintef Raufoss Manufacturing" 15 | __license__ = "LGPLv3" 16 | 17 | 18 | class Robot(URRobot): 19 | 20 | """ 21 | Generic Python interface to an industrial robot. 22 | Compared to the URRobot class, this class adds the possibilty to work directly with matrices 23 | and includes support for setting a reference coordinate system 24 | """ 25 | 26 | def __init__(self, host, use_rt=False, urFirm=None): 27 | URRobot.__init__(self, host, use_rt, urFirm) 28 | self.csys = m3d.Transform() 29 | 30 | def _get_lin_dist(self, target): 31 | pose = URRobot.getl(self, wait=True) 32 | target = m3d.Transform(target) 33 | pose = m3d.Transform(pose) 34 | return pose.dist(target) 35 | 36 | def set_tcp(self, tcp): 37 | """ 38 | set robot flange to tool tip transformation 39 | """ 40 | if isinstance(tcp, m3d.Transform): 41 | tcp = tcp.pose_vector 42 | URRobot.set_tcp(self, tcp) 43 | 44 | def set_csys(self, transform): 45 | """ 46 | Set reference coordinate system to use 47 | """ 48 | self.csys = transform 49 | 50 | def set_orientation(self, orient, acc=0.01, vel=0.01, wait=True, threshold=None): 51 | """ 52 | set tool orientation using a orientation matric from math3d 53 | or a orientation vector 54 | """ 55 | if not isinstance(orient, m3d.Orientation): 56 | orient = m3d.Orientation(orient) 57 | trans = self.get_pose() 58 | trans.orient = orient 59 | self.set_pose(trans, acc, vel, wait=wait, threshold=threshold) 60 | 61 | def translate_tool(self, vect, acc=0.01, vel=0.01, wait=True, threshold=None): 62 | """ 63 | move tool in tool coordinate, keeping orientation 64 | """ 65 | t = m3d.Transform() 66 | if not isinstance(vect, m3d.Vector): 67 | vect = m3d.Vector(vect) 68 | t.pos += vect 69 | return self.add_pose_tool(t, acc, vel, wait=wait, threshold=threshold) 70 | 71 | def back(self, z=0.05, acc=0.01, vel=0.01): 72 | """ 73 | move in z tool 74 | """ 75 | self.translate_tool((0, 0, -z), acc=acc, vel=vel) 76 | 77 | def set_pos(self, vect, acc=0.01, vel=0.01, wait=True, threshold=None): 78 | """ 79 | set tool to given pos, keeping constant orientation 80 | """ 81 | if not isinstance(vect, m3d.Vector): 82 | vect = m3d.Vector(vect) 83 | trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect)) 84 | return self.set_pose(trans, acc, vel, wait=wait, threshold=threshold) 85 | 86 | def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True, threshold=None): 87 | """ 88 | Move Circular: Move to position (circular in tool-space) 89 | see UR documentation 90 | """ 91 | pose_via = self.csys * m3d.Transform(pose_via) 92 | pose_to = self.csys * m3d.Transform(pose_to) 93 | pose = URRobot.movec(self, pose_via.pose_vector, pose_to.pose_vector, acc=acc, vel=vel, wait=wait, threshold=threshold) 94 | if pose is not None: 95 | return self.csys.inverse * m3d.Transform(pose) 96 | 97 | def set_pose(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=None): 98 | """ 99 | move tcp to point and orientation defined by a transformation 100 | UR robots have several move commands, by default movel is used but it can be changed 101 | using the command argument 102 | """ 103 | self.logger.debug("Setting pose to %s", trans.pose_vector) 104 | t = self.csys * trans 105 | pose = URRobot.movex(self, command, t.pose_vector, acc=acc, vel=vel, wait=wait, threshold=threshold) 106 | if pose is not None: 107 | return self.csys.inverse * m3d.Transform(pose) 108 | 109 | def add_pose_base(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=None): 110 | """ 111 | Add transform expressed in base coordinate 112 | """ 113 | pose = self.get_pose() 114 | return self.set_pose(trans * pose, acc, vel, wait=wait, command=command, threshold=threshold) 115 | 116 | def add_pose_tool(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=None): 117 | """ 118 | Add transform expressed in tool coordinate 119 | """ 120 | pose = self.get_pose() 121 | return self.set_pose(pose * trans, acc, vel, wait=wait, command=command, threshold=threshold) 122 | 123 | def get_pose(self, wait=False, _log=True): 124 | """ 125 | get current transform from base to to tcp 126 | """ 127 | pose = URRobot.getl(self, wait, _log) 128 | trans = self.csys.inverse * m3d.Transform(pose) 129 | if _log: 130 | self.logger.debug("Returning pose to user: %s", trans.pose_vector) 131 | return trans 132 | 133 | def get_orientation(self, wait=False): 134 | """ 135 | get tool orientation in base coordinate system 136 | """ 137 | trans = self.get_pose(wait) 138 | return trans.orient 139 | 140 | def get_pos(self, wait=False): 141 | """ 142 | get tool tip pos(x, y, z) in base coordinate system 143 | """ 144 | trans = self.get_pose(wait) 145 | return trans.pos 146 | 147 | def speedl(self, velocities, acc, min_time): 148 | """ 149 | move at given velocities until minimum min_time seconds 150 | """ 151 | v = self.csys.orient * m3d.Vector(velocities[:3]) 152 | w = self.csys.orient * m3d.Vector(velocities[3:]) 153 | vels = np.concatenate((v.array, w.array)) 154 | return self.speedx("speedl", vels, acc, min_time) 155 | 156 | def speedj(self, velocities, acc, min_time): 157 | """ 158 | move at given joint velocities until minimum min_time seconds 159 | """ 160 | return self.speedx("speedj", velocities, acc, min_time) 161 | 162 | def speedl_tool(self, velocities, acc, min_time): 163 | """ 164 | move at given velocities in tool csys until minimum min_time seconds 165 | """ 166 | pose = self.get_pose() 167 | v = pose.orient * m3d.Vector(velocities[:3]) 168 | w = pose.orient * m3d.Vector(velocities[3:]) 169 | self.speedl(np.concatenate((v.array, w.array)), acc, min_time) 170 | 171 | def movex(self, command, pose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=None): 172 | """ 173 | Send a move command to the robot. since UR robotene have several methods this one 174 | sends whatever is defined in 'command' string 175 | """ 176 | t = m3d.Transform(pose) 177 | if relative: 178 | return self.add_pose_base(t, acc, vel, wait=wait, command=command, threshold=threshold) 179 | else: 180 | return self.set_pose(t, acc, vel, wait=wait, command=command, threshold=threshold) 181 | 182 | def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=None): 183 | """ 184 | Concatenate several movex commands and applies a blending radius 185 | pose_list is a list of pose. 186 | This method is usefull since any new command from python 187 | to robot make the robot stop 188 | """ 189 | new_poses = [] 190 | for pose in pose_list: 191 | t = self.csys * m3d.Transform(pose) 192 | pose = t.pose_vector 193 | new_poses.append(pose) 194 | return URRobot.movexs(self, command, new_poses, acc, vel, radius, wait=wait, threshold=threshold) 195 | 196 | def movel_tool(self, pose, acc=0.01, vel=0.01, wait=True, threshold=None): 197 | """ 198 | move linear to given pose in tool coordinate 199 | """ 200 | return self.movex_tool("movel", pose, acc=acc, vel=vel, wait=wait, threshold=threshold) 201 | 202 | def movex_tool(self, command, pose, acc=0.01, vel=0.01, wait=True, threshold=None): 203 | t = m3d.Transform(pose) 204 | self.add_pose_tool(t, acc, vel, wait=wait, command=command, threshold=threshold) 205 | 206 | def getl(self, wait=False, _log=True): 207 | """ 208 | return current transformation from tcp to current csys 209 | """ 210 | t = self.get_pose(wait, _log) 211 | return t.pose_vector.tolist() 212 | 213 | def set_gravity(self, vector): 214 | if isinstance(vector, m3d.Vector): 215 | vector = vector.list 216 | return URRobot.set_gravity(self, vector) 217 | 218 | def new_csys_from_xpy(self): 219 | """ 220 | Restores and returns new coordinate system calculated from three points: X, Origin, Y 221 | 222 | based on math3d: Transform.new_from_xyp 223 | """ 224 | # Set coord. sys. to 0 225 | self.csys = m3d.Transform() 226 | 227 | print("A new coordinate system will be defined from the next three points") 228 | print("Firs point is X, second Origin, third Y") 229 | print("Set it as a new reference by calling myrobot.set_csys(new_csys)") 230 | input("Move to first point and click Enter") 231 | # we do not use get_pose so we avoid rounding values 232 | pose = URRobot.getl(self) 233 | print("Introduced point defining X: {}".format(pose[:3])) 234 | px = m3d.Vector(pose[:3]) 235 | input("Move to second point and click Enter") 236 | pose = URRobot.getl(self) 237 | print("Introduced point defining Origo: {}".format(pose[:3])) 238 | p0 = m3d.Vector(pose[:3]) 239 | input("Move to third point and click Enter") 240 | pose = URRobot.getl(self) 241 | print("Introduced point defining Y: {}".format(pose[:3])) 242 | py = m3d.Vector(pose[:3]) 243 | 244 | new_csys = m3d.Transform.new_from_xyp(px - p0, py - p0, p0) 245 | self.set_csys(new_csys) 246 | 247 | return new_csys 248 | 249 | @property 250 | def x(self): 251 | return self.get_pos().x 252 | 253 | @x.setter 254 | def x(self, val): 255 | p = self.get_pos() 256 | p.x = val 257 | self.set_pos(p) 258 | 259 | @property 260 | def y(self): 261 | return self.get_pos().y 262 | 263 | @y.setter 264 | def y(self, val): 265 | p = self.get_pos() 266 | p.y = val 267 | self.set_pos(p) 268 | 269 | @property 270 | def z(self): 271 | return self.get_pos().z 272 | 273 | @z.setter 274 | def z(self, val): 275 | p = self.get_pos() 276 | p.z = val 277 | self.set_pos(p) 278 | 279 | @property 280 | def rx(self): 281 | return 0 282 | 283 | @rx.setter 284 | def rx(self, val): 285 | p = self.get_pose() 286 | p.orient.rotate_xb(val) 287 | self.set_pose(p) 288 | 289 | @property 290 | def ry(self): 291 | return 0 292 | 293 | @ry.setter 294 | def ry(self, val): 295 | p = self.get_pose() 296 | p.orient.rotate_yb(val) 297 | self.set_pose(p) 298 | 299 | @property 300 | def rz(self): 301 | return 0 302 | 303 | @rz.setter 304 | def rz(self, val): 305 | p = self.get_pose() 306 | p.orient.rotate_zb(val) 307 | self.set_pose(p) 308 | 309 | @property 310 | def x_t(self): 311 | return 0 312 | 313 | @x_t.setter 314 | def x_t(self, val): 315 | t = m3d.Transform() 316 | t.pos.x += val 317 | self.add_pose_tool(t) 318 | 319 | @property 320 | def y_t(self): 321 | return 0 322 | 323 | @y_t.setter 324 | def y_t(self, val): 325 | t = m3d.Transform() 326 | t.pos.y += val 327 | self.add_pose_tool(t) 328 | 329 | @property 330 | def z_t(self): 331 | return 0 332 | 333 | @z_t.setter 334 | def z_t(self, val): 335 | t = m3d.Transform() 336 | t.pos.z += val 337 | self.add_pose_tool(t) 338 | 339 | @property 340 | def rx_t(self): 341 | return 0 342 | 343 | @rx_t.setter 344 | def rx_t(self, val): 345 | t = m3d.Transform() 346 | t.orient.rotate_xb(val) 347 | self.add_pose_tool(t) 348 | 349 | @property 350 | def ry_t(self): 351 | return 0 352 | 353 | @ry_t.setter 354 | def ry_t(self, val): 355 | t = m3d.Transform() 356 | t.orient.rotate_yb(val) 357 | self.add_pose_tool(t) 358 | 359 | @property 360 | def rz_t(self): 361 | return 0 362 | 363 | @rz_t.setter 364 | def rz_t(self, val): 365 | t = m3d.Transform() 366 | t.orient.rotate_zb(val) 367 | self.add_pose_tool(t) 368 | -------------------------------------------------------------------------------- /urx/robotiq_two_finger_gripper.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | """ 4 | Python library to control Robotiq Two Finger Gripper connected to UR robot via 5 | Python-URX 6 | 7 | Tested using a UR5 Version CB3 and Robotiq 2-Finger Gripper Version 85 8 | 9 | SETUP 10 | 11 | You must install the driver first and then power on the gripper from the 12 | gripper UI. The driver can be found here: 13 | 14 | http://support.robotiq.com/pages/viewpage.action?pageId=5963876 15 | 16 | FAQ 17 | 18 | Q: Why does this class group all the commands together and run them as a single 19 | program as opposed to running each line seperately (like most of URX)? 20 | 21 | A: The gripper is controlled by connecting to the robot's computer (TCP/IP) and 22 | then communicating with the gripper via a socket (127.0.0.1:63352). The scope 23 | of the socket is at the program level. It will be automatically closed 24 | whenever a program finishes. Therefore it's important that we run all commands 25 | as a single program. 26 | 27 | DOCUMENTATION 28 | 29 | - This code was developed by downloading the gripper package "DCU-1.0.10" from 30 | http://support.robotiq.com/pages/viewpage.action?pageId=5963876. Or more 31 | directly from http://support.robotiq.com/download/attachments/5963876/DCU-1.0.10.zip 32 | - The file robotiq_2f_gripper_programs_CB3/rq_script.script was referenced to 33 | create this class 34 | 35 | FUTURE FEATURES 36 | 37 | Though I haven't developed it yet, if you look in 38 | robotiq_2f_gripper_programs_CB3/advanced_template_test.script and view function 39 | "rq_get_var" there is an example of how to determine the current state of the 40 | gripper and if it's holding an object. 41 | """ # noqa 42 | 43 | import logging 44 | import os 45 | import time 46 | 47 | from urx.urscript import URScript 48 | 49 | # Gripper Variables 50 | ACT = "ACT" 51 | GTO = "GTO" 52 | ATR = "ATR" 53 | ARD = "ARD" 54 | FOR = "FOR" 55 | SPE = "SPE" 56 | OBJ = "OBJ" 57 | STA = "STA" 58 | FLT = "FLT" 59 | POS = "POS" 60 | 61 | SOCKET_HOST = "127.0.0.1" 62 | SOCKET_PORT = 63352 63 | SOCKET_NAME = "gripper_socket" 64 | 65 | 66 | class RobotiqScript(URScript): 67 | 68 | def __init__(self, 69 | socket_host=SOCKET_HOST, 70 | socket_port=SOCKET_PORT, 71 | socket_name=SOCKET_NAME): 72 | self.socket_host = socket_host 73 | self.socket_port = socket_port 74 | self.socket_name = socket_name 75 | super(RobotiqScript, self).__init__() 76 | 77 | # Reset connection to gripper 78 | self._socket_close(self.socket_name) 79 | self._socket_open(self.socket_host, 80 | self.socket_port, 81 | self.socket_name) 82 | 83 | def _import_rq_script(self): 84 | dir_path = os.path.dirname(os.path.realpath(__file__)) 85 | rq_script = os.path.join(dir_path, 'rq_script.script') 86 | with open(rq_script, 'rb') as f: 87 | rq_script = f.read() 88 | self.add_header_to_program(rq_script) 89 | 90 | def _rq_get_var(self, var_name, nbytes): 91 | self._socket_send_string("GET {}".format(var_name)) 92 | self._socket_read_byte_list(nbytes) 93 | 94 | def _get_gripper_fault(self): 95 | self._rq_get_var(FLT, 2) 96 | 97 | def _get_gripper_object(self): 98 | self._rq_get_var(OBJ, 1) 99 | 100 | def _get_gripper_status(self): 101 | self._rq_get_var(STA, 1) 102 | 103 | def _set_gripper_activate(self): 104 | self._socket_set_var(GTO, 1, self.socket_name) 105 | 106 | def _set_gripper_force(self, value): 107 | """ 108 | FOR is the variable 109 | range is 0 - 255 110 | 0 is no force 111 | 255 is full force 112 | """ 113 | value = self._constrain_unsigned_char(value) 114 | self._socket_set_var(FOR, value, self.socket_name) 115 | 116 | def _set_gripper_position(self, value): 117 | """ 118 | SPE is the variable 119 | range is 0 - 255 120 | 0 is no speed 121 | 255 is full speed 122 | """ 123 | value = self._constrain_unsigned_char(value) 124 | self._socket_set_var(POS, value, self.socket_name) 125 | 126 | def _set_gripper_speed(self, value): 127 | """ 128 | SPE is the variable 129 | range is 0 - 255 130 | 0 is no speed 131 | 255 is full speed 132 | """ 133 | value = self._constrain_unsigned_char(value) 134 | self._socket_set_var(SPE, value, self.socket_name) 135 | 136 | def _set_robot_activate(self): 137 | self._socket_set_var(ACT, 1, self.socket_name) 138 | 139 | 140 | class Robotiq_Two_Finger_Gripper(object): 141 | 142 | def __init__(self, 143 | robot, 144 | payload=0.85, 145 | speed=255, 146 | force=50, 147 | socket_host=SOCKET_HOST, 148 | socket_port=SOCKET_PORT, 149 | socket_name=SOCKET_NAME): 150 | self.robot = robot 151 | self.payload = payload 152 | self.speed = speed 153 | self.force = force 154 | self.socket_host = socket_host 155 | self.socket_port = socket_port 156 | self.socket_name = socket_name 157 | self.logger = logging.getLogger(u"robotiq") 158 | 159 | def _get_new_urscript(self): 160 | """ 161 | Set up a new URScript to communicate with gripper 162 | """ 163 | urscript = RobotiqScript(socket_host=self.socket_host, 164 | socket_port=self.socket_port, 165 | socket_name=self.socket_name) 166 | 167 | # Set input and output voltage ranges 168 | urscript._set_analog_inputrange(0, 0) 169 | urscript._set_analog_inputrange(1, 0) 170 | urscript._set_analog_inputrange(2, 0) 171 | urscript._set_analog_inputrange(3, 0) 172 | urscript._set_analog_outputdomain(0, 0) 173 | urscript._set_analog_outputdomain(1, 0) 174 | urscript._set_tool_voltage(0) 175 | urscript._set_runstate_outputs() 176 | 177 | # Set payload, speed and force 178 | urscript._set_payload(self.payload) 179 | urscript._set_gripper_speed(self.speed) 180 | urscript._set_gripper_force(self.force) 181 | 182 | # Initialize the gripper 183 | urscript._set_robot_activate() 184 | urscript._set_gripper_activate() 185 | 186 | # Wait on activation to avoid USB conflicts 187 | urscript._sleep(0.1) 188 | 189 | return urscript 190 | 191 | def gripper_action(self, value): 192 | """ 193 | Activate the gripper to a given value from 0 to 255 194 | 195 | 0 is open 196 | 255 is closed 197 | """ 198 | urscript = self._get_new_urscript() 199 | 200 | # Move to the position 201 | sleep = 2.0 202 | urscript._set_gripper_position(value) 203 | urscript._sleep(sleep) 204 | 205 | # Send the script 206 | self.robot.send_program(urscript()) 207 | 208 | # sleep the code the same amount as the urscript to ensure that 209 | # the action completes 210 | time.sleep(sleep) 211 | 212 | def open_gripper(self): 213 | self.gripper_action(0) 214 | 215 | def close_gripper(self): 216 | self.gripper_action(255) 217 | -------------------------------------------------------------------------------- /urx/urrobot.py: -------------------------------------------------------------------------------- 1 | """ 2 | Python library to control an UR robot through its TCP/IP interface 3 | Documentation from universal robots: 4 | http://support.universal-robots.com/URRobot/RemoteAccess 5 | """ 6 | 7 | import logging 8 | import numbers 9 | 10 | try: 11 | from collections.abc import Sequence 12 | except ImportError: 13 | from collections import Sequence 14 | 15 | from urx import urrtmon 16 | from urx import ursecmon 17 | 18 | __author__ = "Olivier Roulet-Dubonnet" 19 | __copyright__ = "Copyright 2011-2015, Sintef Raufoss Manufacturing" 20 | __license__ = "LGPLv3" 21 | 22 | 23 | class RobotException(Exception): 24 | pass 25 | 26 | 27 | class URRobot(object): 28 | 29 | """ 30 | Python interface to socket interface of UR robot. 31 | programs are send to port 3002 32 | data is read from secondary interface(10Hz?) and real-time interface(125Hz) (called Matlab interface in documentation) 33 | Since parsing the RT interface uses som CPU, and does not support all robots versions, it is disabled by default 34 | The RT interfaces is only used for the get_force related methods 35 | Rmq: A program sent to the robot i executed immendiatly and any running program is stopped 36 | """ 37 | 38 | def __init__(self, host, use_rt=False, urFirm=None): 39 | self.logger = logging.getLogger("urx") 40 | self.host = host 41 | self.urFirm = urFirm 42 | self.csys = None 43 | 44 | self.logger.debug("Opening secondary monitor socket") 45 | self.secmon = ursecmon.SecondaryMonitor(self.host) # data from robot at 10Hz 46 | 47 | self.rtmon = None 48 | if use_rt: 49 | self.rtmon = self.get_realtime_monitor() 50 | # precision of joint movem used to wait for move completion 51 | # the value must be conservative! otherwise we may wait forever 52 | self.joinEpsilon = 0.01 53 | # It seems URScript is limited in the character length of floats it accepts 54 | self.max_float_length = 6 # FIXME: check max length!!! 55 | 56 | self.secmon.wait() # make sure we get data from robot before letting clients access our methods 57 | 58 | def __repr__(self): 59 | return "Robot Object (IP=%s, state=%s)" % (self.host, self.secmon.get_all_data()["RobotModeData"]) 60 | 61 | def __str__(self): 62 | return self.__repr__() 63 | 64 | def __enter__(self): 65 | return self 66 | 67 | def __exit__(self, exc_type, exc_value, traceback): 68 | self.close() 69 | 70 | def is_running(self): 71 | """ 72 | Return True if robot is running (not 73 | necessary running a program, it might be idle) 74 | """ 75 | return self.secmon.running 76 | 77 | def is_program_running(self): 78 | """ 79 | check if program is running. 80 | Warning!!!!!: After sending a program it might take several 10th of 81 | a second before the robot enters the running state 82 | """ 83 | return self.secmon.is_program_running() 84 | 85 | def send_program(self, prog): 86 | """ 87 | send a complete program using urscript to the robot 88 | the program is executed immediatly and any runnning 89 | program is interrupted 90 | """ 91 | self.logger.info("Sending program: " + prog) 92 | self.secmon.send_program(prog) 93 | 94 | def get_tcp_force(self, wait=True): 95 | """ 96 | return measured force in TCP 97 | if wait==True, waits for next packet before returning 98 | """ 99 | return self.rtmon.getTCFForce(wait) 100 | 101 | def get_force(self, wait=True): 102 | """ 103 | length of force vector returned by get_tcp_force 104 | if wait==True, waits for next packet before returning 105 | """ 106 | tcpf = self.get_tcp_force(wait) 107 | force = 0 108 | for i in tcpf: 109 | force += i**2 110 | return force**0.5 111 | 112 | def get_joint_temperature(self, wait=True): 113 | """ 114 | return measured joint temperature 115 | if wait==True, waits for next packet before returning 116 | """ 117 | return self.rtmon.getJOINTTemperature(wait) 118 | 119 | def get_joint_voltage(self, wait=True): 120 | """ 121 | return measured joint voltage 122 | if wait==True, waits for next packet before returning 123 | """ 124 | return self.rtmon.getJOINTVoltage(wait) 125 | 126 | def get_joint_current(self, wait=True): 127 | """ 128 | return measured joint current 129 | if wait==True, waits for next packet before returning 130 | """ 131 | return self.rtmon.getJOINTCurrent(wait) 132 | 133 | def get_main_voltage(self, wait=True): 134 | """ 135 | return measured Safety Control Board: Main voltage 136 | if wait==True, waits for next packet before returning 137 | """ 138 | return self.rtmon.getMAINVoltage(wait) 139 | 140 | def get_robot_voltage(self, wait=True): 141 | """ 142 | return measured Safety Control Board: Robot voltage (48V) 143 | if wait==True, waits for next packet before returning 144 | """ 145 | return self.rtmon.getROBOTVoltage(wait) 146 | 147 | def get_robot_current(self, wait=True): 148 | """ 149 | return measured Safety Control Board: Robot current 150 | if wait==True, waits for next packet before returning 151 | """ 152 | return self.rtmon.getROBOTCurrent(wait) 153 | 154 | def get_all_rt_data(self, wait=True): 155 | """ 156 | return all data parsed from robot real-time interace as a dict 157 | if wait==True, waits for next packet before returning 158 | """ 159 | return self.rtmon.getALLData(wait) 160 | 161 | 162 | def set_tcp(self, tcp): 163 | """ 164 | set robot flange to tool tip transformation 165 | """ 166 | prog = "set_tcp(p[{}, {}, {}, {}, {}, {}])".format(*tcp) 167 | self.send_program(prog) 168 | 169 | def set_payload(self, weight, cog=None): 170 | """ 171 | set payload in Kg 172 | cog is a vector x,y,z 173 | if cog is not specified, then tool center point is used 174 | """ 175 | if cog: 176 | cog = list(cog) 177 | cog.insert(0, weight) 178 | prog = "set_payload({}, ({},{},{}))".format(*cog) 179 | else: 180 | prog = "set_payload(%s)" % weight 181 | self.send_program(prog) 182 | 183 | def set_gravity(self, vector): 184 | """ 185 | set direction of gravity 186 | """ 187 | prog = "set_gravity(%s)" % list(vector) 188 | self.send_program(prog) 189 | 190 | def send_message(self, msg): 191 | """ 192 | send message to the GUI log tab on the robot controller 193 | """ 194 | prog = "textmsg(%s)" % msg 195 | self.send_program(prog) 196 | 197 | def set_digital_out(self, output, val): 198 | """ 199 | set digital output. val is a bool 200 | """ 201 | if val in (True, 1): 202 | val = "True" 203 | else: 204 | val = "False" 205 | self.send_program('digital_out[%s]=%s' % (output, val)) 206 | 207 | def get_analog_inputs(self): 208 | """ 209 | get analog input 210 | """ 211 | return self.secmon.get_analog_inputs() 212 | 213 | def get_analog_in(self, nb, wait=False): 214 | """ 215 | get analog input 216 | """ 217 | return self.secmon.get_analog_in(nb, wait=wait) 218 | 219 | def get_digital_in_bits(self): 220 | """ 221 | get digital output 222 | """ 223 | return self.secmon.get_digital_in_bits() 224 | 225 | def get_digital_in(self, nb, wait=False): 226 | """ 227 | get digital output 228 | """ 229 | return self.secmon.get_digital_in(nb, wait) 230 | 231 | def get_digital_out(self, val, wait=False): 232 | """ 233 | get digital output 234 | """ 235 | return self.secmon.get_digital_out(val, wait=wait) 236 | 237 | def get_digital_out_bits(self, wait=False): 238 | """ 239 | get digital output as a byte 240 | """ 241 | return self.secmon.get_digital_out_bits(wait=wait) 242 | 243 | def set_analog_out(self, output, val): 244 | """ 245 | set analog output, val is a float 246 | """ 247 | prog = "set_analog_out(%s, %s)" % (output, val) 248 | self.send_program(prog) 249 | 250 | def set_tool_voltage(self, val): 251 | """ 252 | set voltage to be delivered to the tool, val is 0, 12 or 24 253 | """ 254 | prog = "set_tool_voltage(%s)" % (val) 255 | self.send_program(prog) 256 | 257 | def _wait_for_move(self, target, threshold=None, timeout=5, joints=False): 258 | """ 259 | wait for a move to complete. Unfortunately there is no good way to know when a move has finished 260 | so for every received data from robot we compute a dist equivalent and when it is lower than 261 | 'threshold' we return. 262 | if threshold is not reached within timeout, an exception is raised 263 | """ 264 | self.logger.debug("Waiting for move completion using threshold %s and target %s", threshold, target) 265 | start_dist = self._get_dist(target, joints) 266 | if threshold is None: 267 | threshold = start_dist * 0.8 268 | if threshold < 0.001: # roboten precision is limited 269 | threshold = 0.001 270 | self.logger.debug("No threshold set, setting it to %s", threshold) 271 | count = 0 272 | while True: 273 | if not self.is_running(): 274 | raise RobotException("Robot stopped") 275 | dist = self._get_dist(target, joints) 276 | self.logger.debug("distance to target is: %s, target dist is %s", dist, threshold) 277 | if not self.secmon.is_program_running(): 278 | if dist < threshold: 279 | self.logger.debug("we are threshold(%s) close to target, move has ended", threshold) 280 | return 281 | count += 1 282 | if count > timeout * 10: 283 | raise RobotException("Goal not reached but no program has been running for {} seconds. dist is {}, threshold is {}, target is {}, current pose is {}".format(timeout, dist, threshold, target, URRobot.getl(self))) 284 | else: 285 | count = 0 286 | 287 | def _get_dist(self, target, joints=False): 288 | if joints: 289 | return self._get_joints_dist(target) 290 | else: 291 | return self._get_lin_dist(target) 292 | 293 | def _get_lin_dist(self, target): 294 | # FIXME: we have an issue here, it seems sometimes the axis angle received from robot 295 | pose = URRobot.getl(self, wait=True) 296 | dist = 0 297 | for i in range(3): 298 | dist += (target[i] - pose[i]) ** 2 299 | for i in range(3, 6): 300 | dist += ((target[i] - pose[i]) / 5) ** 2 # arbitraty length like 301 | return dist ** 0.5 302 | 303 | def _get_joints_dist(self, target): 304 | joints = self.getj(wait=True) 305 | dist = 0 306 | for i in range(6): 307 | dist += (target[i] - joints[i]) ** 2 308 | return dist ** 0.5 309 | 310 | def getj(self, wait=False): 311 | """ 312 | get joints position 313 | """ 314 | jts = self.secmon.get_joint_data(wait) 315 | return [jts["q_actual0"], jts["q_actual1"], jts["q_actual2"], jts["q_actual3"], jts["q_actual4"], jts["q_actual5"]] 316 | 317 | def speedx(self, command, velocities, acc, min_time): 318 | vels = [round(i, self.max_float_length) for i in velocities] 319 | vels.append(acc) 320 | vels.append(min_time) 321 | prog = "{}([{},{},{},{},{},{}], {}, {})".format(command, *vels) 322 | self.send_program(prog) 323 | 324 | def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold=None): 325 | """ 326 | move in joint space 327 | """ 328 | if relative: 329 | l = self.getj() 330 | joints = [v + l[i] for i, v in enumerate(joints)] 331 | prog = self._format_move("movej", joints, acc, vel) 332 | self.send_program(prog) 333 | if wait: 334 | self._wait_for_move(joints[:6], threshold=threshold, joints=True) 335 | return self.getj() 336 | 337 | def movel(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=None): 338 | """ 339 | Send a movel command to the robot. See URScript documentation. 340 | """ 341 | return self.movex("movel", tpose, acc=acc, vel=vel, wait=wait, relative=relative, threshold=threshold) 342 | 343 | def movep(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=None): 344 | """ 345 | Send a movep command to the robot. See URScript documentation. 346 | """ 347 | return self.movex("movep", tpose, acc=acc, vel=vel, wait=wait, relative=relative, threshold=threshold) 348 | 349 | def servoc(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=None): 350 | """ 351 | Send a servoc command to the robot. See URScript documentation. 352 | """ 353 | return self.movex("servoc", tpose, acc=acc, vel=vel, wait=wait, relative=relative, threshold=threshold) 354 | 355 | def servoj(self, tjoints, acc=0.01, vel=0.01, t=0.1, lookahead_time=0.2, gain=100, wait=True, relative=False, threshold=None): 356 | """ 357 | Send a servoj command to the robot. See URScript documentation. 358 | """ 359 | if relative: 360 | l = self.getj() 361 | tjoints = [v + l[i] for i, v in enumerate(tjoints)] 362 | prog = self._format_servo("servoj", tjoints, acc=acc, vel=vel, t=t, lookahead_time=lookahead_time, gain=gain) 363 | self.send_program(prog) 364 | if wait: 365 | self._wait_for_move(tjoints[:6], threshold=threshold, joints=True) 366 | return self.getj() 367 | 368 | def _format_servo(self, command, tjoints, acc=0.01, vel=0.01, t=0.1, lookahead_time=0.2, gain=100, prefix=""): 369 | tjoints = [round(i, self.max_float_length) for i in tjoints] 370 | tjoints.append(acc) 371 | tjoints.append(vel) 372 | tjoints.append(t) 373 | tjoints.append(lookahead_time) 374 | tjoints.append(gain) 375 | return "{}({}[{},{},{},{},{},{}], a={}, v={}, t={}, lookahead_time={}, gain={})".format(command, prefix, *tjoints) 376 | 377 | def _format_move(self, command, tpose, acc, vel, radius=0, prefix=""): 378 | tpose = [round(i, self.max_float_length) for i in tpose] 379 | tpose.append(acc) 380 | tpose.append(vel) 381 | tpose.append(radius) 382 | return "{}({}[{},{},{},{},{},{}], a={}, v={}, r={})".format(command, prefix, *tpose) 383 | 384 | def movex(self, command, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=None): 385 | """ 386 | Send a move command to the robot. since UR robotene have several methods this one 387 | sends whatever is defined in 'command' string 388 | """ 389 | if relative: 390 | l = self.getl() 391 | tpose = [v + l[i] for i, v in enumerate(tpose)] 392 | prog = self._format_move(command, tpose, acc, vel, prefix="p") 393 | self.send_program(prog) 394 | if wait: 395 | self._wait_for_move(tpose[:6], threshold=threshold) 396 | return self.getl() 397 | 398 | def getl(self, wait=False, _log=True): 399 | """ 400 | get TCP position 401 | """ 402 | pose = self.secmon.get_cartesian_info(wait) 403 | if pose: 404 | pose = [pose["X"], pose["Y"], pose["Z"], pose["Rx"], pose["Ry"], pose["Rz"]] 405 | if _log: 406 | self.logger.debug("Received pose from robot: %s", pose) 407 | return pose 408 | 409 | def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True, threshold=None): 410 | """ 411 | Move Circular: Move to position (circular in tool-space) 412 | see UR documentation 413 | """ 414 | pose_via = [round(i, self.max_float_length) for i in pose_via] 415 | pose_to = [round(i, self.max_float_length) for i in pose_to] 416 | prog = "movec(p%s, p%s, a=%s, v=%s, r=%s)" % (pose_via, pose_to, acc, vel, "0") 417 | self.send_program(prog) 418 | if wait: 419 | self._wait_for_move(pose_to, threshold=threshold) 420 | return self.getl() 421 | 422 | def movejs(self, joint_positions_list, acc=0.01, vel=0.01, radius=0.01, 423 | wait=True, threshold=None): 424 | """ 425 | Concatenate several movej commands and applies a blending radius 426 | joint_positions_list is a list of joint_positions. 427 | This method is usefull since any new command from python 428 | to robot make the robot stop 429 | """ 430 | return URRobot.movexs(self, "movej", joint_positions_list, acc, vel, radius, 431 | wait, threshold=threshold) 432 | 433 | def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, 434 | wait=True, threshold=None): 435 | """ 436 | Concatenate several movel commands and applies a blending radius 437 | pose_list is a list of pose. 438 | This method is usefull since any new command from python 439 | to robot make the robot stop 440 | """ 441 | return self.movexs("movel", pose_list, acc, vel, radius, 442 | wait, threshold=threshold) 443 | 444 | def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, 445 | wait=True, threshold=None): 446 | """ 447 | Concatenate several movex commands and applies a blending radius 448 | pose_list is a list of pose. 449 | This method is usefull since any new command from python 450 | to robot make the robot stop 451 | """ 452 | header = "def myProg():\n" 453 | end = "end\n" 454 | prog = header 455 | # Check if 'vel' is a single number or a sequence. 456 | if isinstance(vel, numbers.Number): 457 | # Make 'vel' a sequence 458 | vel = len(pose_list) * [vel] 459 | elif not isinstance(vel, Sequence): 460 | raise RobotException( 461 | 'movexs: "vel" must be a single number or a sequence!') 462 | # Check for adequate number of speeds 463 | if len(vel) != len(pose_list): 464 | raise RobotException( 465 | 'movexs: "vel" must be a number or a list ' 466 | + 'of numbers the same length as "pose_list"!') 467 | # Check if 'radius' is a single number. 468 | if isinstance(radius, numbers.Number): 469 | # Make 'radius' a sequence 470 | radius = len(pose_list) * [radius] 471 | elif not isinstance(radius, Sequence): 472 | raise RobotException( 473 | 'movexs: "radius" must be a single number or a sequence!') 474 | # Ensure that last pose a stopping pose. 475 | radius[-1] = 0.0 476 | # Require adequate number of radii. 477 | if len(radius) != len(pose_list): 478 | raise RobotException( 479 | 'movexs: "radius" must be a number or a list ' 480 | + 'of numbers the same length as "pose_list"!') 481 | prefix = '' 482 | if command in ['movel', 'movec']: 483 | prefix = 'p' 484 | for idx, pose in enumerate(pose_list): 485 | prog += self._format_move(command, pose, acc, 486 | vel[idx], radius[idx], 487 | prefix=prefix) + "\n" 488 | prog += end 489 | self.send_program(prog) 490 | if wait: 491 | if command == 'movel': 492 | self._wait_for_move(target=pose_list[-1], threshold=threshold, joints=False) 493 | elif command == 'movej': 494 | self._wait_for_move(target=pose_list[-1], threshold=threshold, joints=True) 495 | return self.getl() 496 | 497 | def stopl(self, acc=0.5): 498 | self.send_program("stopl(%s)" % acc) 499 | 500 | def stopj(self, acc=1.5): 501 | self.send_program("stopj(%s)" % acc) 502 | 503 | def stop(self): 504 | self.stopj() 505 | 506 | def close(self): 507 | """ 508 | close connection to robot and stop internal thread 509 | """ 510 | self.logger.info("Closing sockets to robot") 511 | self.secmon.close() 512 | if self.rtmon: 513 | self.rtmon.stop() 514 | 515 | def set_freedrive(self, val, timeout=60): 516 | """ 517 | set robot in freedrive/backdrive mode where an operator can jog 518 | the robot to wished pose. 519 | 520 | Freedrive will timeout at 60 seconds. 521 | """ 522 | if val: 523 | self.send_program("def myProg():\n\tfreedrive_mode()\n\tsleep({})\nend".format(timeout)) 524 | else: 525 | # This is a non-existant program, but running it will stop freedrive 526 | self.send_program("def myProg():\n\tend_freedrive_mode()\nend") 527 | 528 | def set_simulation(self, val): 529 | if val: 530 | self.send_program("set sim") 531 | else: 532 | self.send_program("set real") 533 | 534 | def get_realtime_monitor(self): 535 | """ 536 | return a pointer to the realtime monitor object 537 | usefull to track robot position for example 538 | """ 539 | if not self.rtmon: 540 | self.logger.info("Opening real-time monitor socket") 541 | self.rtmon = urrtmon.URRTMonitor(self.host, self.urFirm) # som information is only available on rt interface 542 | self.rtmon.start() 543 | self.rtmon.set_csys(self.csys) 544 | return self.rtmon 545 | 546 | def translate(self, vect, acc=0.01, vel=0.01, wait=True, command="movel"): 547 | """ 548 | move tool in base coordinate, keeping orientation 549 | """ 550 | p = self.getl() 551 | p[0] += vect[0] 552 | p[1] += vect[1] 553 | p[2] += vect[2] 554 | return self.movex(command, p, vel=vel, acc=acc, wait=wait) 555 | 556 | def up(self, z=0.05, acc=0.01, vel=0.01): 557 | """ 558 | Move up in csys z 559 | """ 560 | p = self.getl() 561 | p[2] += z 562 | self.movel(p, acc=acc, vel=vel) 563 | 564 | def down(self, z=0.05, acc=0.01, vel=0.01): 565 | """ 566 | Move down in csys z 567 | """ 568 | self.up(-z, acc, vel) 569 | -------------------------------------------------------------------------------- /urx/urrtmon.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Module for implementing a UR controller real-time monitor over socket port 30003. 3 | Confer http://support.universal-robots.com/Technical/RealTimeClientInterface 4 | Note: The packet lenght given in the web-page is 740. What is actually received from the controller is 692. 5 | It is assumed that the motor currents, the last group of 48 bytes, are not send. 6 | Originally Written by Morten Lind 7 | Parsing for Firmware 5.9 is added by Byeongdu Lee 8 | ''' 9 | import logging 10 | import socket 11 | import struct 12 | import time 13 | import threading 14 | from copy import deepcopy 15 | 16 | import numpy as np 17 | 18 | import math3d as m3d 19 | 20 | __author__ = "Morten Lind, Olivier Roulet-Dubonnet" 21 | __copyright__ = "Copyright 2011, NTNU/SINTEF Raufoss Manufacturing AS" 22 | __credits__ = ["Morten Lind, Olivier Roulet-Dubonnet"] 23 | __license__ = "LGPLv3" 24 | 25 | 26 | class URRTMonitor(threading.Thread): 27 | 28 | # Struct for revision of the UR controller giving 692 bytes 29 | rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ') 30 | 31 | # for revision of the UR controller giving 540 byte. Here TCP 32 | # pose is not included! 33 | rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d') 34 | 35 | rtstruct5_1 = struct.Struct('>d1d6d6d6d6d6d6d6d6d6d6d6d6d6d6d1d6d1d1d1d6d1d6d3d6d1d1d1d1d1d1d1d6d1d1d3d3d') 36 | rtstruct5_9 = struct.Struct('>d6d6d6d6d6d6d6d6d6d6d6d6d6d6d1d6d1d1d1d6d1d6d3d6d1d1d1d1d1d1d1d6d1d1d3d3d1d') 37 | 38 | def __init__(self, urHost, urFirm=None): 39 | threading.Thread.__init__(self) 40 | self.logger = logging.getLogger(self.__class__.__name__) 41 | self.daemon = True 42 | self._stop_event = True 43 | self._dataEvent = threading.Condition() 44 | self._dataAccess = threading.Lock() 45 | self._rtSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 46 | self._rtSock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) 47 | self._urHost = urHost 48 | self.urFirm = urFirm 49 | # Package data variables 50 | self._timestamp = None 51 | self._ctrlTimestamp = None 52 | self._qActual = None 53 | self._qTarget = None 54 | self._tcp = None 55 | self._tcp_force = None 56 | self._joint_temperature = None 57 | self._joint_voltage = None 58 | self._joint_current = None 59 | self._main_voltage = None 60 | self._robot_voltage = None 61 | self._robot_current = None 62 | self._qdTarget = None 63 | self._qddTarget = None 64 | self._iTarget = None 65 | self._mTarget = None 66 | self._qdActual = None 67 | self._tcp_speed = None 68 | self._tcp = None 69 | self._robot_mode = None 70 | self._joint_modes = None 71 | self._digital_outputs = None 72 | self._program_state = None 73 | self._safety_status = None 74 | 75 | self.__recvTime = 0 76 | self._last_ctrl_ts = 0 77 | # self._last_ts = 0 78 | self._buffering = False 79 | self._buffer_lock = threading.Lock() 80 | self._buffer = [] 81 | self._csys = None 82 | self._csys_lock = threading.Lock() 83 | 84 | def set_csys(self, csys): 85 | with self._csys_lock: 86 | self._csys = csys 87 | 88 | def __recv_bytes(self, nBytes): 89 | ''' Facility method for receiving exactly "nBytes" bytes from 90 | the robot connector socket.''' 91 | # Record the time of arrival of the first of the stream block 92 | recvTime = 0 93 | pkg = b'' 94 | while len(pkg) < nBytes: 95 | pkg += self._rtSock.recv(nBytes - len(pkg)) 96 | if recvTime == 0: 97 | recvTime = time.time() 98 | self.__recvTime = recvTime 99 | return pkg 100 | 101 | def wait(self): 102 | with self._dataEvent: 103 | self._dataEvent.wait() 104 | 105 | def q_actual(self, wait=False, timestamp=False): 106 | """ Get the actual joint position vector.""" 107 | if wait: 108 | self.wait() 109 | with self._dataAccess: 110 | if timestamp: 111 | return self._timestamp, self._qActual 112 | else: 113 | return self._qActual 114 | getActual = q_actual 115 | 116 | def qd_actual(self, wait=False, timestamp=False): 117 | """ Get the actual joint velocity vector.""" 118 | if wait: 119 | self.wait() 120 | with self._dataAccess: 121 | if timestamp: 122 | return self._timestamp, self._qdActual 123 | else: 124 | return self._qdActual 125 | 126 | def q_target(self, wait=False, timestamp=False): 127 | """ Get the target joint position vector.""" 128 | if wait: 129 | self.wait() 130 | with self._dataAccess: 131 | if timestamp: 132 | return self._timestamp, self._qTarget 133 | else: 134 | return self._qTarget 135 | getTarget = q_target 136 | 137 | def tcf_pose(self, wait=False, timestamp=False, ctrlTimestamp=False): 138 | """ Return the tool pose values.""" 139 | if wait: 140 | self.wait() 141 | with self._dataAccess: 142 | tcf = self._tcp 143 | if ctrlTimestamp or timestamp: 144 | ret = [tcf] 145 | if timestamp: 146 | ret.insert(-1, self._timestamp) 147 | if ctrlTimestamp: 148 | ret.insert(-1, self._ctrlTimestamp) 149 | return ret 150 | else: 151 | return tcf 152 | getTCF = tcf_pose 153 | 154 | def tcf_force(self, wait=False, timestamp=False): 155 | """ Get the tool force. The returned tool force is a 156 | six-vector of three forces and three moments.""" 157 | if wait: 158 | self.wait() 159 | with self._dataAccess: 160 | # tcf = self._fwkin(self._qActual) 161 | tcf_force = self._tcp_force 162 | if timestamp: 163 | return self._timestamp, tcf_force 164 | else: 165 | return tcf_force 166 | getTCFForce = tcf_force 167 | 168 | def joint_temperature(self, wait=False, timestamp=False): 169 | """ Get the joint temperature.""" 170 | if wait: 171 | self.wait() 172 | with self._dataAccess: 173 | joint_temperature = self._joint_temperature 174 | if timestamp: 175 | return self._timestamp, joint_temperature 176 | else: 177 | return joint_temperature 178 | getJOINTTemperature = joint_temperature 179 | 180 | def joint_voltage(self, wait=False, timestamp=False): 181 | """ Get the joint voltage.""" 182 | if wait: 183 | self.wait() 184 | with self._dataAccess: 185 | joint_voltage = self._joint_voltage 186 | if timestamp: 187 | return self._timestamp, joint_voltage 188 | else: 189 | return joint_voltage 190 | getJOINTVoltage = joint_voltage 191 | 192 | def joint_current(self, wait=False, timestamp=False): 193 | """ Get the joint current.""" 194 | if wait: 195 | self.wait() 196 | with self._dataAccess: 197 | joint_current = self._joint_current 198 | if timestamp: 199 | return self._timestamp, joint_current 200 | else: 201 | return joint_current 202 | getJOINTCurrent = joint_current 203 | 204 | def main_voltage(self, wait=False, timestamp=False): 205 | """ Get the Safety Control Board: Main voltage.""" 206 | if wait: 207 | self.wait() 208 | with self._dataAccess: 209 | main_voltage = self._main_voltage 210 | if timestamp: 211 | return self._timestamp, main_voltage 212 | else: 213 | return main_voltage 214 | getMAINVoltage = main_voltage 215 | 216 | def robot_voltage(self, wait=False, timestamp=False): 217 | """ Get the Safety Control Board: Robot voltage (48V).""" 218 | if wait: 219 | self.wait() 220 | with self._dataAccess: 221 | robot_voltage = self._robot_voltage 222 | if timestamp: 223 | return self._timestamp, robot_voltage 224 | else: 225 | return robot_voltage 226 | getROBOTVoltage = robot_voltage 227 | 228 | def robot_current(self, wait=False, timestamp=False): 229 | """ Get the Safety Control Board: Robot current.""" 230 | if wait: 231 | self.wait() 232 | with self._dataAccess: 233 | robot_current = self._robot_current 234 | if timestamp: 235 | return self._timestamp, robot_current 236 | else: 237 | return robot_current 238 | getROBOTCurrent = robot_current 239 | 240 | def __recv_rt_data(self): 241 | head = self.__recv_bytes(4) 242 | # Record the timestamp for this logical package 243 | timestamp = self.__recvTime 244 | pkgsize = struct.unpack('>i', head)[0] 245 | self.logger.debug( 246 | 'Received header telling that package is %s bytes long', 247 | pkgsize) 248 | payload = self.__recv_bytes(pkgsize - 4) 249 | if self.urFirm is not None: 250 | if self.urFirm == 5.1: 251 | unp = self.rtstruct5_1.unpack(payload[:self.rtstruct5_1.size]) 252 | if self.urFirm == 5.9: 253 | unp = self.rtstruct5_9.unpack(payload[:self.rtstruct5_9.size]) 254 | else: 255 | if pkgsize >= 692: 256 | unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size]) 257 | elif pkgsize >= 540: 258 | unp = self.rtstruct540.unpack(payload[:self.rtstruct540.size]) 259 | else: 260 | self.logger.warning( 261 | 'Error, Received packet of length smaller than 540: %s ', 262 | pkgsize) 263 | return 264 | 265 | 266 | with self._dataAccess: 267 | self._timestamp = timestamp 268 | # it seems that packet often arrives packed as two... maybe TCP_NODELAY is not set on UR controller?? 269 | # if (self._timestamp - self._last_ts) > 0.010: 270 | # self.logger.warning("Error the we did not receive a packet for {}s ".format( self._timestamp - self._last_ts)) 271 | # self._last_ts = self._timestamp 272 | self._ctrlTimestamp = np.array(unp[0]) 273 | if self._last_ctrl_ts != 0 and ( 274 | self._ctrlTimestamp - 275 | self._last_ctrl_ts) > 0.010: 276 | self.logger.warning( 277 | "Error the controller failed to send us a packet: time since last packet %s s ", 278 | self._ctrlTimestamp - self._last_ctrl_ts) 279 | self._last_ctrl_ts = self._ctrlTimestamp 280 | self._qActual = np.array(unp[31:37]) 281 | self._qdActual = np.array(unp[37:43]) 282 | self._qTarget = np.array(unp[1:7]) 283 | self._tcp_force = np.array(unp[67:73]) 284 | self._tcp = np.array(unp[73:79]) 285 | self._joint_current = np.array(unp[43:49]) 286 | if self.urFirm >= 3.1: 287 | self._joint_temperature = np.array(unp[86:92]) 288 | self._joint_voltage = np.array(unp[124:130]) 289 | self._main_voltage = unp[121] 290 | self._robot_voltage = unp[122] 291 | self._robot_current = unp[123] 292 | 293 | if self.urFirm>= 5.9: 294 | self._qdTarget = np.array(unp[7:13]) 295 | self._qddTarget = np.array(unp[13:19]) 296 | self._iTarget = np.array(unp[19:25]) 297 | self._mTarget = np.array(unp[25:31]) 298 | self._tcp_speed = np.array(unp[61:67]) 299 | self._joint_current = np.array(unp[49:55]) 300 | self._joint_voltage = np.array(unp[124:130]) 301 | self._robot_mode = unp[94] 302 | self._joint_modes = np.array(unp[95:101]) 303 | self._digital_outputs = unp[130] 304 | self._program_state = unp[131] 305 | self._safety_status = unp[138] 306 | 307 | if self._csys: 308 | with self._csys_lock: 309 | # might be a godd idea to remove dependancy on m3d 310 | tcp = self._csys.inverse * m3d.Transform(self._tcp) 311 | self._tcp = tcp.pose_vector 312 | if self._buffering: 313 | with self._buffer_lock: 314 | self._buffer.append( 315 | (self._timestamp, 316 | self._ctrlTimestamp, 317 | self._tcp, 318 | self._qActual)) # FIXME use named arrays of allow to configure what data to buffer 319 | 320 | with self._dataEvent: 321 | self._dataEvent.notifyAll() 322 | 323 | def start_buffering(self): 324 | """ 325 | start buffering all data from controller 326 | """ 327 | self._buffer = [] 328 | self._buffering = True 329 | 330 | def stop_buffering(self): 331 | self._buffering = False 332 | 333 | def try_pop_buffer(self): 334 | """ 335 | return oldest value in buffer 336 | """ 337 | with self._buffer_lock: 338 | if len(self._buffer) > 0: 339 | return self._buffer.pop(0) 340 | else: 341 | return None 342 | 343 | def pop_buffer(self): 344 | """ 345 | return oldest value in buffer 346 | """ 347 | while True: 348 | with self._buffer_lock: 349 | if len(self._buffer) > 0: 350 | return self._buffer.pop(0) 351 | time.sleep(0.001) 352 | 353 | def get_buffer(self): 354 | """ 355 | return a copy of the entire buffer 356 | """ 357 | with self._buffer_lock: 358 | return deepcopy(self._buffer) 359 | 360 | def get_all_data(self, wait=True): 361 | """ 362 | return all data parsed from robot as a dict 363 | """ 364 | if wait: 365 | self.wait() 366 | with self._dataAccess: 367 | return dict( 368 | timestamp=self._timestamp, 369 | ctrltimestamp=self._ctrlTimestamp, 370 | qActual=self._qActual, 371 | qTarget=self._qTarget, 372 | qdActual=self._qdActual, 373 | qdTarget=self._qdTarget, 374 | tcp=self._tcp, 375 | tcp_force=self._tcp_force, 376 | tcp_speed=self._tcp_speed, 377 | joint_temperature=self._joint_temperature, 378 | joint_voltage=self._joint_voltage, 379 | joint_current=self._joint_current, 380 | joint_modes=self._joint_modes, 381 | robot_modes=self._robot_mode, 382 | main_voltage=self._main_voltage, 383 | robot_voltage=self._robot_voltage, 384 | robot_current=self._robot_current, 385 | digital_outputs=self._digital_outputs, 386 | program_state=self._program_state, 387 | safety_status=self._safety_status) 388 | getALLData = get_all_data 389 | 390 | def stop(self): 391 | # print(self.__class__.__name__+': Stopping') 392 | self._stop_event = True 393 | 394 | def close(self): 395 | self.stop() 396 | self.join() 397 | 398 | def run(self): 399 | self._stop_event = False 400 | self._rtSock.connect((self._urHost, 30003)) 401 | while not self._stop_event: 402 | self.__recv_rt_data() 403 | self._rtSock.close() 404 | -------------------------------------------------------------------------------- /urx/urscript.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import logging 4 | 5 | 6 | # Controller Settings 7 | CONTROLLER_PORTS = [0, 1] 8 | CONTROLLER_VOLTAGE = [ 9 | 0, # 0-5V 10 | 2, # 0-10V 11 | ] 12 | 13 | # Tool Settings 14 | TOOL_PORTS = [2, 3] 15 | TOOL_VOLTAGE = [ 16 | 0, # 0-5V 17 | 1, # 0-10V 18 | 2, # 4-20mA 19 | ] 20 | 21 | OUTPUT_DOMAIN_VOLTAGE = [ 22 | 0, # 4-20mA 23 | 1, # 0-10V 24 | ] 25 | 26 | 27 | class URScript(object): 28 | 29 | def __init__(self): 30 | self.logger = logging.getLogger(u"urscript") 31 | # The header is code that is before and outside the myProg() method 32 | self.header = "" 33 | # The program is code inside the myProg() method 34 | self.program = "" 35 | 36 | def __call__(self): 37 | if(self.program == ""): 38 | self.logger.debug(u"urscript program is empty") 39 | return "" 40 | 41 | # Construct the program 42 | myprog = """def myProg():{}\nend""".format(self.program) 43 | 44 | # Construct the full script 45 | script = "" 46 | if self.header: 47 | script = "{}\n\n".format(self.header) 48 | script = "{}{}".format(script, myprog) 49 | return script 50 | 51 | def reset(self): 52 | self.header = "" 53 | self.program = "" 54 | 55 | def add_header_to_program(self, header_line): 56 | self.header = "{}\n{}".format(self.header, header_line) 57 | 58 | def add_line_to_program(self, new_line): 59 | self.program = "{}\n\t{}".format(self.program, new_line) 60 | 61 | def _constrain_unsigned_char(self, value): 62 | """ 63 | Ensure that unsigned char values are constrained 64 | to between 0 and 255. 65 | """ 66 | assert(isinstance(value, int)) 67 | if value < 0: 68 | value = 0 69 | elif value > 255: 70 | value = 255 71 | return value 72 | 73 | def _set_analog_inputrange(self, port, vrange): 74 | if port in CONTROLLER_PORTS: 75 | assert(vrange in CONTROLLER_VOLTAGE) 76 | elif port in TOOL_PORTS: 77 | assert(vrange in TOOL_VOLTAGE) 78 | msg = "set_analog_inputrange({},{})".format(port, vrange) 79 | self.add_line_to_program(msg) 80 | 81 | def _set_analog_output(self, input_id, signal_level): 82 | assert(input_id in [0, 1]) 83 | assert(signal_level in [0, 1]) 84 | msg = "set_analog_output({}, {})".format(input_id, signal_level) 85 | self.add_line_to_program(msg) 86 | 87 | def _set_analog_outputdomain(self, port, domain): 88 | assert(domain in OUTPUT_DOMAIN_VOLTAGE) 89 | msg = "set_analog_outputdomain({},{})".format(port, domain) 90 | self.add_line_to_program(msg) 91 | 92 | def _set_payload(self, mass, cog=None): 93 | msg = "set_payload({}".format(mass) 94 | if cog: 95 | assert(len(cog) == 3) 96 | msg = "{},{}".format(msg, cog) 97 | msg = "{})".format(msg) 98 | self.add_line_to_program(msg) 99 | 100 | def _set_runstate_outputs(self, outputs=None): 101 | if not outputs: 102 | outputs = [] 103 | msg = "set_runstate_outputs({})".format(outputs) 104 | self.add_line_to_program(msg) 105 | 106 | def _set_tool_voltage(self, voltage): 107 | assert(voltage in [0, 12, 24]) 108 | msg = "set_tool_voltage({})".format(voltage) 109 | self.add_line_to_program(msg) 110 | 111 | def _sleep(self, value): 112 | msg = "sleep({})".format(value) 113 | self.add_line_to_program(msg) 114 | 115 | def _socket_close(self, socket_name): 116 | msg = "socket_close(\"{}\")".format(socket_name) 117 | self.add_line_to_program(msg) 118 | 119 | def _socket_get_var(self, var, socket_name): 120 | msg = "socket_get_var(\"{}\",\"{}\")".format(var, socket_name) 121 | self.add_line_to_program(msg) 122 | self._sync() 123 | 124 | def _socket_open(self, socket_host, socket_port, socket_name): 125 | msg = "socket_open(\"{}\",{},\"{}\")".format(socket_host, 126 | socket_port, 127 | socket_name) 128 | self.add_line_to_program(msg) 129 | 130 | def _socket_read_byte_list(self, nbytes, socket_name): 131 | msg = "global var_value = socket_read_byte_list({},\"{}\")".format(nbytes, socket_name) # noqa 132 | self.add_line_to_program(msg) 133 | self._sync() 134 | 135 | def _socket_send_string(self, message, socket_name): 136 | msg = "socket_send_string(\"{}\",\"{}\")".format(message, socket_name) # noqa 137 | self.add_line_to_program(msg) 138 | self._sync() 139 | 140 | def _socket_set_var(self, var, value, socket_name): 141 | msg = "socket_set_var(\"{}\",{},\"{}\")".format(var, value, socket_name) # noqa 142 | self.add_line_to_program(msg) 143 | self._sync() 144 | 145 | def _socket_get_var2var(self, var, varout, socket_name, prefix = ''): 146 | msg = "{}{} = socket_get_var(\"{}\",\"{}\")".format(prefix, varout, var, socket_name) 147 | self.add_line_to_program(msg) 148 | 149 | def _socket_send_byte(self, byte, socket_name): 150 | msg = "socket_send_byte(\"{}\",\"{}\")".format(str(byte), socket_name) # noqa 151 | self.add_line_to_program(msg) 152 | self._sync() 153 | 154 | def _sync(self): 155 | msg = "sync()" 156 | self.add_line_to_program(msg) 157 | -------------------------------------------------------------------------------- /urx/ursecmon.py: -------------------------------------------------------------------------------- 1 | """ 2 | This file contains 2 classes: 3 | - ParseUtils containing utilies to parse data from UR robot 4 | - SecondaryMonitor, a class opening a socket to the robot and with methods to 5 | access data and send programs to the robot 6 | Both use data from the secondary port of the URRobot. 7 | Only the last connected socket on 3001 is the primary client !!!! 8 | So do not rely on it unless you know no other client is running (Hint the UR java interface is a client...) 9 | http://support.universal-robots.com/Technical/PrimaryAndSecondaryClientInterface 10 | """ 11 | 12 | 13 | from threading import Thread, Condition, Lock 14 | import logging 15 | import struct 16 | import socket 17 | from copy import copy 18 | import time 19 | 20 | __author__ = "Olivier Roulet-Dubonnet" 21 | __copyright__ = "Copyright 2011-2013, Sintef Raufoss Manufacturing" 22 | __credits__ = ["Olivier Roulet-Dubonnet"] 23 | __license__ = "LGPLv3" 24 | 25 | 26 | class ParsingException(Exception): 27 | 28 | def __init__(self, *args): 29 | Exception.__init__(self, *args) 30 | 31 | 32 | class Program(object): 33 | def __init__(self, prog): 34 | self.program = prog 35 | self.condition = Condition() 36 | 37 | def __str__(self): 38 | return "Program({})".format(self.program) 39 | __repr__ = __str__ 40 | 41 | 42 | class TimeoutException(Exception): 43 | 44 | def __init__(self, *args): 45 | Exception.__init__(self, *args) 46 | 47 | 48 | class ParserUtils(object): 49 | 50 | def __init__(self): 51 | self.logger = logging.getLogger("ursecmon") 52 | self.version = (0, 0) 53 | 54 | def parse(self, data): 55 | """ 56 | parse a packet from the UR socket and return a dictionary with the data 57 | """ 58 | allData = {} 59 | # print "Total size ", len(data) 60 | while data: 61 | psize, ptype, pdata, data = self.analyze_header(data) 62 | # print "We got packet with size %i and type %s" % (psize, ptype) 63 | if ptype == 16: 64 | allData["SecondaryClientData"] = self._get_data(pdata, "!iB", ("size", "type")) 65 | data = (pdata + data)[5:] # This is the total size so we resend data to parser 66 | elif ptype == 0: 67 | # this parses RobotModeData for versions >=3.0 (i.e. 3.0) 68 | if psize == 38: 69 | self.version = (3, 0) 70 | allData['RobotModeData'] = self._get_data(pdata, "!IBQ???????BBdd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "controlMode", "speedFraction", "speedScaling")) 71 | elif psize == 46: # It's 46 bytes in 3.2 72 | self.version = (3, 2) 73 | allData['RobotModeData'] = self._get_data(pdata, "!IBQ???????BBdd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "controlMode", "speedFraction", "speedScaling", "speedFractionLimit")) 74 | elif psize == 47: 75 | self.version = (3, 5) 76 | allData['RobotModeData'] = self._get_data(pdata, "!IBQ???????BBddc", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "controlMode", "speedFraction", "speedScaling", "speedFractionLimit", "reservedByUR")) 77 | else: 78 | allData["RobotModeData"] = self._get_data(pdata, "!iBQ???????Bd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "speedFraction")) 79 | elif ptype == 1: 80 | tmpstr = ["size", "type"] 81 | for i in range(0, 6): 82 | tmpstr += ["q_actual%s" % i, "q_target%s" % i, "qd_actual%s" % i, "I_actual%s" % i, "V_actual%s" % i, "T_motor%s" % i, "T_micro%s" % i, "jointMode%s" % i] 83 | 84 | allData["JointData"] = self._get_data(pdata, "!iB dddffffB dddffffB dddffffB dddffffB dddffffB dddffffB", tmpstr) 85 | 86 | elif ptype == 4: 87 | if self.version < (3, 2): 88 | allData["CartesianInfo"] = self._get_data(pdata, "iBdddddd", ("size", "type", "X", "Y", "Z", "Rx", "Ry", "Rz")) 89 | else: 90 | allData["CartesianInfo"] = self._get_data(pdata, "iBdddddddddddd", ("size", "type", "X", "Y", "Z", "Rx", "Ry", "Rz", "tcpOffsetX", "tcpOffsetY", "tcpOffsetZ", "tcpOffsetRx", "tcpOffsetRy", "tcpOffsetRz")) 91 | elif ptype == 5: 92 | allData["LaserPointer(OBSOLETE)"] = self._get_data(pdata, "iBddd", ("size", "type")) 93 | elif ptype == 3: 94 | 95 | if self.version >= (3, 0): 96 | fmt = "iBiibbddbbddffffBBb" # firmware >= 3.0 97 | else: 98 | fmt = "iBhhbbddbbddffffBBb" # firmware < 3.0 99 | 100 | allData["MasterBoardData"] = self._get_data(pdata, fmt, ("size", "type", "digitalInputBits", "digitalOutputBits", "analogInputRange0", "analogInputRange1", "analogInput0", "analogInput1", "analogInputDomain0", "analogInputDomain1", "analogOutput0", "analogOutput1", "masterBoardTemperature", "robotVoltage48V", "robotCurrent", "masterIOCurrent")) # , "masterSafetyState" ,"masterOnOffState", "euromap67InterfaceInstalled" )) 101 | elif ptype == 2: 102 | allData["ToolData"] = self._get_data(pdata, "iBbbddfBffB", ("size", "type", "analoginputRange2", "analoginputRange3", "analogInput2", "analogInput3", "toolVoltage48V", "toolOutputVoltage", "toolCurrent", "toolTemperature", "toolMode")) 103 | elif ptype == 9: 104 | continue # This package has a length of 53 bytes. It is used internally by Universal Robots software only and should be skipped. 105 | elif ptype == 8 and self.version >= (3, 2): 106 | allData["AdditionalInfo"] = self._get_data(pdata, "iB??", ("size", "type", "teachButtonPressed", "teachButtonEnabled")) 107 | elif ptype == 7 and self.version >= (3, 2): 108 | allData["ForceModeData"] = self._get_data(pdata, "iBddddddd", ("size", "type", "x", "y", "z", "rx", "ry", "rz", "robotDexterity")) 109 | # elif ptype == 8: 110 | # allData["varMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) 111 | # elif ptype == 7: 112 | # allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) 113 | 114 | elif ptype == 20: 115 | tmp = self._get_data(pdata, "!iB Qbb", ("size", "type", "timestamp", "source", "robotMessageType")) 116 | if tmp["robotMessageType"] == 3: 117 | allData["VersionMessage"] = self._get_data(pdata, "!iBQbb bAbBBiAb", ("size", "type", "timestamp", "source", "robotMessageType", "projectNameSize", "projectName", "majorVersion", "minorVersion", "svnRevision", "buildDate")) 118 | elif tmp["robotMessageType"] == 6: 119 | allData["robotCommMessage"] = self._get_data(pdata, "!iBQbb iiAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "messageText")) 120 | elif tmp["robotMessageType"] == 1: 121 | allData["labelMessage"] = self._get_data(pdata, "!iBQbb iAc", ("size", "type", "timestamp", "source", "robotMessageType", "id", "messageText")) 122 | elif tmp["robotMessageType"] == 2: 123 | allData["popupMessage"] = self._get_data(pdata, "!iBQbb ??BAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "warning", "error", "titleSize", "messageTitle", "messageText")) 124 | elif tmp["robotMessageType"] == 0: 125 | allData["messageText"] = self._get_data(pdata, "!iBQbb Ac", ("size", "type", "timestamp", "source", "robotMessageType", "messageText")) 126 | elif tmp["robotMessageType"] == 8: 127 | allData["varMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) 128 | elif tmp["robotMessageType"] == 7: 129 | allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) 130 | elif tmp["robotMessageType"] == 5: 131 | allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "messageText")) 132 | else: 133 | self.logger.debug("Message type parser not implemented %s", tmp) 134 | else: 135 | self.logger.debug("Unknown packet type %s with size %s", ptype, psize) 136 | 137 | return allData 138 | 139 | def _get_data(self, data, fmt, names): 140 | """ 141 | fill data into a dictionary 142 | data is data from robot packet 143 | fmt is struct format, but with added A for arrays and no support for numerical in fmt 144 | names args are strings used to store values 145 | """ 146 | tmpdata = copy(data) 147 | fmt = fmt.strip() # space may confuse us 148 | d = dict() 149 | i = 0 150 | j = 0 151 | while j < len(fmt) and i < len(names): 152 | f = fmt[j] 153 | if f in (" ", "!", ">", "<"): 154 | j += 1 155 | elif f == "A": # we got an array 156 | # first we need to find its size 157 | if j == len(fmt) - 2: # we are last element, size is the rest of data in packet 158 | arraysize = len(tmpdata) 159 | else: # size should be given in last element 160 | asn = names[i - 1] 161 | if not asn.endswith("Size"): 162 | raise ParsingException("Error, array without size ! %s %s" % (asn, i)) 163 | else: 164 | arraysize = d[asn] 165 | d[names[i]] = tmpdata[0:arraysize] 166 | # print "Array is ", names[i], d[names[i]] 167 | tmpdata = tmpdata[arraysize:] 168 | j += 2 169 | i += 1 170 | else: 171 | fmtsize = struct.calcsize(fmt[j]) 172 | # print "reading ", f , i, j, fmtsize, len(tmpdata) 173 | if len(tmpdata) < fmtsize: # seems to happen on windows 174 | raise ParsingException("Error, length of data smaller than advertized: ", len(tmpdata), fmtsize, "for names ", names, f, i, j) 175 | d[names[i]] = struct.unpack("!" + f, tmpdata[0:fmtsize])[0] 176 | # print names[i], d[names[i]] 177 | tmpdata = tmpdata[fmtsize:] 178 | j += 1 179 | i += 1 180 | return d 181 | 182 | def get_header(self, data): 183 | return struct.unpack("!iB", data[0:5]) 184 | 185 | def analyze_header(self, data): 186 | """ 187 | read first 5 bytes and return complete packet 188 | """ 189 | if len(data) < 5: 190 | raise ParsingException("Packet size %s smaller than header size (5 bytes)" % len(data)) 191 | else: 192 | psize, ptype = self.get_header(data) 193 | if psize < 5: 194 | raise ParsingException("Error, declared length of data smaller than its own header(5): ", psize) 195 | elif psize > len(data): 196 | raise ParsingException("Error, length of data smaller (%s) than declared (%s)" % (len(data), psize)) 197 | return psize, ptype, data[:psize], data[psize:] 198 | 199 | def find_first_packet(self, data): 200 | """ 201 | find the first complete packet in a string 202 | returns None if none found 203 | """ 204 | counter = 0 205 | limit = 10 206 | while True: 207 | if len(data) >= 5: 208 | psize, ptype = self.get_header(data) 209 | if psize < 5 or psize > 2000 or ptype != 16: 210 | data = data[1:] 211 | counter += 1 212 | if counter > limit: 213 | self.logger.warning("tried %s times to find a packet in data, advertised packet size: %s, type: %s", counter, psize, ptype) 214 | self.logger.warning("Data length: %s", len(data)) 215 | limit = limit * 10 216 | elif len(data) >= psize: 217 | self.logger.debug("Got packet with size %s and type %s", psize, ptype) 218 | if counter: 219 | self.logger.info("Remove %s bytes of garbage at begining of packet", counter) 220 | # ok we we have somehting which looks like a packet" 221 | return (data[:psize], data[psize:]) 222 | else: 223 | # packet is not complete 224 | self.logger.debug("Packet is not complete, advertised size is %s, received size is %s, type is %s", psize, len(data), ptype) 225 | return None 226 | else: 227 | # self.logger.debug("data smaller than 5 bytes") 228 | return None 229 | 230 | 231 | class SecondaryMonitor(Thread): 232 | 233 | """ 234 | Monitor data from secondary port and send programs to robot 235 | """ 236 | 237 | def __init__(self, host): 238 | Thread.__init__(self) 239 | self.logger = logging.getLogger("ursecmon") 240 | self._parser = ParserUtils() 241 | self._dict = {} 242 | self._dictLock = Lock() 243 | self.host = host 244 | secondary_port = 30002 # Secondary client interface on Universal Robots 245 | self._s_secondary = socket.create_connection((self.host, secondary_port), timeout=0.5) 246 | self._prog_queue = [] 247 | self._prog_queue_lock = Lock() 248 | self._dataqueue = bytes() 249 | self._trystop = False # to stop thread 250 | self.running = False # True when robot is on and listening 251 | self._dataEvent = Condition() 252 | self.lastpacket_timestamp = 0 253 | 254 | self.start() 255 | try: 256 | self.wait() # make sure we got some data before someone calls us 257 | except TimeoutException as ex: 258 | self.close() 259 | raise ex 260 | 261 | def send_program(self, prog): 262 | """ 263 | send program to robot in URRobot format 264 | If another program is send while a program is running the first program is aborded. 265 | """ 266 | prog.strip() 267 | self.logger.debug("Enqueueing program: %s", prog) 268 | if not isinstance(prog, bytes): 269 | prog = prog.encode() 270 | 271 | data = Program(prog + b"\n") 272 | with data.condition: 273 | with self._prog_queue_lock: 274 | self._prog_queue.append(data) 275 | data.condition.wait() 276 | self.logger.debug("program sendt: %s", data) 277 | 278 | def run(self): 279 | """ 280 | check program execution status in the secondary client data packet we get from the robot 281 | This interface uses only data from the secondary client interface (see UR doc) 282 | Only the last connected client is the primary client, 283 | so this is not guaranted and we cannot rely on information to the primary client. 284 | """ 285 | while not self._trystop: 286 | with self._prog_queue_lock: 287 | if len(self._prog_queue) > 0: 288 | data = self._prog_queue.pop(0) 289 | self._s_secondary.send(data.program) 290 | with data.condition: 291 | data.condition.notify_all() 292 | 293 | data = self._get_data() 294 | try: 295 | tmpdict = self._parser.parse(data) 296 | with self._dictLock: 297 | self._dict = tmpdict 298 | except ParsingException as ex: 299 | self.logger.warning("Error parsing one packet from urrobot: %s", ex) 300 | continue 301 | 302 | if "RobotModeData" not in self._dict: 303 | self.logger.warning("Got a packet from robot without RobotModeData, strange ...") 304 | continue 305 | 306 | self.lastpacket_timestamp = time.time() 307 | 308 | rmode = 0 309 | if self._parser.version >= (3, 0): 310 | rmode = 7 311 | 312 | if self._dict["RobotModeData"]["robotMode"] == rmode \ 313 | and self._dict["RobotModeData"]["isRealRobotEnabled"] is True \ 314 | and self._dict["RobotModeData"]["isEmergencyStopped"] is False \ 315 | and self._dict["RobotModeData"]["isSecurityStopped"] is False \ 316 | and self._dict["RobotModeData"]["isRobotConnected"] is True \ 317 | and self._dict["RobotModeData"]["isPowerOnRobot"] is True: 318 | self.running = True 319 | else: 320 | if self.running: 321 | self.logger.error("Robot not running: " + str(self._dict["RobotModeData"])) 322 | self.running = False 323 | with self._dataEvent: 324 | # print("X: new data") 325 | self._dataEvent.notifyAll() 326 | 327 | def _get_data(self): 328 | """ 329 | returns something that looks like a packet, nothing is guaranted 330 | """ 331 | while True: 332 | # self.logger.debug("data queue size is: {}".format(len(self._dataqueue))) 333 | ans = self._parser.find_first_packet(self._dataqueue[:]) 334 | if ans: 335 | self._dataqueue = ans[1] 336 | # self.logger.debug("found packet of size {}".format(len(ans[0]))) 337 | return ans[0] 338 | else: 339 | # self.logger.debug("Could not find packet in received data") 340 | tmp = self._s_secondary.recv(1024) 341 | self._dataqueue += tmp 342 | 343 | def wait(self, timeout=0.5): 344 | """ 345 | wait for next data packet from robot 346 | """ 347 | tstamp = self.lastpacket_timestamp 348 | with self._dataEvent: 349 | self._dataEvent.wait(timeout) 350 | if tstamp == self.lastpacket_timestamp: 351 | raise TimeoutException("Did not receive a valid data packet from robot in {}".format(timeout)) 352 | 353 | def get_cartesian_info(self, wait=False): 354 | if wait: 355 | self.wait() 356 | with self._dictLock: 357 | if "CartesianInfo" in self._dict: 358 | return self._dict["CartesianInfo"] 359 | else: 360 | return None 361 | 362 | def get_all_data(self, wait=False): 363 | """ 364 | return last data obtained from robot in dictionnary format 365 | """ 366 | if wait: 367 | self.wait() 368 | with self._dictLock: 369 | return self._dict.copy() 370 | 371 | def get_joint_data(self, wait=False): 372 | if wait: 373 | self.wait() 374 | with self._dictLock: 375 | if "JointData" in self._dict: 376 | return self._dict["JointData"] 377 | else: 378 | return None 379 | 380 | def get_digital_out(self, nb, wait=False): 381 | if wait: 382 | self.wait() 383 | with self._dictLock: 384 | output = self._dict["MasterBoardData"]["digitalOutputBits"] 385 | mask = 1 << nb 386 | if output & mask: 387 | return 1 388 | else: 389 | return 0 390 | 391 | def get_digital_out_bits(self, wait=False): 392 | if wait: 393 | self.wait() 394 | with self._dictLock: 395 | return self._dict["MasterBoardData"]["digitalOutputBits"] 396 | 397 | def get_digital_in(self, nb, wait=False): 398 | if wait: 399 | self.wait() 400 | with self._dictLock: 401 | output = self._dict["MasterBoardData"]["digitalInputBits"] 402 | mask = 1 << nb 403 | if output & mask: 404 | return 1 405 | else: 406 | return 0 407 | 408 | def get_digital_in_bits(self, wait=False): 409 | if wait: 410 | self.wait() 411 | with self._dictLock: 412 | return self._dict["MasterBoardData"]["digitalInputBits"] 413 | 414 | def get_analog_in(self, nb, wait=False): 415 | if wait: 416 | self.wait() 417 | with self._dictLock: 418 | return self._dict["MasterBoardData"]["analogInput" + str(nb)] 419 | 420 | def get_analog_inputs(self, wait=False): 421 | if wait: 422 | self.wait() 423 | with self._dictLock: 424 | return self._dict["MasterBoardData"]["analogInput0"], self._dict["MasterBoardData"]["analogInput1"] 425 | 426 | def is_program_running(self, wait=False): 427 | """ 428 | return True if robot is executing a program 429 | Rmq: The refresh rate is only 10Hz so the information may be outdated 430 | """ 431 | if wait: 432 | self.wait() 433 | with self._dictLock: 434 | return self._dict["RobotModeData"]["isProgramRunning"] 435 | 436 | def close(self): 437 | self._trystop = True 438 | self.join() 439 | # with self._dataEvent: #wake up any thread that may be waiting for data before we close. Should we do that? 440 | # self._dataEvent.notifyAll() 441 | if self._s_secondary: 442 | with self._prog_queue_lock: 443 | self._s_secondary.close() 444 | --------------------------------------------------------------------------------