├── .gitignore ├── URToolboxFunctions ├── mArray2pList.m ├── pTransform2mMatrix.m ├── pList2mArray.m ├── pDict2mStruct.m ├── UR_WaitForMove.m ├── license.txt ├── URToolboxVer.m ├── URToolboxUpdate.m ├── UR.m └── URX.m ├── CITATION.cff ├── URToolboxSupport ├── URModulesInstall.py ├── URModulesUpdate.py └── URServer.py ├── LICENSE ├── URToolbox Example SCRIPTS └── SCRIPT_DrawCircle.m ├── URToolboxScript.script └── installURToolbox.m /.gitignore: -------------------------------------------------------------------------------- 1 | *.asv -------------------------------------------------------------------------------- /URToolboxFunctions/mArray2pList.m: -------------------------------------------------------------------------------- 1 | function pList = mArray2pList(mArray) 2 | % Convert MATLAB array into a Python list 3 | % 4 | % M. Kutzer, 25Aug2017, USNA 5 | 6 | %% Check inputs 7 | narginchk(1,1); 8 | % TODO - check input dimensions 9 | mArray = reshape(mArray,1,[]); 10 | 11 | %% Convert to Python list 12 | pList = py.list(mArray); -------------------------------------------------------------------------------- /URToolboxFunctions/pTransform2mMatrix.m: -------------------------------------------------------------------------------- 1 | function H = pTransform2mMatrix(ttrans) 2 | 3 | ndArray = ttrans.matrix.A; 4 | nList = ndArray.tolist; 5 | 6 | nCell = cell(nList); 7 | 8 | % TODO - preallocate memory etc 9 | for i = 1:numel(nCell) 10 | iList = nCell{i}; 11 | iCell = cell(iList); 12 | H(i,:) = cell2mat(iCell); 13 | end -------------------------------------------------------------------------------- /URToolboxFunctions/pList2mArray.m: -------------------------------------------------------------------------------- 1 | function mArray = pList2mArray(pList) 2 | % PLIST2MARRAY convert Python lists into MATLAB arrays 3 | % 4 | % ENS Kevin Strotz, 22Sept2016, USNA 5 | 6 | % Updates 7 | % 25Jan2017 - documentation update (M. Kutzer) 8 | 9 | %% Check inputs 10 | narginchk(1,1); 11 | % TODO - check for Python list class 12 | 13 | %% Convert to MATLAB array 14 | mCell = cell(pList); 15 | mArray = cell2mat(mCell); 16 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you use this software, please cite it as below." 3 | authors: 4 | - family-names: "Kutzer" 5 | given-names: "Michael" 6 | orcid: "https://orcid.org/0000-0001-8246-2409" 7 | - family-names: "Strotz" 8 | given-names: "Kevin" 9 | title: "Universal Robot CB-Series Toolbox for MATLAB leveraging Python" 10 | version: 1.1.6 11 | # doi: XX.XXXX/zenodo.XXX 12 | date-released: 2021-03-26 13 | url: "https://github.com/kutzer/URToolbox" -------------------------------------------------------------------------------- /URToolboxFunctions/pDict2mStruct.m: -------------------------------------------------------------------------------- 1 | function mStruct = pDict2mStruct(pDict) 2 | % PDICT2MSTRUCT converts Python dictionary to MATLAB structure. 3 | % Note: This will stay as Python types within the structure if they 4 | % are also lists, tuples, dictionaries, etc 5 | % 6 | % ENS Kevin Strotz, 22Sept2016, USNA 7 | 8 | %% Check inputs 9 | narginchk(1,1); 10 | % TODO - check input class 11 | 12 | %% Convert to MATLAB structure 13 | mStruct = struct(pDict); 14 | 15 | end -------------------------------------------------------------------------------- /URToolboxSupport/URModulesInstall.py: -------------------------------------------------------------------------------- 1 | # 2 | # installURModules.py 3 | # 4 | # Runs one time to install the necessary modules when the URToolbox 5 | # is first loaded 6 | # 7 | # Eliminates the need for the user to manually add modules in the command 8 | # line window and can help prevent accidental directory mismatch 9 | # 10 | # ENS Kevin Strotz, USN 11 | # 22 September 2016 12 | # 13 | 14 | def installURModules(): 15 | import pip 16 | mathChk = pip.main(["install","math3d","--quiet"]) 17 | numChk = pip.main(["install","numpy","--quiet"]) 18 | urxChk = pip.main(["install","urx","--quiet"]) 19 | 20 | -------------------------------------------------------------------------------- /URToolboxSupport/URModulesUpdate.py: -------------------------------------------------------------------------------- 1 | # 2 | # updateURModules.py 3 | # 4 | # Runs with URToolboxUpdate to make sure modules are upgradede 5 | # 6 | # Eliminates the need for the user to manually add modules in the command 7 | # line window and can help prevent accidental directory mismatch 8 | # 9 | # ENS Kevin Strotz, USN 10 | # 22 September 2016 11 | # 12 | 13 | def updateURModules(): 14 | import pip 15 | mathChk = pip.main(["install","math3d","--upgrade","--quiet"]) 16 | numChk = pip.main(["install","numpy","--upgrade","--quiet"]) 17 | urxChk = pip.main(["install","urx","--upgrade","--quiet"]) 18 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | This software was developed at the United States Naval Academy (USNA) by employees of the Federal Government in the course of their official duties. Pursuant to title 17 Section 105 of the United States Code this software is not subject to copyright protection and is in the public domain. It is an experimental system. USNA assumes no responsibility whatsoever for its use by other parties, and makes no guarantees, expressed or implied, about its quality, reliability, or any other characteristic. We would appreciate acknowledgement if the software is used. This software can be redistributed and/or modified freely provided that any derivative works bear some notice that they are derived from it, and any modified versions bear some notice that they have been modified. -------------------------------------------------------------------------------- /URToolboxFunctions/UR_WaitForMove.m: -------------------------------------------------------------------------------- 1 | function UR_WaitForMove(obj) 2 | % UR_WAITFORMOVE pauses program execution until the hardware finishes 3 | % moving. 4 | % UR_WAITFORMOVE(obj) pauses program execution until the hardware tied ot 5 | % the specified UR object completes a movement. 6 | % 7 | % NOTE: This function requires an established URX connection with the 8 | % robot. 9 | % 10 | % M. Kutzer, 14Mar2018, USNA 11 | 12 | %% Check inputs 13 | narginchk(1,1); 14 | 15 | % TODO: Check to see if the input is a valid object from the UR class 16 | % TODO: Check if the URX connection is established 17 | 18 | %% Wait for movement 19 | fprintf('Moving to position...'); 20 | 21 | X_all = []; 22 | isMoving = true; 23 | while isMoving 24 | TT = obj.TTRANS; 25 | H_cur = pTransform2mMatrix(TT); 26 | X_all(:,end+1) = H_cur(1:3,4); 27 | 28 | if size(X_all,2) > 1 29 | v = X_all(:,end) - X_all(:,end-1); 30 | d = norm(v); 31 | else 32 | d = inf; 33 | end 34 | 35 | if d < 1 36 | isMoving = false; 37 | end 38 | 39 | pause(0.05); 40 | end 41 | fprintf('COMPLETE\n'); -------------------------------------------------------------------------------- /URToolboxFunctions/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, Michael Kutzer 2 | 3 | All rights reserved. 4 | 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | 8 | modification, are permitted provided that the following conditions are 9 | 10 | met: 11 | 12 | 13 | * Redistributions of source code must retain the above copyright 14 | 15 | notice, this list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright 18 | 19 | notice, this list of conditions and the following disclaimer in 20 | 21 | the documentation and/or other materials provided with the distribution 22 | 23 | 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | 27 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | 29 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 30 | 31 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 32 | 33 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 34 | 35 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 36 | 37 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 38 | 39 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 40 | WHETHER IN 41 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 42 | OTHERWISE) 43 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 44 | ADVISED OF THE 45 | POSSIBILITY OF SUCH DAMAGE. 46 | -------------------------------------------------------------------------------- /URToolboxFunctions/URToolboxVer.m: -------------------------------------------------------------------------------- 1 | function varargout = URToolboxVer 2 | % URTOOLBOXVER display the OptiTrack Toolbox information. 3 | % URTOOLBOXVER displays the information to the command prompt. 4 | % 5 | % A = URTOOLBOXVER returns in A the sorted struct array of version 6 | % information for the OptiTrack Toolbox. 7 | % The definition of struct A is: 8 | % A.Name : toolbox name 9 | % A.Version : toolbox version number 10 | % A.Release : toolbox release string 11 | % A.Date : toolbox release date 12 | % 13 | % M. Kutzer, 17Feb2016, USNA 14 | 15 | % Updates 16 | % 10Mar2016 - Updates to plotRigidBody documentation and visualization 17 | % script. 18 | % 10Mar2016 - Corrected plot error in example script and added error 19 | % check for not-tracked issue in plotRigidBody 20 | % 10Mar2016 - Updated error checking in plotRigidBody and example updates 21 | % 19Dec2016 - Added simulation and support components 22 | % 27Feb2017 - Added patch object handles to simulation properties 23 | % 29Mar2017 - Updated UR_DHtable to match values from UR site 24 | % 14Mar2018 - Added waitformove function and example 25 | % 15Mar2018 - Updated to include try/catch for required toolbox 26 | % installations and include msgbox warning when download 27 | % fails. 28 | % 08Jan2021 - Updated ToolboxUpdate 29 | % 26Mar2021 - Migrated simulation tools to a different repository 30 | 31 | A.Name = 'UR Toolbox'; 32 | A.Version = '1.1.6'; 33 | A.Release = '(R2019b)'; 34 | A.Date = '26-Mar-2021'; 35 | A.URLVer = 1; 36 | 37 | msg{1} = sprintf('MATLAB %s Version: %s %s',A.Name, A.Version, A.Release); 38 | msg{2} = sprintf('Release Date: %s',A.Date); 39 | 40 | n = 0; 41 | for i = 1:numel(msg) 42 | n = max( [n,numel(msg{i})] ); 43 | end 44 | 45 | fprintf('%s\n',repmat('-',1,n)); 46 | for i = 1:numel(msg) 47 | fprintf('%s\n',msg{i}); 48 | end 49 | fprintf('%s\n',repmat('-',1,n)); 50 | 51 | if nargout == 1 52 | varargout{1} = A; 53 | end -------------------------------------------------------------------------------- /URToolboxFunctions/URToolboxUpdate.m: -------------------------------------------------------------------------------- 1 | function URToolboxUpdate 2 | % URTOOLBOXUPDATE download and update the UR Toolbox. 3 | % 4 | % M. Kutzer 27Feb2016, USNA 5 | 6 | % Updates 7 | % 15Mar2018 - Updated to include try/catch for required toolbox 8 | % installations and include msgbox warning when download 9 | % fails. 10 | % 08Jan2021 - Updated ToolboxUpdate 11 | 12 | % TODO - Find a location for "URToolbox Example SCRIPTS" 13 | % TODO - update function for general operation 14 | 15 | % Install UR Toolbox 16 | ToolboxUpdate('UR'); 17 | 18 | updateModule = py.importlib.import_module('URModulesUpdate'); 19 | 20 | fprintf('Updating Python modules...'); 21 | try 22 | updateModule.updateURModules(); 23 | fprintf('[Complete]\n'); 24 | catch 25 | % TODO - add appropriate update "run" info 26 | fprintf('[Failed]\n') 27 | fprintf(2,'Failed to update necessary Python modules. To install manually:\n') 28 | fprintf(2,'\t - Open the Command Prompt,\n'); 29 | fprintf(2,'\t - Switch to the Python 3.4 Scripts directory\n'); 30 | fprintf(2,'\t\t run "cd C:\\Python34\\Scripts"\n'); 31 | fprintf(2,'\t - Install the math3D module\n'); 32 | fprintf(2,'\t\t run "pip install math3d"\n'); 33 | fprintf(2,'\t - Install the numpy module\n'); 34 | fprintf(2,'\t\t run "pip install numpy"\n'); 35 | fprintf(2,'\t - Install the urx module\n'); 36 | fprintf(2,'\t\t run "pip install urx"\n'); 37 | end 38 | 39 | end 40 | 41 | function ToolboxUpdate(toolboxName) 42 | 43 | %% Setup functions 44 | ToolboxVer = str2func( sprintf('%sToolboxVer',toolboxName) ); 45 | installToolbox = str2func( sprintf('install%sToolbox',toolboxName) ); 46 | 47 | %% Check current version 48 | try 49 | A = ToolboxVer; 50 | catch ME 51 | A = []; 52 | fprintf('No previous version of %s detected.\n',toolboxName); 53 | end 54 | 55 | %% Setup temporary file directory 56 | fprintf('Downloading the %s Toolbox...',toolboxName); 57 | tmpFolder = sprintf('%sToolbox',toolboxName); 58 | pname = fullfile(tempdir,tmpFolder); 59 | if isfolder(pname) 60 | % Remove existing directory 61 | [ok,msg] = rmdir(pname,'s'); 62 | end 63 | % Create new directory 64 | [ok,msg] = mkdir(tempdir,tmpFolder); 65 | 66 | %% Download and unzip toolbox (GitHub) 67 | url = sprintf('https://github.com/kutzer/%sToolbox/archive/master.zip',toolboxName); 68 | try 69 | %fnames = unzip(url,pname); 70 | %urlwrite(url,fullfile(pname,tmpFname)); 71 | tmpFname = sprintf('%sToolbox-master.zip',toolboxName); 72 | websave(fullfile(pname,tmpFname),url); 73 | fnames = unzip(fullfile(pname,tmpFname),pname); 74 | delete(fullfile(pname,tmpFname)); 75 | 76 | fprintf('SUCCESS\n'); 77 | confirm = true; 78 | catch ME 79 | fprintf('FAILED\n'); 80 | confirm = false; 81 | fprintf(2,'ERROR MESSAGE:\n\t%s\n',ME.message); 82 | end 83 | 84 | %% Check for successful download 85 | alternativeInstallMsg = [... 86 | sprintf('Manually download the %s Toolbox using the following link:\n',toolboxName),... 87 | newline,... 88 | sprintf('%s\n',url),... 89 | newline,... 90 | sprintf('Once the file is downloaded:\n'),... 91 | sprintf('\t(1) Unzip your download of the "%sToolbox"\n',toolboxName),... 92 | sprintf('\t(2) Change your "working directory" to the location of "install%sToolbox.m"\n',toolboxName),... 93 | sprintf('\t(3) Enter "install%sToolbox" (without quotes) into the command window\n',toolboxName),... 94 | sprintf('\t(4) Press Enter.')]; 95 | 96 | if ~confirm 97 | warning('InstallToolbox:FailedDownload','Failed to download updated version of %s Toolbox.',toolboxName); 98 | fprintf(2,'\n%s\n',alternativeInstallMsg); 99 | 100 | msgbox(alternativeInstallMsg, sprintf('Failed to download %s Toolbox',toolboxName),'warn'); 101 | return 102 | end 103 | 104 | %% Find base directory 105 | install_pos = strfind(fnames, sprintf('install%sToolbox.m',toolboxName) ); 106 | sIdx = cell2mat( install_pos ); 107 | cIdx = ~cell2mat( cellfun(@isempty,install_pos,'UniformOutput',0) ); 108 | 109 | pname_star = fnames{cIdx}(1:sIdx-1); 110 | 111 | %% Get current directory and temporarily change path 112 | cpath = cd; 113 | cd(pname_star); 114 | 115 | %% Install Toolbox 116 | installToolbox(true); 117 | 118 | %% Move back to current directory and remove temp file 119 | cd(cpath); 120 | [ok,msg] = rmdir(pname,'s'); 121 | if ~ok 122 | warning('Unable to remove temporary download folder. %s',msg); 123 | end 124 | 125 | %% Complete installation 126 | fprintf('Installation complete.\n'); 127 | 128 | end -------------------------------------------------------------------------------- /URToolbox Example SCRIPTS/SCRIPT_DrawCircle.m: -------------------------------------------------------------------------------- 1 | %% SCRIPT_DrawCircle 2 | % This script demonstrates use of the URsim class capabilities with the UR 3 | % class. The URsim object is used to visualize the robot and calculate 4 | % inverse kinematics for the system. Waypoints are executed by getting 5 | % joint angle from the simulation object and sending them to the 6 | % URToolboxScript controller running on the UR control box. 7 | % 8 | % M. Kutzer, 14Mar2018, USNA 9 | 10 | clearvars -EXCEPT hwObj 11 | close all 12 | clc 13 | 14 | %% Create hardware flag (for debugging) 15 | % -> Set value to true if you are connected to hardware 16 | useHardware = false; 17 | 18 | %% Initialize simulation 19 | % -> This code has been tested on the UR5 and UR10 systems. Minor 20 | % adjustments may be required to use it with the UR3. 21 | 22 | if ~exist('simObj') 23 | % Create object 24 | simObj = URsim; 25 | % Initialize simulation 26 | simObj.Initialize; 27 | % Set a tool frame offset (e.g. for Robotiq end-effector not shown in 28 | % visualization) 29 | simObj.FrameT = Tz(160); 30 | 31 | % Hide frames 32 | frames = '0123456E'; 33 | for i = 1:numel(frames) 34 | hideTriad(simObj.(sprintf('hFrame%s',frames(i)))); 35 | end 36 | end 37 | 38 | %% Connect to hardware 39 | % -> The message to the user *assumes* that you have: 40 | % (1) Properly installed URToolboxScript on the UR controller. See 41 | % the link below for instructions: 42 | % 43 | % https://www.usna.edu/Users/weapsys/kutzer/_Code-Development/UR_Toolbox.php 44 | % 45 | % (2) Configure the network card connecting the PC to the UR 46 | % controller to a static IP of 10.1.1.5 47 | % 48 | % (3) Set the UR controller IP to 10.1.1.2 49 | 50 | if ~exist('hwObj') && useHardware 51 | instruct = sprintf([... 52 | '\tPython module imported.\n',... 53 | '\tEnter server IP address: 10.1.1.5\n',... 54 | '\tEnter port: 30002\n',... 55 | '\tEnter number of connections to be made: 1\n',... 56 | '\tServer created.\n',... 57 | '\tBegin onboard controller, then press ENTER.\n',... 58 | '\tConnections established.\n',... 59 | '\tWould you like to create a URX connection as well? y\n',... 60 | '\tEnter URX address: 10.1.1.2\n',... 61 | '\tURX connection established.\n']); 62 | fprintf('PLEASE USE THE FOLLOWING RESPONSES:\n\n'); 63 | fprintf(2,'%s\n\n',instruct) 64 | 65 | hwObj = UR; 66 | end 67 | 68 | %% Create path 69 | % -> NOTE: This is provided for demonstration only. Perfect execution of 70 | % this path will require infinite accelerations at the initial and final 71 | % waypoints meaning the robot will not track perfectly. 72 | 73 | % Define dependent variable 74 | s = linspace(0,1,1000); 75 | % Define circle radius (mm) 76 | r = 100; 77 | % Define position data 78 | X = []; 79 | X(1,:) = r*cos(2*pi*s); % x-position 80 | X(2,:) = r*sin(2*pi*s); % y-position 81 | X(3,:) = 0; % z-position 82 | X(4,:) = 1; % Append 1 (homogeneous coordinate) 83 | 84 | % Transform coordinates into the workspace of the robot 85 | X = Tz(500)*Rx(pi/2)*Tz(500)*X; 86 | 87 | % Plot waypoints 88 | plt_Waypoints = plot3(simObj.Axes,X(1,:),X(2,:),X(3,:),'.m'); 89 | 90 | %% Animate simulation and move the robot to test 91 | % Home simulation 92 | simObj.Home; 93 | % Move robot to match simulation 94 | if useHardware 95 | % Get joint position from the simulation 96 | q = simObj.Joints; 97 | % Send waypoint to the robot 98 | % -> Message syntax sends 6 joint positions and 6 joint velocities to 99 | % define a desired waypoint for the URToolboxScript controller. We are 100 | % assuming a desired velocity of zero for all joints at each waypoint. 101 | msg(hwObj,sprintf('(%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f)',q,zeros(6,1))); 102 | % Wait for the robot to finish executing the move 103 | UR_WaitForMove(hwObj); 104 | end 105 | % Allow plot to update 106 | drawnow 107 | 108 | % Move through waypoints 109 | for i = 1:numel(s) 110 | % Define pose from waypoint 111 | H_cur = Tx(X(1,i))*Ty(X(2,i))*Tz(X(3,i))*Rx(pi/2); 112 | 113 | % Set simulation toolpose to waypoint pose 114 | simObj.ToolPose = H_cur; 115 | 116 | % Move robot to match simulation 117 | q = simObj.Joints; 118 | if useHardware 119 | % Get joint position from the simulation 120 | q = simObj.Joints; 121 | % Send waypoint to the robot 122 | msg(hwObj,sprintf('(%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f)',q,zeros(6,1))); 123 | % Wait for move only on the first waypoint 124 | if i == 1 125 | % Wait for the robot to finish executing the move 126 | UR_WaitForMove(hwObj); 127 | end 128 | end 129 | 130 | % Allow plot to update 131 | drawnow; 132 | end 133 | -------------------------------------------------------------------------------- /URToolboxSupport/URServer.py: -------------------------------------------------------------------------------- 1 | # 2 | # Class Definition for Constant Socket Comms to UR Platforms 3 | # 4 | # ENS Kevin Strotz 5 | # Assistant Professor Michael Kutzer, Principal Investigator 6 | # 19 AUG 2016 7 | # 8 | # Referenced in MATLAB class, make sure directory is appropriate and command names match 9 | # Do NOT need direct Python interface for functionality, everything is called in MATLAB 10 | # 11 | # Be careful of variable types when passing values Python <-> MATLAB 12 | # - Some are not supported in the other, ie. tuples, MATLAB doubles 13 | # - Manually convert to force compatibility 14 | # 15 | # Ensure modules and variable to be returned have proper scope (usually global) 16 | # 17 | 18 | # Initialize socket in correct family, bind as a server, and begin listening for clients 19 | # Return socket structure to the MATLAB workspace for future use 20 | # Three input parameters: 21 | # host = server IP address 22 | # port = port to listen for connections on 23 | # backlog = number of connections to listen for 24 | def initServer(host,port,backlog): 25 | import socket # import socket module to Python environment 26 | global socketname # create global handle to for server socket to be returned to MATLAB 27 | socketname = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # initialize socket type 28 | socketname.bind((host, port)) # Bind socket to specified host address and port 29 | socketname.listen(backlog) # Begin listening for client connections from manipulators 30 | return socketname # return established server socket to MATLAB workspace 31 | 32 | # Accept client connections on a previously established server 33 | # One input parameter: 34 | # socketname = established server socket handle 35 | def cnctClient(socketname): 36 | global client # create global handle for client to be returned to MATLAB 37 | sockettuple = socketname.accept() # Accept a connection and store [client, address] in handle 38 | client = sockettuple[0] # Separate actual socket object out of tuple 39 | return client # return established client socket to MATLAB workspace 40 | 41 | # Create URX connection to manipulator 42 | # One input parameter: 43 | # robotip = IP address of manipulator 44 | def cnctURX(robotip): 45 | import urx # import module 46 | global r # global handle for urx connection to be returned to MATLAB 47 | r = urx.Robot(robotip) # establish connection 48 | return r # return connection to MATLAB 49 | 50 | # Send message from server to client (UR robot) 51 | # Message is simply a string, Python does not format it at all 52 | # Ensure correct formatting in MATLAB that will be understood by onboard controllers 53 | # Needs .encode() to convert to binary before sending 54 | # Two input parameters: 55 | # client = established client socket destination 56 | # msg = string to be sent to client 57 | def sendmsg(client,msg): 58 | client.send(msg.encode()) # Encode string as binary and send to designated client 59 | 60 | # Close socket connection 61 | # Does not eliminate handle or object, simply closes connection 62 | # One dual manipulator setup has three sockets to close: 2x clients, 1x server 63 | # One input parameter: 64 | # socketname = handle of socket to be closed 65 | def closeSocket(socketname): 66 | socketname.close() # Close designated socket 67 | 68 | # Close URX connection 69 | # Same functionality as closeSocket 70 | # One dual manipulator setup has two URX connections 71 | def closeURX(robot): 72 | robot.close() 73 | 74 | #************************************TELEMETRY********************************************* 75 | # Following section contains functions to get data from a UR manipulator and return it to MATLAB 76 | # All functions have one input argument of the URX connection handle 77 | # -> When called in MATLAB, should not need input as the internal class function handles it 78 | # Generally return a Python list to MATLAB which can be parsed there 79 | 80 | # Get current joint positions 81 | # ***Prexisting function, no need to call dictionary and manually extract data 82 | def getJPos(robot): 83 | import urx 84 | global jPos 85 | jPos = robot.getj() 86 | return jPos 87 | 88 | # Get current joint velocities from a manipulator 89 | # ***NO NATIVE URX FUNCTION, CONSIDER IMPLEMENTING THIS IN URX MODULE VICE HERE*** 90 | def getJVels(robot): 91 | global jVels # create global handle for joint velocities to be returned to MATLAB 92 | jData = robot.secmon._dict["JointData"] # Extract all joint info from URX dictionary using key "JointData" 93 | # Extract joint velocities from all joint data using variable names 94 | jVels = [jData["qd_actual0"],jData["qd_actual1"],jData["qd_actual2"],jData["qd_actual3"],jData["qd_actual4"],jData["qd_actual5"]] 95 | return jVels # return joint velocity list to MATLAB workspace 96 | 97 | # Get current joint torques 98 | def getJTorq(robot): 99 | global jTorq 100 | jData = robot.secmon._dict["JointData"] 101 | jTorq = [jData["T_motor0"],jData["T_motor1"],jData["T_motor2"],jData["T_motor3"],jData["T_motor4"],jData["T_motor5"]] 102 | return jTorq 103 | 104 | # Get current joint voltages 105 | def getJVolt(robot): 106 | global jVolt 107 | jData = robot.secmon._dict["JointData"] 108 | jVolt = [jData["V_actual0"],jData["V_actual1"],jData["V_actual2"],jData["V_actual3"],jData["V_actual4"],jData["V_actual5"]] 109 | return jVolt 110 | 111 | # Get current joint currents 112 | def getJCurr(robot): 113 | global jCurr 114 | jData = robot.secmon._dict["JointData"] 115 | jCurr = [jData["I_actual0"],jData["I_actual1"],jData["I_actual2"],jData["I_actual3"],jData["I_actual4"],jData["I_actual5"]] 116 | return jCurr 117 | 118 | # Get current tool position [X,Y,Z] 119 | def getTPos(robot): 120 | global tPos 121 | tData = robot.secmon._dict["CartesianInfo"] 122 | tPos = [tData["X"],tData["Y"],tData["Z"]] 123 | return tPos 124 | 125 | # Get current tool rotation vectors [Rx,Ry,Rz] 126 | def getTVec(robot): 127 | global tVec 128 | tData = robot.secmon._dict["CartesianInfo"] 129 | tVec = [tData["Rx"],tData["Ry"],tData["Rz"]] 130 | return tVec 131 | 132 | # Get current tool transform 133 | def getTTrans(robot): 134 | global tTrans 135 | tTrans = robot.get_pose() 136 | return tTrans 137 | 138 | # Get general tool data {returns as dictionary, not list} 139 | def getTInfo(robot): 140 | global tInfo 141 | tInfo = robot.secmon._dict["ToolData"] 142 | return tInfo 143 | 144 | # Get current analog inputs 145 | def getAIn(robot): 146 | global analogIn 147 | ioData = robot.secmon._dict["MasterBoardInfo"] 148 | analogIn = [ioData["analog_in0"],ioData["analog_in1"],ioData["analog_in2"],ioData["analog_in3"],ioData["analog_in4"],ioData["analog_in5"]] 149 | return analogIn 150 | 151 | # Get current digital inputs 152 | def getDIn(robot): 153 | global digitalIn 154 | ioData = robot.secmon._dict["MasterBoardInfo"] 155 | digitalIn = [ioData["digital_in0"],ioData["digital_in1"],ioData["digital_in2"],ioData["digital_in3"],ioData["digital_in4"],ioData["digital_in5"]] 156 | return digitalIn 157 | 158 | # Get current analog outputs 159 | def getAOut(robot): 160 | global analogOut 161 | ioData = robot.secmon._dict["MasterBoardInfo"] 162 | analogOut = [ioData["analog_out0"],ioData["analog_out1"],ioData["analog_out2"],ioData["analog_out3"],ioData["analog_out4"],ioData["analog_out5"]] 163 | return analogOut 164 | 165 | # Get current digital outputs 166 | def getDOut(robot): 167 | global digitalOut 168 | ioData = robot.secmon._dict["MasterBoardInfo"] 169 | digitalOut = [ioData["digital_out0"],ioData["digital_out1"],ioData["digital_out2"],ioData["digital_out3"],ioData["digital_out4"],ioData["digital_out5"]] 170 | return digitalOut 171 | 172 | 173 | 174 | 175 | -------------------------------------------------------------------------------- /URToolboxFunctions/UR.m: -------------------------------------------------------------------------------- 1 | %{ 2 | 3 | MATLAB Class for Python Socket to UR5 4 | written by ENS Kevin Strotz, USN 5 | 26JUL16 6 | 7 | Use independent Python class for MATLAB to enable native 8 | communications in MATLAB without needing Python proficiency 9 | 10 | Consider renaming 11 | 12 | %} 13 | 14 | % Define class as 'handle' type --> allows properties to update properly 15 | classdef UR < handle 16 | % Define properties that will be set as part of the object 17 | properties 18 | MODULE % Python module 19 | HOST % Server IP address (on computer) --> generally set to 10.1.1.5 20 | PORT % Server port to listen on --> should be 30002 21 | BACKLOG % Number of connections to listen to --> should be 2 22 | CLIENT % Client (UR manipulators) 23 | SERVER % Server socket object --> created during initialization 24 | MSG % Message to be sent to UR robot --> input by user 25 | STATE % Socket state --> can be OPEN or CLOSE 26 | URX % URX connection 27 | URXADDR % URX address 28 | JPOS % Joint positions 29 | JVELS % Joint velocities 30 | JTORQ % Joint torques 31 | JVOLT % Joint voltages 32 | JCURR % Joint currents 33 | TPOS % Tool position 34 | TVEC % Tool rotation vectors 35 | TTRANS % Tool transform 36 | TINFO % Tool info 37 | AIN % Analog inputs 38 | AOUT % Analog outputs 39 | DIN % Digital inputs 40 | DOUT % Digital outputs 41 | 42 | end 43 | % End properties section 44 | 45 | % Define methods to update properties and communicate across the LAN 46 | methods 47 | % Initialization function creating class object 48 | % host = server IP address on computer [10.1.1.5] 49 | % port = IP port on UR manipulators [30002] 50 | % backlog = number of connections [2] 51 | function obj = UR(prevServer) 52 | % IMPORT PYTHON MODULES: DO NOT INCLUDE .py 53 | obj.MODULE = py.importlib.import_module('URServer'); 54 | fprintf('Python module imported.\n'); % Indicate modules imported 55 | if nargin < 1 56 | host = input('Enter server IP address: ','s'); 57 | port = input('Enter port: '); 58 | backlog = input('Enter number of connections to be made: '); 59 | inputVal{1,1} = host; % Put host in cell for comparison 60 | port = int32(port); % Convert double to integer for Python 61 | backlog = int32(backlog); % Convert double to integer for Python 62 | if iscellstr(inputVal) % If host is a string 63 | obj.HOST = host; % Set HOST property 64 | else 65 | error('Host must be a string.'); % Raise error if host is not string 66 | end 67 | if isnumeric(port) % If port is a number 68 | obj.PORT = port; % Set PORT property 69 | else 70 | error('Port must be an integer.'); % Raise error if port is not number 71 | end 72 | if isnumeric(backlog) % If backlog is a number 73 | obj.BACKLOG = backlog; % Set BACKLOG property 74 | else 75 | error('Backlog must be an integer.'); % Raise error if backlog is not a number 76 | end 77 | 78 | % addpath('C:\Users\Research\Google Drive\GitHub\UR') % Add path with modules 79 | 80 | obj.SERVER = obj.MODULE.initServer(obj.HOST,obj.PORT,obj.BACKLOG); % Create server socket on computer 81 | fprintf('Server created.\n') % Indicate server started 82 | else 83 | obj.HOST = prevServer.HOST; 84 | obj.PORT = prevServer.PORT; 85 | obj.BACKLOG = prevServer.BACKLOG; 86 | obj.SERVER = prevServer.SERVER; 87 | end 88 | 89 | fprintf('Begin onboard controller, then press ENTER.') % Press start on manipulators. 90 | obj.CLIENT = obj.MODULE.cnctClient(obj.SERVER); % Wait for first onboard controller to connect as client 91 | obj.STATE = 'OPEN'; % Set STATE property to indicate connections 92 | input(''); % Wait for key press to indicate controllers started 93 | fprintf('Connections established.\n'); % Indicate clients connected 94 | 95 | urxFlag = input('Would you like to create a URX connection as well? ','s'); % Ask if user wants URX connections too 96 | % Check if user indicates yes 97 | if (strcmp(urxFlag,'YES') || strcmp(urxFlag,'yes') || strcmp(urxFlag,'Y') || strcmp(urxFlag,'y')) 98 | obj.URXADDR = input('Enter URX address: ','s'); % Set first URX address property 99 | obj.URX = obj.MODULE.cnctURX(obj.URXADDR); % Create first URX connection 100 | fprintf('URX connection established.\n'); % Indicate URX connection 101 | else % If user does not want URX connections 102 | fprintf('No URX connections created.'); % Indicate no URX connections 103 | end 104 | end 105 | 106 | % Set STATE property 107 | function obj = set.STATE(obj,state) 108 | obj.STATE = state; 109 | end 110 | % Get STATE property 111 | function state = get.STATE(obj) 112 | state = obj.STATE; 113 | end 114 | % Set MSG property 115 | function obj = set.MSG(obj,msg) 116 | obj.MSG = msg; 117 | end 118 | % Get MSG property 119 | function msg = get.MSG(obj) 120 | msg = obj.MSG; 121 | end 122 | % Set JPOS property 123 | function obj = set.JPOS(obj,joints) 124 | obj.JPOS = joints; 125 | end 126 | % Get JPOS property 127 | function joints = get.JPOS(obj) 128 | obj.JPOS = obj.MODULE.getJPos(obj.URX); 129 | joints = obj.JPOS; 130 | end 131 | % Set JVELS property 132 | function obj = set.JVELS(obj,velocities) 133 | obj.JVELS = velocities; 134 | end 135 | % Get JVELS property 136 | function vels = get.JVELS(obj) 137 | obj.JVELS = obj.MODULE.getJVels(obj.URX); 138 | vels = obj.JVELS; 139 | end 140 | 141 | function obj = set.JTORQ(obj,torq) 142 | obj.JTORQ = torq; 143 | end 144 | 145 | function torq = get.JTORQ(obj) 146 | obj.JTORQ = obj.MODULE.getJTorq(obj.URX); 147 | torq = obj.JTORQ; 148 | end 149 | 150 | function obj = set.JVOLT(obj,volt) 151 | obj.JVOLT = volt; 152 | end 153 | 154 | function volt = get.JVOLT(obj) 155 | obj.JVOLT = obj.MODULE.getJVolt(obj.URX); 156 | volt = obj.JVOLT; 157 | end 158 | 159 | function obj = set.JCURR(obj,curr) 160 | obj.JCURR = curr; 161 | end 162 | 163 | function curr = get.JCURR(obj) 164 | obj.JCURR = obj.MODULE.getJCurr(obj.URX); 165 | curr = obj.JCURR; 166 | end 167 | 168 | function obj = set.TPOS(obj,pose) 169 | obj.TPOS = pose; 170 | end 171 | 172 | function tpose = get.TPOS(obj) 173 | obj.TPOS = obj.MODULE.getTPos(obj.URX); 174 | tpose = obj.TPOS; 175 | end 176 | 177 | function obj = set.TVEC(obj,tvec) 178 | obj.TVEC = tvec; 179 | end 180 | 181 | function tvec = get.TVEC(obj) 182 | obj.TVEC = obj.MODULE.getTVec(obj.URX); 183 | tvec = obj.TVEC; 184 | end 185 | 186 | function obj = set.TTRANS(obj,ttrans) 187 | obj.TTRANS = ttrans; 188 | end 189 | 190 | function ttrans = get.TTRANS(obj) 191 | obj.TTRANS = obj.MODULE.getTTrans(obj.URX); 192 | ttrans = obj.TTRANS; 193 | end 194 | 195 | function obj = set.TINFO(obj,tinfo) 196 | obj.TINFO = tinfo; 197 | end 198 | 199 | function tinfo = get.TINFO(obj) 200 | obj.TINFO = obj.MODULE.getTInfo(obj.URX); 201 | tinfo = obj.TINFO; 202 | end 203 | 204 | function obj = set.AIN(obj,ain) 205 | obj.AIN = ain; 206 | end 207 | 208 | function ain = get.AIN(obj) 209 | obj.AIN = obj.MODULE.getAIn(obj.URX); 210 | ain = obj.AIN; 211 | end 212 | 213 | function obj = set.AOUT(obj,aout) 214 | obj.AOUT = aout; 215 | end 216 | 217 | function aout = get.AOUT(obj) 218 | obj.AOUT = obj.MODULE.getAOut(obj.URX); 219 | aout = obj.AOUT; 220 | end 221 | 222 | function obj = set.DIN(obj,din) 223 | obj.DIN = din; 224 | end 225 | 226 | function din = get.DIN(obj) 227 | obj.DIN = obj.MODULE.getDIn(obj.URX); 228 | din = obj.DIN; 229 | end 230 | 231 | function obj = set.DOUT(obj,dout) 232 | obj.DOUT = dout; 233 | end 234 | 235 | function dout = get.DOUT(obj) 236 | obj.DOUT = obj.MODULE.getDOut(obj.URX); 237 | dout = obj.DOUT; 238 | end 239 | 240 | % Function to send message to the manipulator 241 | % Two user specified arguments: 242 | % message = data string to transmit 243 | function obj = msg(obj,message) 244 | obj.MSG = message; % Set MSG property to latest message 245 | obj.MODULE.sendmsg(obj.CLIENT,obj.MSG); % Send message to CLIENT 246 | end 247 | 248 | function obj = UpdateAll(obj) 249 | obj.JPOS = obj.MODULE.getJPos(obj.URX); 250 | obj.JVELS = obj.MODULE.getJVels(obj.URX); 251 | obj.JTORQ = obj.MODULE.getJTorq(obj.URX); 252 | obj.JVOLT = obj.MODULE.getJVolt(obj.URX); 253 | obj.JCURR = obj.MODULE.getJCurr(obj.URX); 254 | obj.TPOS = obj.MODULE.getTPos(obj.URX); 255 | obj.TVEC = obj.MODULE.getTVec(obj.URX); 256 | obj.TTRANS = obj.MODULE.getTTrans(obj.URX); 257 | obj.TINFO = obj.MODULE.getTInfo(obj.URX); 258 | obj.AIN = obj.MODULE.getAIn(obj.URX); 259 | obj.DIN = obj.MODULE.getDIn(obj.URX); 260 | obj.AOUT = obj.MODULE.getAOut(obj.URX); 261 | obj.DOUT = obj.MODULE.getDOut(obj.URX); 262 | end 263 | 264 | % Function to close ports at conclusion of operation 265 | % No user specified arguments 266 | function obj = clse(obj) 267 | % Make sure the user intends to close the connections 268 | check = input('Are you sure you want to close connections? ','s'); 269 | % If they are sure they want to close 270 | if (strcmp(check,'YES') || strcmp(check,'yes') || strcmp(check,'Y') || strcmp(check,'y')) 271 | obj.STATE = 'CLOSED'; % Set STATE property to closed 272 | obj.MODULE.closeSocket(obj.CLIENT); % Close first client connection 273 | obj.MODULE.closeSocket(obj.SERVER); % Close server on computer 274 | obj.MODULE.closeURX(obj.URX); 275 | else % If the user wants to keep connections 276 | fprintf('Maintaining connections.'); % Keep connections open 277 | end 278 | end 279 | 280 | end 281 | % End methods section 282 | 283 | % End class definition 284 | end 285 | -------------------------------------------------------------------------------- /URToolboxScript.script: -------------------------------------------------------------------------------- 1 | #URToolboxScript 2 | # 3 | # UR Toolbox Script 4 | # UR Script interface for controlling smooth trajectories of Universal 5 | # Robot manipulators. This script is intended for use with the UR class 6 | # included with the UR Toolbox for MATLAB. 7 | # 8 | # This script leverages a PID controller on the trajectory error in joint 9 | # space. Three scalar gain values are defined: 10 | # Kp - proportional gain applied to difference between the desired and 11 | # current joint configuration. 12 | # Ki - integral gain applied to the integral of the difference between 13 | # the desired and current joint configuration. 14 | # -> NOTE: Large values of Ki can lead to integrator windup 15 | # issues. 16 | # Kd - velocity tracking gain applied to the desired joint velocity. 17 | # 18 | # Basic Controller Equation: 19 | # qdot = Kp(qd-q) + Ki*integral(qd-q)dt + Kd*qd_dot 20 | # 21 | # Basic Controller Equation (LaTeX): 22 | # \dot{q} = K_p (q_{d}(t) - q(t)) + K_i \int_{0}^{t}(q_{d}(t) - q(t)) + K_d \dot{q}_{d}(t)dt 23 | # 24 | # Valid Script Inputs: 25 | # Joint Trajectory Waypoint Command (using PID controller) 26 | # 12 element floating point array containing: 27 | # qd - 6 element desired joint position 28 | # qd_dot - 6 element desired joint velocity 29 | # msg = (%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f) 30 | # Controller Gain Update Command 31 | # 3 element floating point array containing: 32 | # Kp - scalar proportional gain 33 | # Ki - scalar integral gain 34 | # Kd - scalar derivative gain 35 | # msg = (%f,%f,%f) 36 | # Joint Position Command (using movej, linear movement in joint space) 37 | # 7 element floating point array containing: 38 | # qd - 6 element desired joint position 39 | # msg = (1,%f,%f,%f,%f,%f,%f) 40 | # Joint Velocity Command (using speedj) 41 | # 7 element floating point array containing: 42 | # qd_dot - 6 element desired joint velocity 43 | # msg = (2,%f,%f,%f,%f,%f,%f) 44 | # Joint Position Command (using movel, linear movement in task space) 45 | # 7 element floating point array containing: 46 | # qd - 6 element desired joint position 47 | # msg = (3,%f,%f,%f,%f,%f,%f) 48 | # 49 | # K. Strotz, M. Kutzer, & L. DeVries, 15Aug2016, USNA 50 | 51 | # Close any existing socket connections 52 | socket_close() 53 | 54 | # Establish new parameters and open connection to server 55 | # ADDRESS = "${HOSTNAME}" # Auto generate hostname 56 | ADDRESS = "10.1.1.5" # Hardcoded hostname 57 | # PORT = ${HOSTPORT} # Auto generate portname 58 | PORT = 30002 # Hardcoded portname 59 | 60 | isRunning = socket_open(ADDRESS,PORT) # Open socket 61 | 62 | ########################################################################### 63 | # Check socket 64 | ########################################################################### 65 | if isRunning: 66 | textmsg("Successfully connected to server.") 67 | textmsg("-> Address: ",ADDRESS) 68 | textmsg("-> Port: ",PORT) 69 | else: 70 | textmsg("Unable to connect to server.") 71 | textmsg("-> Address: ",ADDRESS) 72 | textmsg("-> Port: ",PORT) 73 | end 74 | 75 | ########################################################################### 76 | # Initialize global variables 77 | ########################################################################### 78 | # Set initial trigger conditions 79 | jwaypointTrigger = False # Waypoint controller trigger 80 | jmotionTrigger = False # Standard motion module function trigger 81 | 82 | # Set initial motion type flag 83 | jmotionFlag = 0 84 | 85 | # Initialize gains 86 | Kp = 1.0 87 | Ki = 0.0 88 | Kd = 0.0 89 | 90 | # Initialize joint values 91 | qd = get_actual_joint_positions() # Initialize desired joint positions 92 | q = get_actual_joint_positions() # Initialize current joint positions 93 | q_dot = [0,0,0,0,0,0] # Initialize current joint velocities 94 | qd_dot = [0,0,0,0,0,0] # Initialize desired joint velocities 95 | 96 | # Initialize integral term for controller 97 | Int_Delta_q = [0,0,0,0,0,0] # Initialize numeric integral error \int_{0}^{t}(q_{d}(t) - q(t)) 98 | Delta_q = [0,0,0,0,0,0] # Initialize joint error 99 | Old_Delta_q = [0,0,0,0,0,0] # Initialize old joint error 100 | 101 | # Initialize timer parameters 102 | frame = 0 # Current frame 103 | dt = 0.008 # Frame time (seconds) 104 | t = 0 # Current script time (seconds) 105 | New_t = 0 # Updated time (seconds) 106 | Old_t = 0 # Previous time (seconds) 107 | 108 | # Initialize motion parameters 109 | v = 1.05 # Joint velocity of leading axis (rad/s) 110 | a = 1.40 # Joint acceleration of leading axis (rad/s^2) 111 | n_min = 9 # Number of frames before function returns 112 | t_min = n_min*dt # minimal time before function returns 113 | 114 | ########################################################################### 115 | # Timing using threads and frames 116 | # Time is estimated using the UR documentation assuming every thread is 117 | # executed in 0.008 seconds (125 Hz). Each thread increments the frame 118 | # and subsequently the time associated. 119 | # 120 | # NOTE: This is a current work-around until a URscript time parameter is 121 | # available. There is no current documentation in the URscript API 122 | # to access the UR system time. This should be replaced in the 123 | # future. 124 | ########################################################################### 125 | 126 | ########################################################################### 127 | # Define control thread 128 | ########################################################################### 129 | thread controlThread(): 130 | while isRunning: 131 | 132 | # Update frame and time 133 | frame = frame + 1 134 | t = frame * dt 135 | 136 | if jwaypointTrigger: 137 | ############################################################### 138 | # Define waypoint trajectory control module 139 | ############################################################### 140 | New_t = t # Get current time 141 | 142 | q = get_actual_joint_positions() # Current joint positions 143 | q_dot = get_actual_joint_speeds() # Current joint velocities 144 | 145 | # Apply controller 146 | i = 0 # Joint number 147 | while i < 6: 148 | # Update error term 149 | Delta_q[i] = qd[i]-q[i] 150 | 151 | # Update integral term 152 | # - Integral error using the actual time difference 153 | Int_Delta_q[i] = Int_Delta_q[i] + (Delta_q[i]+Old_Delta_q[i])/2 * (New_t - Old_t) 154 | # - Integral error using the assumed 125 Hz thread rate 155 | #Int_Delta_q[i] = Int_Delta_q[i] + (Delta_q[i]+Old_Delta_q[i])/2 * dt 156 | 157 | # Calculate current velocity 158 | q_dot[i] = Kp*(qd[i]-q[i]) + Ki*(Int_Delta_q[i]) + Kd*qd_dot[i] 159 | i=i+1 160 | end 161 | 162 | if norm(q_dot) > 0.000030: 163 | # Move toward waypoint 164 | speedj(q_dot,a,t_min) # Apply joint velocity 165 | frame = frame + (n_min-1) # Account for speedj execution 166 | else: 167 | # Stop movement 168 | speedj([0,0,0,0,0,0],a,t_min) # Zero joint velocity 169 | frame = frame + (n_min-1) # Account for speedj execution 170 | 171 | textmsg("Static waypoint achieved: ",norm(q_dot)) 172 | jwaypointTrigger = False 173 | end 174 | 175 | Old_t = New_t # Save old time 176 | Old_Delta_q = Delta_q # Save old joint error 177 | 178 | elif jmotionTrigger: 179 | ############################################################### 180 | # Define standard motion module 181 | ############################################################### 182 | New_t = t # Get current time 183 | 184 | if jmotionFlag == 1: 185 | # movej 186 | movej(qd) # Set joint position 187 | elif jmotionFlag == 2: 188 | # speedj 189 | if New_t - Old_t < 0.5: 190 | speedj(qd_dot,a,t_min) # Set joint velocity 191 | frame = frame + (n_min-1) # Account for speedj execution 192 | else: 193 | # Stop movement 194 | speedj([0,0,0,0,0,0],a,t_min) # Zero joint velocity 195 | frame = frame + (n_min-1) # Account for speedj execution 196 | 197 | textmsg("Joint velocity timeout.") 198 | jmotionTrigger = False 199 | end 200 | elif jmotionFlag == 3: 201 | # movel 202 | movel(qd) # Set joint position 203 | else: 204 | # No motion is required 205 | # Sleep for part of frame prior to sync() 206 | jmotionTrigger = False 207 | sleep(dt/2) 208 | end 209 | 210 | else: 211 | # No motion is required 212 | # Sleep for part of frame prior to sync() 213 | sleep(dt/2) 214 | 215 | end # trigger 216 | 217 | sync() 218 | 219 | end # running 220 | end # thread 221 | 222 | ########################################################################### 223 | # Start threads 224 | ########################################################################### 225 | controlHandle = run controlThread() 226 | 227 | ########################################################################### 228 | # Check if threads are running 229 | ########################################################################### 230 | if controlHandle: 231 | textmsg("Control thread is running.") 232 | else: 233 | textmsg("Unable to start control thread.") 234 | isRunning = False 235 | end 236 | 237 | ########################################################################### 238 | # Begin main loop 239 | # Receive values from socket, 240 | # Calculate velocities, and 241 | # Send velocities hand off to thread 242 | ########################################################################### 243 | t = 0 # Reinitialize time 244 | frame = 0 # Reinitialize frame 245 | iter = 0 # Main loop iteration counter 246 | while isRunning: 247 | # Get current command 248 | msg = socket_read_ascii_float(12) # Read message 249 | 250 | if msg[0] <= 0: 251 | # No message received 252 | elif msg[0] == 3: 253 | ################################################################### 254 | # Waypoint control gain update 255 | ################################################################### 256 | if msg[1] < 0: 257 | textmsg("Kp must be greater than or equal to 0.") 258 | elif msg[2] < 0: 259 | textmsg("Ki must be greater than or equal to 0.") 260 | elif msg[3] < 0: 261 | textmsg("Kd must be greater than or equal to 0.") 262 | else: 263 | Kp = msg[1] 264 | Ki = msg[2] 265 | Kd = msg[3] 266 | textmsg("Waypoint trajectory gain update: ",t) 267 | textmsg("-> Kp: ",Kp) 268 | textmsg("-> Ki: ",Ki) 269 | textmsg("-> Kd: ",Kd) 270 | end 271 | elif msg[0] == 7: 272 | ################################################################### 273 | # Joint position or velocity command 274 | ################################################################### 275 | if msg[1] == 1: 276 | ############################################################### 277 | # Joint position command (using movej) 278 | # Linear move in joint space 279 | ############################################################### 280 | jwaypointTrigger = False # Disable waypoint controller 281 | jmotionFlag = msg[1] # Set motion flag 282 | 283 | qd = [msg[2],msg[3],msg[4],msg[5],msg[6],msg[7]] # Define desired joint position 284 | 285 | textmsg("Joint position command (using movej): ",t) # Display message description 286 | textmsg("-> movej: ", qd) # Display joint position 287 | 288 | jmotionTrigger = True # Enable standard motion module 289 | elif msg[1] == 2: 290 | ############################################################### 291 | # Joint velocity command (using speedj) 292 | ############################################################### 293 | jwaypointTrigger = False # Disable waypoint controller 294 | jmotionFlag = msg[1] # Set motion flag 295 | 296 | qd_dot = [msg[2],msg[3],msg[4],msg[5],msg[6],msg[7]] # Define desired joint position 297 | 298 | textmsg("Joint velocity command (using speedj): ",t) # Display message description 299 | textmsg("-> speedj: ", qd_dot) # Display joint velocity 300 | 301 | Old_t = t # Reinitialize prior time 302 | jmotionTrigger = True # Enable standard motion module 303 | elif msg[1] == 3: 304 | ############################################################### 305 | # Joint position command (using movel) 306 | # Linear move in task space 307 | ############################################################### 308 | jwaypointTrigger = False # Disable waypoint controller 309 | jmotionFlag = msg[1] # Set motion flag 310 | 311 | qd = [msg[2],msg[3],msg[4],msg[5],msg[6],msg[7]] # Define desired joint position 312 | 313 | textmsg("Joint position command (using movel): ", t) # Display message description 314 | textmsg("-> movel: ", qd) # Display joint position 315 | 316 | jmotionTrigger = True # Enable standard motion module 317 | else: 318 | textmsg("Invalid message identifier: ",t) 319 | textmsg("-> msg: ", msg) 320 | end 321 | elif msg[0] == 12: 322 | ################################################################### 323 | # Waypoint control command 324 | ################################################################### 325 | jmotionTrigger = False # Disable standard motion module 326 | jmotionFlag = 0 # Set motion flag 327 | 328 | qd = [msg[1],msg[2],msg[3],msg[4],msg[5],msg[6]] # Desired joint position 329 | qd_dot = [msg[7],msg[8],msg[9],msg[10],msg[11],msg[12]] # Desired joint velocity 330 | 331 | textmsg("Waypoint control command: ", t) 332 | textmsg("-> qd: ", qd) # Display desired joint position 333 | textmsg("-> qd_dot: ", qd_dot) # Display desired joint velocity 334 | 335 | Old_t = t # Reinitialize prior time 336 | jwaypointTrigger = True # Enable waypoint controller 337 | else: 338 | textmsg("Unknown message length: ",msg[0]) 339 | textmsg("-> msg: ", msg) 340 | end 341 | 342 | if t < 0: 343 | isRunning = False 344 | break 345 | else: 346 | isRunning = True 347 | end 348 | 349 | # Display main loop iteration, frame number, and estimated time 350 | iter = iter + 1 351 | #textmsg(iter) 352 | #textmsg(frame) 353 | #textmsg("t = ",t) 354 | textmsg("[Loop Iteration, Frame, Time (s)]: ", [iter, frame, t]) 355 | 356 | sync() 357 | end 358 | 359 | ########################################################################### 360 | # Terminate program 361 | ########################################################################### 362 | if controlHandle: 363 | textmsg("Killing control thread.") 364 | join controlHandle # Allow the thread to finish 365 | kill controlHandle # Kill the thread (this is redundant) 366 | end 367 | 368 | textmsg("Closing socket.") 369 | socket_close() 370 | 371 | textmsg("Terminating program.") 372 | halt # Terminate the program 373 | -------------------------------------------------------------------------------- /installURToolbox.m: -------------------------------------------------------------------------------- 1 | function installURToolbox(replaceExisting) 2 | % INSTALLURTOOLBOX installs UR Toolbox for MATLAB. 3 | % INSTALLURTOOLBOX installs UR Toolbox into the following 4 | % locations: 5 | % Source: Destination 6 | % URToolboxFunctions: matlabroot\toolbox\ur 7 | % URToolboxSupport: matlabroot\toolbox\ur\OptiTrackToolboxSupport 8 | % 9 | % INSTALLURTOOLBOX(true) installs UR Toolbox regardless of 10 | % whether a copy of the UR toolbox exists in the MATLAB root. 11 | % 12 | % INSTALLURTOOLBOX(false) installs UR Toolbox only if no copy 13 | % of the UR toolbox exists in the MATLAB root. 14 | % 15 | % M. Kutzer 17Feb2016, USNA 16 | 17 | % Updates 18 | % 19Dec2016 - Added genpath to include all simulation components in the 19 | % path. 20 | % 15Mar2018 - Updated to include try/catch for required toolbox 21 | % installations and include msgbox warning when download 22 | % fails. 23 | % 08Jan2021 - Updated ToolboxUpdate 24 | % 08Jan2021 - Updated action cancelled when python is not installed 25 | 26 | % TODO - Allow users to create a local version if admin rights are not 27 | % possible. 28 | 29 | %% Define support toolboxes 30 | supportToolboxes = {... 31 | 'URSimulation'}; 32 | 33 | %% Assign tool/toolbox specific parameters 34 | dirName = 'ur'; 35 | 36 | %% Check inputs 37 | if nargin == 0 38 | replaceExisting = []; 39 | end 40 | 41 | %% Installation error solution(s) 42 | adminSolution = sprintf(... 43 | ['Possible solution:\n',... 44 | '\t(1) Close current instance of MATLAB\n',... 45 | '\t(2) Open a new instance of MATLAB "as administrator"\n',... 46 | '\t\t(a) Locate MATLAB shortcut\n',... 47 | '\t\t(b) Right click\n',... 48 | '\t\t(c) Select "Run as administrator"\n']); 49 | 50 | %% Check for toolbox directory 51 | toolboxRoot = fullfile(matlabroot,'toolbox',dirName); 52 | isToolbox = exist(toolboxRoot,'file'); 53 | if isToolbox == 7 54 | % Apply replaceExisting argument 55 | if isempty(replaceExisting) 56 | choice = questdlg(sprintf(... 57 | ['MATLAB Root already contains the UR Toolbox.\n',... 58 | 'Would you like to replace the existing toolbox?']),... 59 | 'Replace Existing Universal Robot Toolbox','Yes','No','Cancel','Yes'); 60 | elseif replaceExisting 61 | choice = 'Yes'; 62 | else 63 | choice = 'No'; 64 | end 65 | % Replace existing or cancel installation 66 | switch choice 67 | case 'Yes' 68 | rmpath(toolboxRoot); 69 | warning off 70 | [isRemoved, msg, msgID] = rmdir(toolboxRoot,'s'); 71 | warning on 72 | if isRemoved 73 | fprintf('Previous version of UR Toolbox removed successfully.\n'); 74 | else 75 | fprintf('Failed to remove old UR Toolbox folder:\n\t"%s"\n',toolboxRoot); 76 | fprintf(adminSolution); 77 | error(msgID,msg); 78 | end 79 | case 'No' 80 | fprintf('UR Toolbox currently exists, installation cancelled.\n'); 81 | return 82 | case 'Cancel' 83 | fprintf('Action cancelled.\n'); 84 | return 85 | otherwise 86 | error('Unexpected response.'); 87 | end 88 | end 89 | 90 | %% Create Toolbox Path 91 | [isDir,msg,msgID] = mkdir(toolboxRoot); 92 | if isDir 93 | fprintf('UR toolbox folder created successfully:\n\t"%s"\n',toolboxRoot); 94 | else 95 | fprintf('Failed to create UR Toolbox folder:\n\t"%s"\n',toolboxRoot); 96 | fprintf(adminSolution); 97 | error(msgID,msg); 98 | end 99 | 100 | %% Migrate toolbox folder contents 101 | toolboxContent = 'URToolboxFunctions'; 102 | if ~isdir(toolboxContent) 103 | error(sprintf(... 104 | ['Change your working directory to the location of "installURToolbox.m".\n',... 105 | '\n',... 106 | 'If this problem persists:\n',... 107 | '\t(1) Unzip your original download of "URToolbox" into a new directory\n',... 108 | '\t(2) Open a new instance of MATLAB "as administrator"\n',... 109 | '\t\t(a) Locate MATLAB shortcut\n',... 110 | '\t\t(b) Right click\n',... 111 | '\t\t(c) Select "Run as administrator"\n',... 112 | '\t(3) Change your "working directory" to the location of "installURToolbox.m"\n',... 113 | '\t(4) Enter "installURToolbox" (without quotes) into the command window\n',... 114 | '\t(5) Press Enter.'])); 115 | end 116 | files = dir(toolboxContent); 117 | wb = waitbar(0,'Copying UR Toolbox toolbox contents...'); 118 | n = numel(files); 119 | fprintf('Copying UR Toolbox contents:\n'); 120 | for i = 1:n 121 | % source file location 122 | source = fullfile(toolboxContent,files(i).name); 123 | % destination location 124 | destination = toolboxRoot; 125 | if files(i).isdir 126 | switch files(i).name 127 | case '.' 128 | %Ignore 129 | case '..' 130 | %Ignore 131 | otherwise 132 | fprintf('\t%s...',files(i).name); 133 | nDestination = fullfile(destination,files(i).name); 134 | [isDir,msg,msgID] = mkdir(nDestination); 135 | if isDir 136 | [isCopy,msg,msgID] = copyfile(source,nDestination,'f'); 137 | if isCopy 138 | fprintf('[Complete]\n'); 139 | else 140 | bin = msg == char(10); 141 | msg(bin) = []; 142 | bin = msg == char(13); 143 | msg(bin) = []; 144 | fprintf('[Failed: "%s"]\n',msg); 145 | end 146 | else 147 | bin = msg == char(10); 148 | msg(bin) = []; 149 | bin = msg == char(13); 150 | msg(bin) = []; 151 | fprintf('[Failed: "%s"]\n',msg); 152 | end 153 | end 154 | else 155 | fprintf('\t%s...',files(i).name); 156 | [isCopy,msg,msgID] = copyfile(source,destination,'f'); 157 | 158 | if isCopy == 1 159 | fprintf('[Complete]\n'); 160 | else 161 | bin = msg == char(10); 162 | msg(bin) = []; 163 | bin = msg == char(13); 164 | msg(bin) = []; 165 | fprintf('[Failed: "%s"]\n',msg); 166 | end 167 | end 168 | waitbar(i/n,wb); 169 | end 170 | set(wb,'Visible','off'); 171 | 172 | %% Migrate toolbox support folder 173 | % TODO - search for python directory and prompt user to find the correct 174 | % one... 175 | if exist('C:\Python34\Lib','dir') ~= 7 176 | % Python 3.4 is not installed in the standard directory 177 | fprintf(2,'Python 3.4.4 was not found in the standard directory.\n'); 178 | pythonRoot = uigetdir('', 'Select Python 3.4.4 "Lib" directory.'); 179 | else 180 | pythonRoot = fullfile('C:\Python34\Lib'); 181 | end 182 | 183 | toolboxContent = 'URToolboxSupport'; 184 | if ~isdir(toolboxContent) 185 | error(sprintf(... 186 | ['Change your working directory to the location of "installURToolbox.m".\n',... 187 | '\n',... 188 | 'If this problem persists:\n',... 189 | '\t(1) Unzip your original download of "URToolbox" into a new directory\n',... 190 | '\t(2) Open a new instance of MATLAB "as administrator"\n',... 191 | '\t\t(a) Locate MATLAB shortcut\n',... 192 | '\t\t(b) Right click\n',... 193 | '\t\t(c) Select "Run as administrator"\n',... 194 | '\t(3) Change your "working directory" to the location of "installURToolbox.m"\n',... 195 | '\t(4) Enter "installURToolbox" (without quotes) into the command window\n',... 196 | '\t(5) Press Enter.'])); 197 | end 198 | files = dir(toolboxContent); 199 | 200 | if numel(pythonRoot) == 1 && pythonRoot == 0 201 | fprintf(2,' -> ACTION CANCELLED\n'); 202 | fprintf(2,['',... 203 | '\tNOTE: All simulation and kinematic functions should be fully\n',... 204 | '\tinstalled. You will not be able to interact with UR hardware\n',... 205 | '\tuntil Python 3.4.4 is successfully installed. Please install\n',... 206 | '\tPython and run installURToolbox again.\n']); 207 | else 208 | wb = waitbar(0,'Copying UR Toolbox toolbox contents...'); 209 | n = numel(files); 210 | fprintf('Copying UR Toolbox contents:\n'); 211 | for i = 1:n 212 | % source file location 213 | source = fullfile(toolboxContent,files(i).name); 214 | % destination location 215 | destination = pythonRoot; 216 | if files(i).isdir 217 | switch files(i).name 218 | case '.' 219 | %Ignore 220 | case '..' 221 | %Ignore 222 | otherwise 223 | fprintf('\t%s...',files(i).name); 224 | nDestination = fullfile(destination,files(i).name); 225 | [isDir,msg,msgID] = mkdir(nDestination); 226 | if isDir 227 | [isCopy,msg,msgID] = copyfile(source,nDestination,'f'); 228 | if isCopy 229 | fprintf('[Complete]\n'); 230 | else 231 | bin = msg == char(10); 232 | msg(bin) = []; 233 | bin = msg == char(13); 234 | msg(bin) = []; 235 | fprintf('[Failed: "%s"]\n',msg); 236 | end 237 | else 238 | bin = msg == char(10); 239 | msg(bin) = []; 240 | bin = msg == char(13); 241 | msg(bin) = []; 242 | fprintf('[Failed: "%s"]\n',msg); 243 | end 244 | end 245 | else 246 | fprintf('\t%s...',files(i).name); 247 | if all( destination == 0 ) 248 | % Ignore cancelled operation... 249 | isCopy = false; 250 | else 251 | [isCopy,msg,msgID] = copyfile(source,destination,'f'); 252 | end 253 | 254 | if isCopy == 1 255 | fprintf('[Complete]\n'); 256 | else 257 | bin = msg == char(10); 258 | msg(bin) = []; 259 | bin = msg == char(13); 260 | msg(bin) = []; 261 | fprintf('[Failed: "%s"]\n',msg); 262 | end 263 | end 264 | waitbar(i/n,wb); 265 | end 266 | set(wb,'Visible','off'); 267 | end 268 | 269 | %% Save toolbox path 270 | addpath(genpath(toolboxRoot),'-end'); 271 | %addpath(toolboxRoot,'-end'); 272 | savepath; 273 | 274 | %% Rehash toolbox cache 275 | fprintf('Rehashing Toolbox Cache...'); 276 | rehash TOOLBOXCACHE 277 | fprintf('[Complete]\n'); 278 | 279 | %% Install Python Modules 280 | % TODO - error check! 281 | if numel(pythonRoot) == 1 && pythonRoot == 0 282 | % ACTION CANCELLED 283 | else 284 | fprintf('Installing necessary Python modules...'); 285 | try 286 | installModule = py.importlib.import_module('URModulesInstall'); 287 | installModule.installURModules(); 288 | fprintf('[Complete]\n'); 289 | catch ME 290 | fprintf('[Failed]\n') 291 | fprintf(2,'ERROR MESSAGE:\n\t%s\n\n',ME.message); 292 | fprintf(2,'Failed to install necessary Python modules. To install manually:\n') 293 | fprintf(2,'\t - Open the Command Prompt,\n'); 294 | fprintf(2,'\t - Switch to the Python 3.4 Scripts directory\n'); 295 | fprintf(2,'\t\t run "cd C:\\Python34\\Scripts"\n'); 296 | fprintf(2,'\t - Install the math3D module\n'); 297 | fprintf(2,'\t\t run "pip install math3d"\n'); 298 | fprintf(2,'\t - Install the numpy module\n'); 299 | fprintf(2,'\t\t run "pip install numpy"\n'); 300 | fprintf(2,'\t - Install the urx module\n'); 301 | fprintf(2,'\t\t run "pip install urx"\n'); 302 | end 303 | end 304 | 305 | %% Install/Update required toolboxes 306 | for i = 1:numel(supportToolboxes) 307 | ToolboxUpdate( supportToolboxes{i} ); 308 | end 309 | 310 | end 311 | 312 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 313 | %%% TOOLBOX UPDATE 314 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 315 | % 26Mar2021 316 | function ToolboxUpdate(toolboxName) 317 | 318 | %% Setup functions 319 | ToolboxVer = str2func( sprintf('%sToolboxVer',toolboxName) ); 320 | installToolbox = str2func( sprintf('install%sToolbox',toolboxName) ); 321 | 322 | %% Check current version 323 | try 324 | A = ToolboxVer; 325 | catch ME 326 | A = []; 327 | fprintf('No previous version of %s detected.\n',toolboxName); 328 | end 329 | 330 | %% Setup temporary file directory 331 | % TODO - check "ok" 332 | fprintf('Creating %s Toolbox temporary directory...',toolboxName); 333 | tmpFolder = sprintf('%sToolbox',toolboxName); 334 | pname = fullfile(tempdir,tmpFolder); 335 | if isfolder(pname) 336 | % Remove existing directory 337 | [ok,msg] = rmdir(pname,'s'); 338 | end 339 | % Create new directory 340 | [ok,msg] = mkdir(tempdir,tmpFolder); 341 | fprintf('SUCCESS\n'); 342 | 343 | %% Download and unzip toolbox (GitHub) 344 | %url = sprintf('https://github.com/kutzer/%sToolbox/archive/master.zip',toolboxName); <--- Github removed references to "master" 345 | %url = sprintf('https://github.com/kutzer/%sToolbox/archive/refs/heads/main.zip',toolboxName); 346 | 347 | % Check possible branches 348 | defBranches = {'master','main'}; 349 | for i = 1:numel(defBranches) 350 | % Check default branch 351 | defBranch = defBranches{i}; 352 | url = sprintf('https://github.com/kutzer/%sToolbox/archive/refs/heads/%s.zip',... 353 | toolboxName,defBranch); 354 | % Check url 355 | [~,status] = urlread(url); 356 | fprintf('Checking for branch "%s"...',defBranch); 357 | if status 358 | fprintf('FOUND\n'); 359 | break 360 | else 361 | fprintf('INVALID\n'); 362 | end 363 | end 364 | 365 | % Download and unzip repository 366 | fprintf('Downloading the %s Toolbox...',toolboxName); 367 | try 368 | %fnames = unzip(url,pname); 369 | %urlwrite(url,fullfile(pname,tmpFname)); 370 | tmpFname = sprintf('%sToolbox-master.zip',toolboxName); 371 | websave(fullfile(pname,tmpFname),url); 372 | fnames = unzip(fullfile(pname,tmpFname),pname); 373 | delete(fullfile(pname,tmpFname)); 374 | 375 | fprintf('SUCCESS\n'); 376 | confirm = true; 377 | catch ME 378 | fprintf('FAILED\n'); 379 | confirm = false; 380 | fprintf(2,'ERROR MESSAGE:\n\t%s\n',ME.message); 381 | end 382 | 383 | %% Check for successful download 384 | alternativeInstallMsg = [... 385 | sprintf('Manually download the %s Toolbox using the following link:\n',toolboxName),... 386 | newline,... 387 | sprintf('%s\n',url),... 388 | newline,... 389 | sprintf('Once the file is downloaded:\n'),... 390 | sprintf('\t(1) Unzip your download of the "%sToolbox"\n',toolboxName),... 391 | sprintf('\t(2) Change your "working directory" to the location of "install%sToolbox.m"\n',toolboxName),... 392 | sprintf('\t(3) Enter "install%sToolbox" (without quotes) into the command window\n',toolboxName),... 393 | sprintf('\t(4) Press Enter.')]; 394 | 395 | if ~confirm 396 | warning('InstallToolbox:FailedDownload','Failed to download updated version of %s Toolbox.',toolboxName); 397 | fprintf(2,'\n%s\n',alternativeInstallMsg); 398 | 399 | msgbox(alternativeInstallMsg, sprintf('Failed to download %s Toolbox',toolboxName),'warn'); 400 | return 401 | end 402 | 403 | %% Find base directory 404 | install_pos = strfind(fnames, sprintf('install%sToolbox.m',toolboxName) ); 405 | sIdx = cell2mat( install_pos ); 406 | cIdx = ~cell2mat( cellfun(@isempty,install_pos,'UniformOutput',0) ); 407 | 408 | pname_star = fnames{cIdx}(1:sIdx-1); 409 | 410 | %% Get current directory and temporarily change path 411 | cpath = cd; 412 | cd(pname_star); 413 | 414 | %% Install Toolbox 415 | installToolbox(true); 416 | 417 | %% Move back to current directory and remove temp file 418 | cd(cpath); 419 | [ok,msg] = rmdir(pname,'s'); 420 | if ~ok 421 | warning('Unable to remove temporary download folder. %s',msg); 422 | end 423 | 424 | %% Complete installation 425 | fprintf('Installation complete.\n'); 426 | 427 | end 428 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 429 | %%% END TOOLBOX UPDATE 430 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -------------------------------------------------------------------------------- /URToolboxFunctions/URX.m: -------------------------------------------------------------------------------- 1 | classdef URX < matlab.mixin.SetGet % Handle 2 | % URX handle class for accessing functions from the URX Python Library 3 | % 4 | % obj = URX(RobotIP) creates a URX object for interfacing the robot. 5 | % 6 | % URX Methods 7 | % Initialize 8 | % Home 9 | % Stow 10 | % Zero 11 | % Undo 12 | % set 13 | % get 14 | % delete 15 | % 16 | % URX Properties 17 | % -Connection details 18 | % RobotIP 19 | % 20 | % -Python module and connection properties (hidden) 21 | % URServerModule 22 | % URXModule 23 | % 24 | % -Universal Robot Model 25 | % URmodel - string argument defining model of Universal Robot 26 | % 27 | % -DH Table 28 | % DHtable - nx4 array defining the DH table of the Universal 29 | % Robot 30 | % 31 | % -Joint Values 32 | % Joints - 1x6 array containing joint values (radians) 33 | % Joint1 - scalar value containing joint 1 (radians) 34 | % Joint2 - scalar value containing joint 2 (radians) 35 | % Joint3 - scalar value containing joint 3 (radians) 36 | % Joint4 - scalar value containing joint 4 (radians) 37 | % Joint5 - scalar value containing joint 5 (radians) 38 | % Joint6 - scalar value containing joint 6 (radians) 39 | % 40 | % -Joint Velocities 41 | % JointVelocities - 1x6 array containing joint velocitie (radians/s) 42 | % 43 | % -Joint Torques 44 | % JointTorques - 1x6 array containing joint torques (TBD) 45 | % 46 | % -End-effector pose 47 | % Pose - 4x4 homogeneous transform representing the 48 | % end-effector pose relative to the world frame 49 | % 50 | % -Tool pose 51 | % ToolPose - 4x4 homogeneous transform representing the 52 | % tool pose relative to the world frame 53 | % 54 | % -Frame Definitions 55 | % Frame0 - Frame 0 (transformation relative to World Frame) 56 | % FrameT - Tool Frame (transformation relative to the 57 | % End-effector Frame) 58 | 59 | % -------------------------------------------------------------------- 60 | % General properties 61 | % -------------------------------------------------------------------- 62 | properties(GetAccess='public', SetAccess='private') 63 | RobotIP 64 | end 65 | 66 | properties(GetAccess='public', SetAccess='private', Hidden=true) 67 | URXModule 68 | URServer 69 | end 70 | 71 | properties(GetAccess='public', SetAccess='private') 72 | URmodel 73 | DHtable 74 | end 75 | 76 | properties(GetAccess='public', SetAccess='public') 77 | Joints % 1x6 array containing joint values (radians) 78 | JointVelocities % 1x6 array containing joint velocitie (radians/s) 79 | Pose % End-effector pose relative to the world frame 80 | ToolPose % Tool pose relative to the world frame 81 | Frame0 % Frame 0 (transformation relative to World Frame) 82 | FrameT % Tool Frame (transformation relative to the End-effector Frame) 83 | end 84 | 85 | properties(GetAccess='public', SetAccess='private') 86 | JointTorques 87 | JointCurrents 88 | JointVoltages 89 | AnalogInputs 90 | AnalogOutputs 91 | DigitalInputs 92 | DigitalOutputs 93 | ToolData 94 | end 95 | 96 | properties(GetAccess='public', SetAccess='public', Hidden=true) 97 | Joint1 % scalar value containing joint 1 (radians) 98 | Joint2 % scalar value containing joint 2 (radians) 99 | Joint3 % scalar value containing joint 3 (radians) 100 | Joint4 % scalar value containing joint 4 (radians) 101 | Joint5 % scalar value containing joint 5 (radians) 102 | Joint6 % scalar value containing joint 6 (radians) 103 | end 104 | 105 | % -------------------------------------------------------------------- 106 | % Internal properties 107 | % -------------------------------------------------------------------- 108 | properties(GetAccess='public', SetAccess='private', Hidden=true) 109 | Joints_Old % Previous joint configuration (used with Undo) 110 | isConnected % Connection status with Universal Robot (URX) 111 | end 112 | 113 | % -------------------------------------------------------------------- 114 | % Constructor/Destructor 115 | % -------------------------------------------------------------------- 116 | methods(Access='public') 117 | function obj = URX(robotIP) 118 | % Create URX Object 119 | if nargin < 1 120 | % TODO - Prompt user for IP address 121 | error('Please specify the Univeral Robot''s IP address.'); 122 | end 123 | % Initialize connection status 124 | obj.isConnected = false; 125 | % Initialize frame offsets 126 | obj.Frame0 = eye(4); 127 | obj.FrameT = eye(4); 128 | % Set the IP 129 | obj.RobotIP = robotIP; 130 | % Import URServer module 131 | fprintf('Importing URServer module...'); 132 | try 133 | obj.URServer = py.importlib.import_module('URServer'); 134 | fprintf('[IMPORTED]\n'); 135 | catch 136 | fprintf('[IMPORT FAILED]\n'); 137 | delete(obj); 138 | error('Failed to import URServer module.'); 139 | end 140 | end 141 | 142 | function delete(obj) 143 | % Delete function destructor 144 | if ~isempty(obj.URXModule) 145 | % Disconnect from URXModule 146 | if obj.isConnected 147 | obj.URServer.closeURX(obj.URXModule); 148 | end 149 | end 150 | % TODO - unload the module 151 | %clear classes % Suggested by MATLAB Documentation 152 | delete(obj); 153 | end 154 | 155 | end % end methods 156 | 157 | % -------------------------------------------------------------------- 158 | % Initialization 159 | % -------------------------------------------------------------------- 160 | methods(Access='public') 161 | function Initialize(obj) 162 | % Initialize and establish a connection with the Universal 163 | % Robot. 164 | fprintf('Connecting to Universal Robot...'); 165 | try 166 | obj.URXModule = obj.URServer.cnctURX(obj.RobotIP); 167 | obj.isConnected = true; 168 | fprintf('[CONNECTED]\n'); 169 | catch 170 | fprintf('[CONNECTION FAILED]\n'); 171 | delete(obj); 172 | error('Failed to connect to Universal Robot.'); 173 | end 174 | end 175 | 176 | end % end methods 177 | 178 | % -------------------------------------------------------------------- 179 | % General Use 180 | % -------------------------------------------------------------------- 181 | methods(Access='public') 182 | function Home(obj) 183 | % Move the UR to the home configuration 184 | % TODO - confirm home position of UR3 and UR5 185 | joints = [ 0.00;... 186 | -pi/2;... 187 | 0.00;... 188 | -pi/2;... 189 | 0.00;... 190 | 0.00]; 191 | obj.Joints = joints; 192 | end 193 | 194 | function Stow(obj) 195 | % Move the UR to the stow configuration 196 | % TODO - confirm stow position of UR3 and UR5 197 | joints = [ 0.00000;... 198 | -0.01626;... 199 | -2.77643;... 200 | 1.22148;... 201 | 1.57080;... 202 | 0.00000]; 203 | obj.Joints = joints; 204 | end 205 | 206 | function Zero(obj) 207 | % Move the UR to the zero configuration 208 | obj.Joints = zeros(6,1); 209 | end 210 | 211 | function Undo(obj) 212 | % Undo the previous move of the UR 213 | alljoints = obj.Joints_Old; 214 | if ~isempty(alljoints) 215 | obj.Joints = alljoints(:,end); 216 | alljoints(:,end) = []; 217 | obj.Joints_Old = alljoints; 218 | end 219 | end 220 | 221 | end % end methods 222 | 223 | % -------------------------------------------------------------------- 224 | % Getters/Setters 225 | % -------------------------------------------------------------------- 226 | methods 227 | % GetAccess & SetAccess ------------------------------------------ 228 | % TODO - implement setters 229 | 230 | % Joints - 1x6 array containing joint values (radians) 231 | function joints = get.Joints(obj) 232 | joints = obj.URServer.getJPos(obj.URXModule); 233 | joints = pList2mArray(joints); 234 | end 235 | %{ 236 | function obj = set.Joints(obj,joints) 237 | joints = mArray2pList(joints); 238 | obj.URServer.setJPos(obj.URXModule,joints); 239 | end 240 | %} 241 | % Joint1 - scalar value containing joint 1 (radians) 242 | function joint1 = get.Joint1(obj) 243 | joints = obj.Joints; 244 | joint1 = joints(1); 245 | end 246 | %{ 247 | function obj = set.Joint1(obj,joint1) 248 | joints = obj.Joints; 249 | joints(1) = joint1; 250 | obj.Joints = joints; 251 | end 252 | %} 253 | % Joint2 - scalar value containing joint 2 (radians) 254 | function joint2 = get.Joint2(obj) 255 | joints = obj.Joints; 256 | joint2 = joints(2); 257 | end 258 | %{ 259 | function obj = set.Joint2(obj,joint2) 260 | joints = obj.Joints; 261 | joints(2) = joint2; 262 | obj.Joints = joints; 263 | end 264 | %} 265 | % Joint3 - scalar value containing joint 3 (radians) 266 | function joint3 = get.Joint3(obj) 267 | joints = obj.Joints; 268 | joint3 = joints(3); 269 | end 270 | %{ 271 | function obj = set.Joint3(obj,joint3) 272 | joints = obj.Joints; 273 | joints(3) = joint3; 274 | obj.Joints = joints; 275 | end 276 | %} 277 | % Joint4 - scalar value containing joint 4 (radians) 278 | function joint4 = get.Joint4(obj) 279 | joints = obj.Joints; 280 | joint4 = joints(4); 281 | end 282 | %{ 283 | function obj = set.Joint4(obj,joint4) 284 | joints = obj.Joints; 285 | joints(4) = joint4; 286 | obj.Joints = joints; 287 | end 288 | %} 289 | % Joint5 - scalar value containing joint 5 (radians) 290 | function joint5 = get.Joint5(obj) 291 | joints = obj.Joints; 292 | joint5 = joints(5); 293 | end 294 | %{ 295 | function obj = set.Joint5(obj,joint5) 296 | joints = obj.Joints; 297 | joints(5) = joint5; 298 | obj.Joints = joints; 299 | end 300 | %} 301 | % Joint6 - scalar value containing joint 6 (radians) 302 | function joint6 = get.Joint6(obj) 303 | joints = obj.Joints; 304 | joint6 = joints(6); 305 | end 306 | %{ 307 | function obj = set.Joint6(obj,joint6) 308 | joints = obj.Joints; 309 | joints(6) = joint6; 310 | obj.Joints = joints; 311 | end 312 | %} 313 | % JointVelocities - 1x6 array containing joint velocitie (radians/s) 314 | function jointVelocities = get.JointVelocities(obj) 315 | jointVelocities = obj.URServer.getJVels(obj.URXModule); 316 | jointVelocities = pList2mArray(jointVelocities); 317 | end 318 | %{ 319 | function obj = set.JointVelocities(obj,jointVelocities) 320 | jointVelocities = mArray2pList(jointVelocities); 321 | obj.URServer.setJVel(obj.URXModule,jointVelocities); 322 | end 323 | %} 324 | % Pose - 4x4 homogeneous transform representing the 325 | % end-effector pose relative to the world frame 326 | function pose = get.Pose(obj) 327 | H_o2w = obj.Frame0; 328 | H_e2o = obj.URServer.getTTrans(obj.URXModule); 329 | H_e2o = pTransform2mMatrix( H_e2o ); % transformation in meters 330 | H_e2o(1:3,4) = H_e2o(1:3,4) * 1000; % transformation in millimeters 331 | H_e2w = H_o2w * H_e2o; 332 | pose = H_e2w; 333 | end 334 | %{ 335 | function obj = set.Pose(obj,pose) 336 | H_o2w = obj.Frame0; 337 | H_e2w = pose; 338 | H_e2o = invSE(H_o2w) * H_e2w; 339 | % TODO - CONVERT TO PYTHON TRANSFORMATION 340 | obj.URServer.setTTrans(obj.URXModule,H_e2o); 341 | end 342 | %} 343 | % ToolPose - 4x4 homogeneous transform representing the tool pose 344 | % relative to the world frame 345 | function toolPose = get.ToolPose(obj) 346 | H_e2w = obj.Pose; 347 | H_t2e = obj.FrameT; 348 | H_t2w = H_e2w * H_t2e; 349 | toolPose = H_t2w; 350 | end 351 | %{ 352 | function obj = set.ToolPose(obj,toolPose) 353 | H_t2w = toolPose; 354 | H_t2e = obj.FrameT; 355 | H_e2w = H_t2w * invSE(H_t2e); 356 | obj.Pose = H_e2w; 357 | end 358 | %} 359 | % Frame0 - Frame 0 (transformation relative to World Frame) 360 | function frame0 = get.Frame0(obj) 361 | frame0 = obj.Frame0; 362 | end 363 | 364 | function obj = set.Frame0(obj,frame0) 365 | % TODO - check for SE(3) 366 | obj.Frame0 = frame0; 367 | end 368 | 369 | % FrameT - Tool Frame (transformation relative to the end-effector 370 | % Frame) 371 | function frameT = get.FrameT(obj) 372 | frameT = obj.FrameT; 373 | end 374 | 375 | function obj = set.FrameT(obj,frameT) 376 | % TODO - check for SE(3) 377 | obj.FrameT = frameT; 378 | end 379 | 380 | % GetAccess ------------------------------------------------------ 381 | 382 | % JointTorques - 1x6 array containing joint torques (TBD) 383 | function jointTorques = get.JointTorques(obj) 384 | jointTorques = obj.URServer.getJTorq(obj.URXModule); 385 | jointTorques = pList2mArray(jointTorques); 386 | end 387 | 388 | % JointCurrents 389 | function jointCurrents = get.JointCurrents(obj) 390 | jointCurrents = obj.URServer.getJCurr(obj.URXModule); 391 | jointCurrents = pList2mArray(jointCurrents); 392 | end 393 | 394 | % JointVoltages 395 | function jointVoltages = get.JointVoltages(obj) 396 | jointVoltages = obj.URServer.getJVolt(obj.URXModule); 397 | jointVoltages = pList2mArray(jointVoltages); 398 | end 399 | 400 | % AnalogInputs 401 | function analogInputs = get.AnalogInputs(obj) 402 | analogInputs = obj.URServer.getAIn(obj.URXModule); 403 | analogInputs = pList2mArray(analogInputs); 404 | end 405 | 406 | % AnalogOutputs 407 | function analogOutputs = get.AnalogOutputs(obj) 408 | analogOutputs = obj.URServer.getAOut(obj.URXModule); 409 | analogOutputs = pList2mArray(analogOutputs); 410 | end 411 | 412 | % DigitalInputs 413 | function digitalInputs = get.DigitalInputs(obj) 414 | digitalInputs = obj.URServer.getDIn(obj.URXModule); 415 | digitalInputs = pList2mArray(digitalInputs); 416 | end 417 | 418 | % DigitalOutputs 419 | function digitalOutputs = get.DigitalOutputs(obj) 420 | digitalOutputs = obj.URServer.getDOut(obj.URXModule); 421 | digitalOutputs = pList2mArray(digitalOutputs); 422 | end 423 | 424 | % ToolData 425 | function toolData = get.ToolData(obj) 426 | toolData = obj.URServer.getTInfo(obj.URXModule); 427 | toolData = pDict2mStruct(toolData); 428 | % TODO - convert structured fields to MATLAB format 429 | end 430 | 431 | end % end methods 432 | end % end classdef 433 | --------------------------------------------------------------------------------