From a9febce7d89a970707b3115aead745f0c06c5625 Mon Sep 17 00:00:00 2001 From: John Wason Date: Thu, 21 Dec 2023 15:34:31 -0500 Subject: [PATCH 1/6] Refactor package --- .gitignore | 161 ++++++++++++++++++ examples/example.py | 33 ++++ examples/io_example.py | 9 + examples/multi_coord_example.py | 34 ++++ examples/multi_example.py | 34 ++++ examples/read_joint_example.py | 11 ++ pyproject.toml | 17 ++ setup.py | 13 -- .../__init__.py | 1 + .../fanuc_motion_program_exec_client.py | 144 ---------------- 10 files changed, 300 insertions(+), 157 deletions(-) create mode 100644 examples/example.py create mode 100644 examples/io_example.py create mode 100644 examples/multi_coord_example.py create mode 100644 examples/multi_example.py create mode 100644 examples/read_joint_example.py create mode 100644 pyproject.toml delete mode 100644 setup.py create mode 100644 src/fanuc_motion_program_exec_client/__init__.py rename fanuc_motion_program_exec_client.py => src/fanuc_motion_program_exec_client/fanuc_motion_program_exec_client.py (85%) diff --git a/.gitignore b/.gitignore index 2c8dc7d..566c4dd 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,164 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +#pdm.lock +# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it +# in version control. +# https://pdm.fming.dev/#use-with-ide +.pdm.toml + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/ + *.pc *.tp build/* diff --git a/examples/example.py b/examples/example.py new file mode 100644 index 0000000..a90b166 --- /dev/null +++ b/examples/example.py @@ -0,0 +1,33 @@ +from fanuc_motion_program_exec_client import * + +tp = TPMotionProgram() + +# robtarget(motion group, uframe, utool, [x,y,z], [w,p,r], confdata, external axis) +pt1 = robtarget(1,1,2,[1850,200,290],[-180,0,0],confdata('N','U','T',0,0,0),[0]*6) +pt2 = robtarget(1,1,2,[1850,200,589],[-180,0,0],confdata('N','U','T',0,0,0),[0]*6) +# jointtarget(motion group, uframe, utool, [j1,j2,j3,j4,j5,j6], confdata, external axis) +jt1 = jointtarget(1,1,2,[0,20,-10,0,-20,10],[0]*6) +pt3 = robtarget(1,1,2,[1850,250,400],[-180,0,0],confdata('N','U','T',0,0,0),[0]*6) + +tp.moveL(pt1,50,'mmsec',100) +tp.moveL(pt2,50,'mmsec',-1) +tp.moveJ(jt1,100,'%',-1) +tp.moveL(pt2,50,'mmsec',100) +tp.moveC(pt3,pt1,50,'mmsec',-1) +tp.moveL(pt2,50,'mmsec',-1) + +print(tp.get_tp()) + +client = FANUCClient() +res = client.execute_motion_program(tp) + +with open("fanuc_log.csv","wb") as f: + f.write(res) + +print(res.decode('utf-8')) + +tp = TPMotionProgram() +tp.moveL(pt1,50,'mmsec',100) +tp.moveL(pt2,50,'mmsec',-1) +client = FANUCClient() +res = client.execute_motion_program(tp) \ No newline at end of file diff --git a/examples/io_example.py b/examples/io_example.py new file mode 100644 index 0000000..cfdbe8f --- /dev/null +++ b/examples/io_example.py @@ -0,0 +1,9 @@ +from fanuc_motion_program_exec_client import * + +client = FANUCClient('192.168.0.1') +client.set_ioport('DOUT',10,True) +res = client.read_ioport('DOUT',10) +print("The port is:",res) +client.set_ioport('DOUT',10,False) +res = client.read_ioport('DOUT',10) +print("The port is:",res) \ No newline at end of file diff --git a/examples/multi_coord_example.py b/examples/multi_coord_example.py new file mode 100644 index 0000000..689f5ba --- /dev/null +++ b/examples/multi_coord_example.py @@ -0,0 +1,34 @@ +from fanuc_motion_program_exec_client import * + +tp_lead = TPMotionProgram() +tp_follow = TPMotionProgram() + +# robtarget(motion group, uframe, utool, [x,y,z], [w,p,r], confdata, external axis) +# jointtarget(motion group, uframe, utool, [j1,j2,j3,j4,j5,j6], confdata, external axis) + +# these are for follower robot, follower must be motion group 1 +jt11 = jointtarget(1,1,2,[-49.7,4.3,-30.9,-20.9,-35.8,52.1],[0]*6) +pt12 = robtarget(1,1,2,[1383.1,-484.0,940.6],[171.5,-26.8,-9.8],confdata('N','U','T',0,0,0),[0]*6) +pt13 = robtarget(1,1,2,[1166.0,0,1430.0],[180.0,0,0],confdata('N','U','T',0,0,0),[0]*6) + +# these are for leader robot, leader must be motion group 2 +jt21 = jointtarget(2,1,2,[38.3,23.3,-10.7,45.7,-101.9,-48.3],[0]*6) +pt22 = robtarget(2,1,2,[994.0,924.9,1739.5],[163.1,1.5,-1.0],confdata('N','U','T',0,0,0),[0]*6) +pt23 = robtarget(2,1,2,[1620.0,0,1930.0],[180.0,0,0],confdata('N','U','T',0,0,0),[0]*6) + +# two program must have the exact same motion primitives +tp_follow.moveJ(jt11,100,'%',-1) # moveJ does not support coordinated motion +tp_follow.moveL(pt12,500,'mmsec',100,'COORD') # add 'COORD' option +tp_follow.moveL(pt13,500,'mmsec',-1,'COORD') + +tp_lead.moveJ(jt21,100,'%',-1) +tp_lead.moveL(pt22,500,'mmsec',100,'COORD') +tp_lead.moveL(pt23,500,'mmsec',-1,'COORD') + +client = FANUCClient() +res = client.execute_motion_program_coord(tp_lead,tp_follow) # lead put in front + +with open("fanuc_log.csv","wb") as f: + f.write(res) + +print(res.decode('utf-8')) \ No newline at end of file diff --git a/examples/multi_example.py b/examples/multi_example.py new file mode 100644 index 0000000..97d86e7 --- /dev/null +++ b/examples/multi_example.py @@ -0,0 +1,34 @@ +from fanuc_motion_program_exec_client import * + +tp1 = TPMotionProgram() +tp2 = TPMotionProgram() + +# robtarget(motion group, uframe, utool, [x,y,z], [w,p,r], confdata, external axis) +# jointtarget(motion group, uframe, utool, [j1,j2,j3,j4,j5,j6], confdata, external axis) + +# use uframe=1 and utool=2 +# these are for motion group 1 (robot 1) +jt11 = jointtarget(1,1,2,[38.3,23.3,-10.7,45.7,-101.9,-48.3],[0]*6) +pt12 = robtarget(1,1,2,[994.0,924.9,1739.5],[163.1,1.5,-1.0],confdata('N','U','T',0,0,0),[0]*6) +pt13 = robtarget(1,1,2,[1620.0,0,1930.0],[180.0,0,0],confdata('N','U','T',0,0,0),[0]*6) + +# these are for motion group 2 (robot 2) +jt21 = jointtarget(2,1,2,[-49.7,4.3,-30.9,-20.9,-35.8,52.1],[0]*6) +pt22 = robtarget(2,1,2,[1383.1,-484.0,940.6],[171.5,-26.8,-9.8],confdata('N','U','T',0,0,0),[0]*6) +pt23 = robtarget(2,1,2,[1166.0,0,1430.0],[180.0,0,0],confdata('N','U','T',0,0,0),[0]*6) + +tp1.moveJ(jt11,100,'%',-1) +tp1.moveL(pt12,500,'mmsec',100) +tp1.moveL(pt13,500,'mmsec',-1) + +tp2.moveJ(jt21,100,'%',-1) +tp2.moveL(pt22,500,'mmsec',100) +tp2.moveL(pt23,500,'mmsec',-1) + +client = FANUCClient() +res = client.execute_motion_program_multi(tp1,tp2) + +with open("fanuc_log.csv","wb") as f: + f.write(res) + +print(res.decode('utf-8')) \ No newline at end of file diff --git a/examples/read_joint_example.py b/examples/read_joint_example.py new file mode 100644 index 0000000..da866f1 --- /dev/null +++ b/examples/read_joint_example.py @@ -0,0 +1,11 @@ +from fanuc_motion_program_exec_client import * + +client = FANUCClient('192.168.0.1') + +read_N=3 +st=time.perf_counter() +res = client.get_joint_angle(read_N=read_N) +print(res) +et=time.perf_counter() +print("Total time:",et-st) +print("Time per Read:",(et-st)/read_N) \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..fc10fc7 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,17 @@ +[build-system] +requires = ["setuptools", "wheel"] + +[project] +name = "fanuc_motion_program_exec" +version = "2.0.0" +description = "Module to execute motion commands on FANUC robots and log results" +license = {text = "Apache-2.0"} +requires-python = ">=3.6" +dependencies = [ + "numpy", + "general_robotics_toolbox" +] +readme = "README.md" + +[project.urls] +homepage = "https://github.com/eric565648/fanuc_motion_program_exec" diff --git a/setup.py b/setup.py deleted file mode 100644 index bc01f94..0000000 --- a/setup.py +++ /dev/null @@ -1,13 +0,0 @@ -from setuptools import setup - -setup( - name='fanuc_motion_program_exec', - version='1.1.2', - description='Module to execute motion commands on FANUC robots and log results', - url='https://github.com/eric565648/fanuc_motion_program_exec', - py_modules=['fanuc_motion_program_exec_client'], - install_requires=[ - 'numpy', - 'general_robotics_toolbox' - ] -) \ No newline at end of file diff --git a/src/fanuc_motion_program_exec_client/__init__.py b/src/fanuc_motion_program_exec_client/__init__.py new file mode 100644 index 0000000..bc3ce1d --- /dev/null +++ b/src/fanuc_motion_program_exec_client/__init__.py @@ -0,0 +1 @@ +from .fanuc_motion_program_exec_client import * \ No newline at end of file diff --git a/fanuc_motion_program_exec_client.py b/src/fanuc_motion_program_exec_client/fanuc_motion_program_exec_client.py similarity index 85% rename from fanuc_motion_program_exec_client.py rename to src/fanuc_motion_program_exec_client/fanuc_motion_program_exec_client.py index e4e39f0..7ba0221 100644 --- a/fanuc_motion_program_exec_client.py +++ b/src/fanuc_motion_program_exec_client/fanuc_motion_program_exec_client.py @@ -850,147 +850,3 @@ def set_ioport(self,io_port,io_num,io_on): cmd_url='http://'+self.robot_ip+'/KCL/SET%20PORT%20'+port_str res=urlopen(cmd_url) -def read_io_test(): - - client = FANUCClient('192.168.0.1') - client.set_ioport('DOUT',10,True) - res = client.read_ioport('DOUT',10) - print("The port is:",res) - client.set_ioport('DOUT',10,False) - res = client.read_ioport('DOUT',10) - print("The port is:",res) - -def read_joint_test(): - - client = FANUCClient('192.168.0.1') - - read_N=3 - st=time.perf_counter() - res = client.get_joint_angle(read_N=read_N) - print(res) - et=time.perf_counter() - print("Total time:",et-st) - print("Time per Read:",(et-st)/read_N) - -def multi_robot_coord(): - - tp_lead = TPMotionProgram() - tp_follow = TPMotionProgram() - - # robtarget(motion group, uframe, utool, [x,y,z], [w,p,r], confdata, external axis) - # jointtarget(motion group, uframe, utool, [j1,j2,j3,j4,j5,j6], confdata, external axis) - - # these are for follower robot, follower must be motion group 1 - jt11 = jointtarget(1,1,2,[-49.7,4.3,-30.9,-20.9,-35.8,52.1],[0]*6) - pt12 = robtarget(1,1,2,[1383.1,-484.0,940.6],[171.5,-26.8,-9.8],confdata('N','U','T',0,0,0),[0]*6) - pt13 = robtarget(1,1,2,[1166.0,0,1430.0],[180.0,0,0],confdata('N','U','T',0,0,0),[0]*6) - - # these are for leader robot, leader must be motion group 2 - jt21 = jointtarget(2,1,2,[38.3,23.3,-10.7,45.7,-101.9,-48.3],[0]*6) - pt22 = robtarget(2,1,2,[994.0,924.9,1739.5],[163.1,1.5,-1.0],confdata('N','U','T',0,0,0),[0]*6) - pt23 = robtarget(2,1,2,[1620.0,0,1930.0],[180.0,0,0],confdata('N','U','T',0,0,0),[0]*6) - - # two program must have the exact same motion primitives - tp_follow.moveJ(jt11,100,'%',-1) # moveJ does not support coordinated motion - tp_follow.moveL(pt12,500,'mmsec',100,'COORD') # add 'COORD' option - tp_follow.moveL(pt13,500,'mmsec',-1,'COORD') - - tp_lead.moveJ(jt21,100,'%',-1) - tp_lead.moveL(pt22,500,'mmsec',100,'COORD') - tp_lead.moveL(pt23,500,'mmsec',-1,'COORD') - - client = FANUCClient() - res = client.execute_motion_program_coord(tp_lead,tp_follow) # lead put in front - - with open("fanuc_log.csv","wb") as f: - f.write(res) - - print(res.decode('utf-8')) - -def multi_robot(): - - tp1 = TPMotionProgram() - tp2 = TPMotionProgram() - - # robtarget(motion group, uframe, utool, [x,y,z], [w,p,r], confdata, external axis) - # jointtarget(motion group, uframe, utool, [j1,j2,j3,j4,j5,j6], confdata, external axis) - - # use uframe=1 and utool=2 - # these are for motion group 1 (robot 1) - jt11 = jointtarget(1,1,2,[38.3,23.3,-10.7,45.7,-101.9,-48.3],[0]*6) - pt12 = robtarget(1,1,2,[994.0,924.9,1739.5],[163.1,1.5,-1.0],confdata('N','U','T',0,0,0),[0]*6) - pt13 = robtarget(1,1,2,[1620.0,0,1930.0],[180.0,0,0],confdata('N','U','T',0,0,0),[0]*6) - - # these are for motion group 2 (robot 2) - jt21 = jointtarget(2,1,2,[-49.7,4.3,-30.9,-20.9,-35.8,52.1],[0]*6) - pt22 = robtarget(2,1,2,[1383.1,-484.0,940.6],[171.5,-26.8,-9.8],confdata('N','U','T',0,0,0),[0]*6) - pt23 = robtarget(2,1,2,[1166.0,0,1430.0],[180.0,0,0],confdata('N','U','T',0,0,0),[0]*6) - - tp1.moveJ(jt11,100,'%',-1) - tp1.moveL(pt12,500,'mmsec',100) - tp1.moveL(pt13,500,'mmsec',-1) - - tp2.moveJ(jt21,100,'%',-1) - tp2.moveL(pt22,500,'mmsec',100) - tp2.moveL(pt23,500,'mmsec',-1) - - client = FANUCClient() - res = client.execute_motion_program_multi(tp1,tp2) - - with open("fanuc_log.csv","wb") as f: - f.write(res) - - print(res.decode('utf-8')) - -def single_robot(): - - tp = TPMotionProgram() - - # robtarget(motion group, uframe, utool, [x,y,z], [w,p,r], confdata, external axis) - pt1 = robtarget(1,1,2,[1850,200,290],[-180,0,0],confdata('N','U','T',0,0,0),[0]*6) - pt2 = robtarget(1,1,2,[1850,200,589],[-180,0,0],confdata('N','U','T',0,0,0),[0]*6) - # jointtarget(motion group, uframe, utool, [j1,j2,j3,j4,j5,j6], confdata, external axis) - jt1 = jointtarget(1,1,2,[0,20,-10,0,-20,10],[0]*6) - pt3 = robtarget(1,1,2,[1850,250,400],[-180,0,0],confdata('N','U','T',0,0,0),[0]*6) - - tp.moveL(pt1,50,'mmsec',100) - tp.moveL(pt2,50,'mmsec',-1) - tp.moveJ(jt1,100,'%',-1) - tp.moveL(pt2,50,'mmsec',100) - tp.moveC(pt3,pt1,50,'mmsec',-1) - tp.moveL(pt2,50,'mmsec',-1) - - print(tp.get_tp()) - - client = FANUCClient() - res = client.execute_motion_program(tp) - - with open("fanuc_log.csv","wb") as f: - f.write(res) - - print(res.decode('utf-8')) - - tp = TPMotionProgram() - tp.moveL(pt1,50,'mmsec',100) - tp.moveL(pt2,50,'mmsec',-1) - client = FANUCClient() - res = client.execute_motion_program(tp) - -def main(): - - # single robot - # single_robot() - - # multi robot - # multi_robot() - # multi robot with coordinated motion - # multi_robot_coord() - - # read joint - read_joint_test() - - # read IO - # read_io_test() - -if __name__ == "__main__": - main() From 6c02610821b6e84a158815b7612d32ab35ab0008 Mon Sep 17 00:00:00 2001 From: John Wason Date: Thu, 21 Dec 2023 15:39:15 -0500 Subject: [PATCH 2/6] Rename top level package to fanuc_motion_program_exec --- examples/example.py | 2 +- examples/io_example.py | 2 +- examples/multi_coord_example.py | 2 +- examples/multi_example.py | 2 +- examples/read_joint_example.py | 2 +- .../__init__.py | 0 .../fanuc_motion_program_exec_client.py | 0 tools/roboguide_base_adjust.py | 2 +- 8 files changed, 6 insertions(+), 6 deletions(-) rename src/{fanuc_motion_program_exec_client => fanuc_motion_program_exec}/__init__.py (100%) rename src/{fanuc_motion_program_exec_client => fanuc_motion_program_exec}/fanuc_motion_program_exec_client.py (100%) diff --git a/examples/example.py b/examples/example.py index a90b166..d3567fb 100644 --- a/examples/example.py +++ b/examples/example.py @@ -1,4 +1,4 @@ -from fanuc_motion_program_exec_client import * +from fanuc_motion_program_exec import * tp = TPMotionProgram() diff --git a/examples/io_example.py b/examples/io_example.py index cfdbe8f..3027fb4 100644 --- a/examples/io_example.py +++ b/examples/io_example.py @@ -1,4 +1,4 @@ -from fanuc_motion_program_exec_client import * +from fanuc_motion_program_exec import * client = FANUCClient('192.168.0.1') client.set_ioport('DOUT',10,True) diff --git a/examples/multi_coord_example.py b/examples/multi_coord_example.py index 689f5ba..29852ab 100644 --- a/examples/multi_coord_example.py +++ b/examples/multi_coord_example.py @@ -1,4 +1,4 @@ -from fanuc_motion_program_exec_client import * +from fanuc_motion_program_exec import * tp_lead = TPMotionProgram() tp_follow = TPMotionProgram() diff --git a/examples/multi_example.py b/examples/multi_example.py index 97d86e7..bb0723e 100644 --- a/examples/multi_example.py +++ b/examples/multi_example.py @@ -1,4 +1,4 @@ -from fanuc_motion_program_exec_client import * +from fanuc_motion_program_exec import * tp1 = TPMotionProgram() tp2 = TPMotionProgram() diff --git a/examples/read_joint_example.py b/examples/read_joint_example.py index da866f1..2498389 100644 --- a/examples/read_joint_example.py +++ b/examples/read_joint_example.py @@ -1,4 +1,4 @@ -from fanuc_motion_program_exec_client import * +from fanuc_motion_program_exec import * client = FANUCClient('192.168.0.1') diff --git a/src/fanuc_motion_program_exec_client/__init__.py b/src/fanuc_motion_program_exec/__init__.py similarity index 100% rename from src/fanuc_motion_program_exec_client/__init__.py rename to src/fanuc_motion_program_exec/__init__.py diff --git a/src/fanuc_motion_program_exec_client/fanuc_motion_program_exec_client.py b/src/fanuc_motion_program_exec/fanuc_motion_program_exec_client.py similarity index 100% rename from src/fanuc_motion_program_exec_client/fanuc_motion_program_exec_client.py rename to src/fanuc_motion_program_exec/fanuc_motion_program_exec_client.py diff --git a/tools/roboguide_base_adjust.py b/tools/roboguide_base_adjust.py index 63e8a3e..fa7353b 100644 --- a/tools/roboguide_base_adjust.py +++ b/tools/roboguide_base_adjust.py @@ -8,7 +8,7 @@ import sys,os,time,traceback,platform,subprocess from general_robotics_toolbox import * -from fanuc_motion_program_exec_client import * +from fanuc_motion_program_exec import * class BaseGUI(QDialog): def __init__(self, parent=None): From d3406b9a94cbd90772d7cc5df180d506b8f6d252 Mon Sep 17 00:00:00 2001 From: John Wason Date: Thu, 21 Dec 2023 21:19:32 -0500 Subject: [PATCH 3/6] Add robotraconteur service --- ...id_motion_program_robot_default_config.yml | 162 +++++++++ ...ti_motion_program_robot_default_config.yml | 288 ++++++++++++++++ examples/robotraconteur/example_client.py | 165 +++++++++ .../robotraconteur/multi_example_client.py | 173 ++++++++++ pyproject.toml | 13 + .../robotraconteur/__init__.py | 0 .../robotraconteur/__main__.py | 4 + .../robotraconteur/_motion_program_conv.py | 318 ++++++++++++++++++ ...xperimental.robotics.motion_program.robdef | 196 +++++++++++ ...anuc_motion_program_exec_robotraconteur.py | 187 ++++++++++ 10 files changed, 1506 insertions(+) create mode 100644 config/FANUC_lrmate200id_motion_program_robot_default_config.yml create mode 100644 config/FANUC_multi_motion_program_robot_default_config.yml create mode 100644 examples/robotraconteur/example_client.py create mode 100644 examples/robotraconteur/multi_example_client.py create mode 100644 src/fanuc_motion_program_exec/robotraconteur/__init__.py create mode 100644 src/fanuc_motion_program_exec/robotraconteur/__main__.py create mode 100644 src/fanuc_motion_program_exec/robotraconteur/_motion_program_conv.py create mode 100644 src/fanuc_motion_program_exec/robotraconteur/experimental.robotics.motion_program.robdef create mode 100644 src/fanuc_motion_program_exec/robotraconteur/fanuc_motion_program_exec_robotraconteur.py diff --git a/config/FANUC_lrmate200id_motion_program_robot_default_config.yml b/config/FANUC_lrmate200id_motion_program_robot_default_config.yml new file mode 100644 index 0000000..7772223 --- /dev/null +++ b/config/FANUC_lrmate200id_motion_program_robot_default_config.yml @@ -0,0 +1,162 @@ +robot_info: + device_info: + device: + name: fanuc_lrmate200id + manufacturer: + name: FANUC + uuid: d80db3c2-3de8-41e3-97f9-62765a7063b8 + model: + name: lrmate200id + uuid: 1d3f5111-01fa-4ecf-a11e-76810d5e1dde + user_description: FANUC lrmate200id Robot + serial_number: 123456789 + device_classes: + - class_identifier: + name: robot + uuid: 39b513e7-21b9-4b49-8654-7537473030eb + subclasses: + - serial + - serial_six_axis + - cobot + implemented_types: + - com.robotraconteur.robotics.robot.Robot + robot_type: serial + robot_capabilities: + - jog_command + - trajectory_command + - position_command + - velocity_command + chains: + - kin_chain_identifier: robot_arm + H: + - x: 0.0 + y: 0.0 + z: 1.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 1.0 + y: 0.0 + z: 0.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 1.0 + y: 0.0 + z: 0.0 + P: + - x: 0.0 + y: 0.0 + z: 330 + - x: 50 + y: 0.0 + z: 0.0 + - x: 0. + y: 0.0 + z: 330 + - x: 0.0 + y: 0.0 + z: 35 + - x: 335 + y: 0.0 + z: 0.0 + - x: 80 + y: 0.0 + z: 0.0 + - x: 0.0 + y: 0.0 + z: 0.0 + flange_identifier: tool0 + flange_pose: + orientation: + w: 0.7071067811882787 + x: 0.0 + y: 0.7071067811882787 + z: 0.0 + position: + x: 0 + y: 0.0 + z: 0 + joint_numbers: + - 0 + - 1 + - 2 + - 3 + - 4 + - 5 + joint_info: + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_1 + joint_limits: + effort: 1000 + lower: -2.96705972 + upper: 2.96705972 + velocity: 7.85398163 + acceleration: 10 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_2 + joint_limits: + effort: 1000 + lower: -1.74532925 + upper: 2.53072742 + velocity: 6.63225116 + acceleration: 15 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_3 + joint_limits: + effort: 1000 + lower: -3.57792497 + upper: 1.22173048 + velocity: 9.07571211 + acceleration: 15 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_4 + joint_limits: + effort: 1000 + lower: -3.31612557 + upper: 3.31612557 + velocity: 9.59931089 + acceleration: 20 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_5 + joint_limits: + effort: 1000 + lower: -2.18166156 + upper: 2.18166156 + velocity: 9.51204442 + acceleration: 20 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_6 + joint_limits: + effort: 1000 + lower: -6.28318530718 + upper: 6.28318530718 + velocity: 17.45329252 + acceleration: 20 + joint_type: revolute + passive: false +supported_commands: + - MoveAbsJ + - MoveJ + - MoveL + - MoveC + - WaitTime diff --git a/config/FANUC_multi_motion_program_robot_default_config.yml b/config/FANUC_multi_motion_program_robot_default_config.yml new file mode 100644 index 0000000..295a30e --- /dev/null +++ b/config/FANUC_multi_motion_program_robot_default_config.yml @@ -0,0 +1,288 @@ +robot_info: + device_info: + device: + name: fanuc_robot_multi + manufacturer: + name: FANUC + uuid: d80db3c2-3de8-41e3-97f9-62765a7063b8 + model: + name: multi_robot + uuid: 52d11a44-9f73-4e4a-be0e-3bee4873961c + user_description: FANUC lrmate200id Robot + serial_number: 123456789 + device_classes: + - class_identifier: + name: robot + uuid: 39b513e7-21b9-4b49-8654-7537473030eb + subclasses: + - serial + - serial_six_axis + - cobot + implemented_types: + - com.robotraconteur.robotics.robot.Robot + robot_type: serial + robot_capabilities: + - jog_command + - trajectory_command + - position_command + - velocity_command + chains: + - kin_chain_identifier: robot_arm1 + H: + - x: 0.0 + y: 0.0 + z: 1.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 1.0 + y: 0.0 + z: 0.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 1.0 + y: 0.0 + z: 0.0 + P: + - x: 0.0 + y: 0.0 + z: 330 + - x: 50 + y: 0.0 + z: 0.0 + - x: 0. + y: 0.0 + z: 330 + - x: 0.0 + y: 0.0 + z: 35 + - x: 335 + y: 0.0 + z: 0.0 + - x: 80 + y: 0.0 + z: 0.0 + - x: 0.0 + y: 0.0 + z: 0.0 + flange_identifier: robot1_tool0 + flange_pose: + orientation: + w: 0.7071067811882787 + x: 0.0 + y: 0.7071067811882787 + z: 0.0 + position: + x: 0 + y: 0.0 + z: 0 + joint_numbers: + - 0 + - 1 + - 2 + - 3 + - 4 + - 5 + - kin_chain_identifier: robot_arm2 + H: + - x: 0.0 + y: 0.0 + z: 1.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 1.0 + y: 0.0 + z: 0.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 1.0 + y: 0.0 + z: 0.0 + P: + - x: 0.0 + y: 0.0 + z: 450 + - x: 150 + y: 0.0 + z: 0.0 + - x: 0. + y: 0.0 + z: 600 + - x: 0.0 + y: 0.0 + z: 200 + - x: 640 + y: 0.0 + z: 0.0 + - x: 100 + y: 0.0 + z: 0.0 + - x: 0.0 + y: 0.0 + z: 0.0 + flange_identifier: robot2_tool0 + flange_pose: + orientation: + w: 0.7071067811882787 + x: 0.0 + y: 0.7071067811848163 + z: 0.0 + position: + x: 0 + y: 0.0 + z: 0 + joint_numbers: + - 6 + - 7 + - 8 + - 9 + - 10 + - 11 + joint_info: + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_1 + joint_limits: + effort: 1000 + lower: -2.96705972 + upper: 2.96705972 + velocity: 7.85398163 + acceleration: 10 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_2 + joint_limits: + effort: 1000 + lower: -1.74532925 + upper: 2.53072742 + velocity: 6.63225116 + acceleration: 15 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_3 + joint_limits: + effort: 1000 + lower: -3.57792497 + upper: 1.22173048 + velocity: 9.07571211 + acceleration: 15 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_4 + joint_limits: + effort: 1000 + lower: -3.31612557 + upper: 3.31612557 + velocity: 9.59931089 + acceleration: 20 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_5 + joint_limits: + effort: 1000 + lower: -2.18166156 + upper: 2.18166156 + velocity: 9.51204442 + acceleration: 20 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_6 + joint_limits: + effort: 1000 + lower: -6.28318530718 + upper: 6.28318530718 + velocity: 17.45329252 + acceleration: 20 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_1 + joint_limits: + effort: 1000 + lower: -2.96705972 + upper: 2.96705972 + velocity: 3.66519142 + acceleration: 10 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_2 + joint_limits: + effort: 1000 + lower: -1.57079632 + upper: 2.79252680 + velocity: 3.31612557 + acceleration: 15 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_3 + joint_limits: + effort: 1000 + lower: -3.14159265 + upper: 1.55334303 + velocity: 3.66519142 + acceleration: 15 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_4 + joint_limits: + effort: 1000 + lower: -3.31612557 + upper: 3.31612557 + velocity: 6.98131700 + acceleration: 20 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_5 + joint_limits: + effort: 1000 + lower: -2.44346095 + upper: 2.44346095 + velocity: 6.98131700 + acceleration: 20 + joint_type: revolute + passive: false + - default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_6 + joint_limits: + effort: 1000 + lower: -6.28318530718 + upper: 6.28318530718 + velocity: 10.47197551 + acceleration: 20 + joint_type: revolute + passive: false +supported_commands: + - MoveAbsJ + - MoveJ + - MoveL + - MoveC + - WaitTime diff --git a/examples/robotraconteur/example_client.py b/examples/robotraconteur/example_client.py new file mode 100644 index 0000000..5f6bf8c --- /dev/null +++ b/examples/robotraconteur/example_client.py @@ -0,0 +1,165 @@ +from RobotRaconteur.Client import * + +import numpy as np +import general_robotics_toolbox as rox +import math + +c = RRN.ConnectService("rr+tcp://localhost:59845?service=mp_robot") + +robot_pose_type = RRN.GetStructureType("experimental.robotics.motion_program.RobotPose",c) +moveabsj_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveAbsJCommand",c) +movej_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveJCommand",c) +movel_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveLCommand",c) +movec_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveCCommand",c) +motionprogram_type = RRN.GetStructureType("experimental.robotics.motion_program.MotionProgram",c) +toolinfo_type = RRN.GetStructureType("com.robotraconteur.robotics.tool.ToolInfo",c) +transform_dt = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Transform",c) +spatialinertia_dt = RRN.GetNamedArrayDType("com.robotraconteur.geometry.SpatialInertia",c) + +def wpr2R(wpr): + return rox.rot([0,0,1],math.radians(wpr[2]))@rox.rot([0,1,0],math.radians(wpr[1]))@rox.rot([1,0,0],math.radians(wpr[0])) + +def robot_pose(p,wpr,conf): + R = wpr2R(wpr) + q = rox.R2q(R) + + ret = robot_pose_type() + ret.tcp_pose[0]["orientation"]["w"] = q[0] + ret.tcp_pose[0]["orientation"]["x"] = q[1] + ret.tcp_pose[0]["orientation"]["y"] = q[2] + ret.tcp_pose[0]["orientation"]["z"] = q[3] + ret.tcp_pose[0]["position"]["x"] = p[0]*1e-3 + ret.tcp_pose[0]["position"]["y"] = p[1]*1e-3 + ret.tcp_pose[0]["position"]["z"] = p[2]*1e-3 + + rr_confdata = [None]*6 + for i in range(3): + rr_confdata[i] = RR.VarValue(conf[i],"string") + for i in range(3,6): + rr_confdata[i] = RR.VarValue(int(conf[i]),"int32") + + confdata = RR.VarValue(rr_confdata, "varvalue{list}") + + return ret, confdata + +jt1 = np.deg2rad(np.array([0,20,-10,0,-20,10],dtype=np.float64)) +pt1 = robot_pose([1850,200,290],[-180,0,0],('N','U','T',0,0,0)) +pt2 = robot_pose([1850,200,589],[-180,0,0],('N','U','T',0,0,0)) +pt3 = robot_pose([1850,250,400],[-180,0,0],('N','U','T',0,0,0)) + +setup_cmds = [] +mp_cmds = [] + +def moveabsj(j,velocity,blend_radius,fine_point, velocity_units = None): + cmd = moveabsj_type() + cmd.joint_position = j + cmd.tcp_velocity = velocity + cmd.blend_radius = blend_radius + cmd.fine_point = fine_point + if velocity_units is not None: + cmd.extended = { + "tcp_velocity_units": RR.VarValue(velocity_units, "string") + } + return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveAbsJCommand") + +def movel(robot_pose,velocity,blend_radius,fine_point, j = None, velocity_units = None): + cmd = movel_type() + cmd.extended = {} + if robot_pose is not None: + cmd.tcp_pose = robot_pose[0] + cmd.extended["confdata"] = robot_pose[1] + cmd.tcp_velocity = velocity + cmd.blend_radius = blend_radius + cmd.fine_point = fine_point + + if j is not None: + # Fanuc allows for passing joint positions to moveJ. Use extended field + cmd.extended["joint_position"] = RR.VarValue(j, "double[]") + if velocity_units is not None: + # Set the velocity units + cmd.extended["tcp_velocity_units"] = RR.VarValue(velocity_units, "string") + + + return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveLCommand") + +def movej(robot_pose,velocity,blend_radius,fine_point, j = None, velocity_units = None): + cmd = movej_type() + cmd.extended = {} + if robot_pose is not None: + cmd.tcp_pose = robot_pose[0] + cmd.extended["confdata"] = robot_pose[1] + cmd.tcp_velocity = velocity + cmd.blend_radius = blend_radius + cmd.fine_point = fine_point + + if j is not None: + # Fanuc allows for passing joint positions to moveL. Use extended field + cmd.extended["joint_position"] = RR.VarValue(j, "double[]") + if velocity_units is not None: + # Set the velocity units + cmd.extended["tcp_velocity_units"] = RR.VarValue(velocity_units, "string") + return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveJCommand") + +def movec(robot_pose,robot_via_pose,velocity,blend_radius,fine_point, j = None, j_via = None, velocity_units = None): + cmd = movec_type() + cmd.extended = {} + if robot_pose is not None: + cmd.tcp_pose = robot_pose[0] + cmd.extended["confdata"] = robot_pose[1] + cmd.tcp_via_pose = robot_via_pose[0] + cmd.extended["confdata_via"] = robot_via_pose[1] + cmd.tcp_velocity = velocity + cmd.blend_radius = blend_radius + cmd.fine_point = fine_point + + if j is not None: + # Fanuc allows for passing joint positions to moveC. Use extended field + cmd.extended["joint_position"] = RR.VarValue(j, "double[]") + cmd.extended["joint_via_position"] = RR.VarValue(j_via, "double[]") + if velocity_units is not None: + # Set the velocity units + cmd.extended["tcp_velocity_units"] = RR.VarValue(velocity_units, "string") + return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveCCommand") + +mp_cmds.append(movel(pt1,0.050,0.100,False)) +mp_cmds.append(movel(pt2,0.050,0.100,True)) +mp_cmds.append(moveabsj(jt1,100,0.100,False,velocity_units = "%")) +mp_cmds.append(movel(pt2,0.050,0.100,False)) +mp_cmds.append(movec(pt3,pt1,0.050,0.100,True)) +mp_cmds.append(movel(pt2,0.050,0.100,True)) + +mp = motionprogram_type() + +mp.motion_program_commands = mp_cmds +mp.motion_setup_commands = setup_cmds + +# If multimove is available, optionally use a different robot other than first robot. +# See multimove example for demonstration of synchronizing multiple robots +# Groups are zero indexed +# mp.extended = { +# "groups": RR.VarValue(1, "int32") +# } + +mp_gen = c.execute_motion_program_record(mp,False) + +res = None +status = None + +while True: + res, status1 = mp_gen.TryNext() + if not res: + break + status = status1 + print(status) + +print(f"recording_handle: {status.recording_handle}") + +robot_recording = c.read_recording(status.recording_handle).NextAll()[0] + +print(robot_recording.time) +print(robot_recording.command_number) +print(robot_recording.joints) + +c.clear_recordings() + +print("Done!") diff --git a/examples/robotraconteur/multi_example_client.py b/examples/robotraconteur/multi_example_client.py new file mode 100644 index 0000000..922ae5b --- /dev/null +++ b/examples/robotraconteur/multi_example_client.py @@ -0,0 +1,173 @@ +from RobotRaconteur.Client import * + +import numpy as np +import general_robotics_toolbox as rox +import math + +c = RRN.ConnectService("rr+tcp://localhost:59845?service=mp_robot") + +robot_pose_type = RRN.GetStructureType("experimental.robotics.motion_program.RobotPose",c) +moveabsj_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveAbsJCommand",c) +movej_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveJCommand",c) +movel_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveLCommand",c) +movec_type = RRN.GetStructureType("experimental.robotics.motion_program.MoveCCommand",c) +motionprogram_type = RRN.GetStructureType("experimental.robotics.motion_program.MotionProgram",c) +toolinfo_type = RRN.GetStructureType("com.robotraconteur.robotics.tool.ToolInfo",c) +transform_dt = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Transform",c) +spatialinertia_dt = RRN.GetNamedArrayDType("com.robotraconteur.geometry.SpatialInertia",c) + +def wpr2R(wpr): + return rox.rot([0,0,1],math.radians(wpr[2]))@rox.rot([0,1,0],math.radians(wpr[1]))@rox.rot([1,0,0],math.radians(wpr[0])) + +def robot_pose(p,wpr,conf): + R = wpr2R(wpr) + q = rox.R2q(R) + + ret = robot_pose_type() + ret.tcp_pose[0]["orientation"]["w"] = q[0] + ret.tcp_pose[0]["orientation"]["x"] = q[1] + ret.tcp_pose[0]["orientation"]["y"] = q[2] + ret.tcp_pose[0]["orientation"]["z"] = q[3] + ret.tcp_pose[0]["position"]["x"] = p[0]*1e-3 + ret.tcp_pose[0]["position"]["y"] = p[1]*1e-3 + ret.tcp_pose[0]["position"]["z"] = p[2]*1e-3 + + rr_confdata = [None]*6 + for i in range(3): + rr_confdata[i] = RR.VarValue(conf[i],"string") + for i in range(3,6): + rr_confdata[i] = RR.VarValue(int(conf[i]),"int32") + + confdata = RR.VarValue(rr_confdata, "varvalue{list}") + + return ret, confdata + +def moveabsj(j,velocity,blend_radius,fine_point, velocity_units = None): + cmd = moveabsj_type() + cmd.joint_position = j + cmd.tcp_velocity = velocity + cmd.blend_radius = blend_radius + cmd.fine_point = fine_point + if velocity_units is not None: + cmd.extended = { + "tcp_velocity_units": RR.VarValue(velocity_units, "string") + } + return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveAbsJCommand") + +def movel(robot_pose,velocity,blend_radius,fine_point, j = None, velocity_units = None): + cmd = movel_type() + cmd.extended = {} + if robot_pose is not None: + cmd.tcp_pose = robot_pose[0] + cmd.extended["confdata"] = robot_pose[1] + cmd.tcp_velocity = velocity + cmd.blend_radius = blend_radius + cmd.fine_point = fine_point + + if j is not None: + # Fanuc allows for passing joint positions to moveJ. Use extended field + cmd.extended["joint_position"] = RR.VarValue(j, "double[]") + if velocity_units is not None: + # Set the velocity units + cmd.extended["tcp_velocity_units"] = RR.VarValue(velocity_units, "string") + + + return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveLCommand") + +def movej(robot_pose,velocity,blend_radius,fine_point, j = None, velocity_units = None): + cmd = movej_type() + cmd.extended = {} + if robot_pose is not None: + cmd.tcp_pose = robot_pose[0] + cmd.extended["confdata"] = robot_pose[1] + cmd.tcp_velocity = velocity + cmd.blend_radius = blend_radius + cmd.fine_point = fine_point + + if j is not None: + # Fanuc allows for passing joint positions to moveL. Use extended field + cmd.extended["joint_position"] = RR.VarValue(j, "double[]") + if velocity_units is not None: + # Set the velocity units + cmd.extended["tcp_velocity_units"] = RR.VarValue(velocity_units, "string") + return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveJCommand") + +def movec(robot_pose,robot_via_pose,velocity,blend_radius,fine_point, j = None, j_via = None, velocity_units = None): + cmd = movec_type() + cmd.extended = {} + if robot_pose is not None: + cmd.tcp_pose = robot_pose[0] + cmd.extended["confdata"] = robot_pose[1] + cmd.tcp_via_pose = robot_via_pose[0] + cmd.extended["confdata_via"] = robot_via_pose[1] + cmd.tcp_velocity = velocity + cmd.blend_radius = blend_radius + cmd.fine_point = fine_point + + if j is not None: + # Fanuc allows for passing joint positions to moveC. Use extended field + cmd.extended["joint_position"] = RR.VarValue(j, "double[]") + cmd.extended["joint_via_position"] = RR.VarValue(j_via, "double[]") + if velocity_units is not None: + # Set the velocity units + cmd.extended["tcp_velocity_units"] = RR.VarValue(velocity_units, "string") + return RR.VarValue(cmd,"experimental.robotics.motion_program.MoveCCommand") + +jt11 = np.deg2rad(np.array([38.3,23.3,-10.7,45.7,-101.9,-48.3],dtype=np.float64)) +pt12 = robot_pose([994.0,924.9,1739.5],[163.1,1.5,-1.0],('N','U','T',0,0,0)) +pt12 = robot_pose([1620.0,0,1930.0],[180.0,0,0],('N','U','T',0,0,0)) + +jt21 = np.deg2rad(np.array([-49.7,4.3,-30.9,-20.9,-35.8,52.1],dtype=np.float64)) +pt22 = robot_pose([1383.1,-484.0,940.6],[171.5,-26.8,-9.8],('N','U','T',0,0,0)) +pt22 = robot_pose([1166.0,0,1430.0],[180.0,0,0],('N','U','T',0,0,0)) + +setup_cmds = [] +mp_cmds = [] + +setup_cmds2 = [] +mp_cmds2 = [] + +mp_cmds.append(moveabsj(jt11,100,0.100,True,velocity_units = "%")) +mp_cmds.append(movel(pt12,0.500,0.100,False)) +mp_cmds.append(movel(pt12,0.500,0.100,True)) + +mp_cmds2.append(moveabsj(jt21,100,0.100,True,velocity_units = "%")) +mp_cmds2.append(movel(pt22,0.500,0.100,False)) +mp_cmds2.append(movel(pt22,0.500,0.100,True)) + +mp = motionprogram_type() +mp.motion_program_commands = mp_cmds +mp.motion_setup_commands = setup_cmds + +mp2 = motionprogram_type() +mp2.motion_program_commands = mp_cmds2 +mp2.motion_setup_commands = setup_cmds2 + +mp.extended = { + "multi_motion_programs": RR.VarValue([mp2], "experimental.robotics.motion_program.MotionProgram{list}"), + "groups": RR.VarValue([0,1], "int32[]") +} + +mp_gen = c.execute_motion_program_record(mp,False) + +res = None +status = None + +while True: + res, status1 = mp_gen.TryNext() + if not res: + break + status = status1 + print(status) + +print(f"recording_handle: {status.recording_handle}") + +robot_recording = c.read_recording(status.recording_handle).NextAll()[0] + +print(robot_recording.time) +print(robot_recording.command_number) +print(robot_recording.joints) + +c.clear_recordings() + +print("Done!") \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml index fc10fc7..0d33268 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -15,3 +15,16 @@ readme = "README.md" [project.urls] homepage = "https://github.com/eric565648/fanuc_motion_program_exec" + +[project.optional-dependencies] +test = [ + "pytest", +] +robotraconteur = [ + "robotraconteur", + "robotraconteurcompanion", + "drekar-launch-process" +] + +[tool.setuptools.package-data] +"fanuc_motion_program_exec.robotraconteur" = ["*.robdef"] \ No newline at end of file diff --git a/src/fanuc_motion_program_exec/robotraconteur/__init__.py b/src/fanuc_motion_program_exec/robotraconteur/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/fanuc_motion_program_exec/robotraconteur/__main__.py b/src/fanuc_motion_program_exec/robotraconteur/__main__.py new file mode 100644 index 0000000..bc88829 --- /dev/null +++ b/src/fanuc_motion_program_exec/robotraconteur/__main__.py @@ -0,0 +1,4 @@ +from .fanuc_motion_program_exec_robotraconteur import main + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/fanuc_motion_program_exec/robotraconteur/_motion_program_conv.py b/src/fanuc_motion_program_exec/robotraconteur/_motion_program_conv.py new file mode 100644 index 0000000..a998ec6 --- /dev/null +++ b/src/fanuc_motion_program_exec/robotraconteur/_motion_program_conv.py @@ -0,0 +1,318 @@ +from contextlib import suppress +from RobotRaconteur.RobotRaconteurPythonUtil import NamedArrayToArray +import numpy as np +from .. import fanuc_motion_program_exec_client as fanuc_exec +import RobotRaconteur as RR +import general_robotics_toolbox as rox +import re + +def rr_pose_to_fanuc(rr_pose): + a = NamedArrayToArray(rr_pose) + wpr = fanuc_exec.R2wpr(rox.q2R(a[0][0:4])) + return a[0][4:7]*1000.0, wpr + +def rr_joints_to_fanuc(rr_joints, rr_joint_units, motion_group = 1, uframe = 1, utool = 1): + #TODO: joint units + #TODO: use "extended" for external axes + return fanuc_exec.jointtarget(motion_group, uframe, utool, np.rad2deg(rr_joints), [6e5]*6) + +def rr_conf_to_fanuc(rr_joint_seed, rr_confdata_extra): + # TODO: use joint_position_seed to compute confdata + assert rr_confdata_extra is not None, "confdata_extra must be specified in extended field" + assert rr_confdata_extra.datatype == "varvalue{list}" + assert len(rr_confdata_extra.data) == 6 + confdata_a = [None]*6 + for i in range(3): + assert rr_confdata_extra.data[i].datatype == "string" + confdata_a[i] = rr_confdata_extra.data[i].data + for i in range(3,6): + if rr_confdata_extra.data[i].datatype == "string": + confdata_a[i] = int(rr_confdata_extra.data[i].data) + elif rr_confdata_extra.data[i].datatype in ("int32[]", "double[]"): + confdata_a[i] = int(rr_confdata_extra.data[i].data[0]) + else: + assert False, "Invalid confdata_extra type" + + return fanuc_exec.confdata(*confdata_a) + + +def rr_robot_pose_to_fanuc(rr_robot_pose, cfx_robot, confdata_extra = None, motion_group = 1, uframe = 1, utool = 1): + #TODO: joint units + #TODO: use "extended" for external axes + xyz, wpr = rr_pose_to_fanuc(rr_robot_pose.tcp_pose) + + cd = rr_conf_to_fanuc(rr_robot_pose.joint_position_seed, confdata_extra) + return fanuc_exec.robtarget(motion_group, uframe, utool, xyz,wpr, cd, [6e5]*6) + +def rr_target_to_fanuc(rr_robot_pose, cfx_robot, joint_position, joint_position_units, confdata_extra = None, motion_group = 1, uframe = 1, utool = 1): + if rr_robot_pose is None: + # Assume joint target + return rr_joints_to_fanuc(joint_position, joint_position_units, motion_group, uframe, utool) + else: + return rr_robot_pose_to_fanuc(rr_robot_pose, cfx_robot, confdata_extra, motion_group, uframe, utool) + +def rr_zone_to_fanuc(fine_point, blend_radius): + if fine_point: + return -1 + else: + return blend_radius*1000.0 + +def rr_speed_to_fanuc(rr_velocity, rr_velocity_units): + if rr_velocity_units is None: + return [rr_velocity*1000.0, "mmsec"] + rr_velocity_units = rr_velocity_units.data.lower() + if rr_velocity_units == "mmsec": + return [rr_velocity, "mmsec"] + elif rr_velocity_units == "msec": + return [rr_velocity*1000.0, "mmsec"] + elif rr_velocity_units == "%": + return [rr_velocity, "%"] + assert False, "Invalid velocity units" + +cmd_get_arg_sentinel = object() +cmd_arg_no_default = object() + +def cmd_get_arg(cmd, arg_name, default_value = cmd_arg_no_default): + if isinstance(cmd, RR.VarValue): + cmd = cmd.data + + val = getattr(cmd, arg_name, cmd_get_arg_sentinel) + if val is cmd_get_arg_sentinel: + freeform_args = getattr(cmd, "command_args", cmd_get_arg_sentinel) + assert freeform_args is not cmd_get_arg_sentinel, f"Invalid command type, missing argument {arg_name}" + + val = freeform_args.get(arg_name, cmd_get_arg_sentinel) + if val is cmd_get_arg_sentinel and default_value is not cmd_arg_no_default: + return default_value + assert val is not cmd_get_arg_sentinel, f"Invalid command type, missing argument {arg_name}" + + if isinstance(val, RR.VarValue): + val = val.data + + return val + +def cmd_get_extended(cmd, extended_name, default_value = None): + if isinstance(cmd, RR.VarValue): + cmd = cmd.data + if not cmd.extended: + return default_value + val = cmd.extended.get(extended_name, default_value) + return val + +def get_common_args(kwargs): + motion_group = kwargs.get("motion_group",1) + uframe = kwargs.get("uframe",1) + utool = kwargs.get("utool",1) + return [motion_group, uframe, utool] + +class MoveAbsJCommandConv: + rr_types = ["experimental.robotics.motion_program.MoveAbsJCommand"] + freeform_names = ["MoveAbsJ", "MoveAbsJCommand", "experimental.robotics.motion_program.MoveAbsJCommand"] + + def apply_rr_command(self, cmd, mp, **kwargs): + zd = rr_zone_to_fanuc(cmd_get_arg(cmd,"fine_point"),cmd_get_arg(cmd,"blend_radius")) + sd = rr_speed_to_fanuc(cmd_get_arg(cmd, "tcp_velocity"), cmd_get_extended(cmd, "tcp_velocity_units", None)) + jt = rr_joints_to_fanuc(cmd_get_arg(cmd,"joint_position", None), cmd_get_arg(cmd,"joint_units", []), *get_common_args(kwargs)) + mp.moveJ(*([jt] + sd + [zd])) + +class MoveJCommandConv: + rr_types = ["experimental.robotics.motion_program.MoveJCommand"] + freeform_names = ["MoveJ","MoveJCommand","experimental.robotics.motion_program.MoveJCommand"] + + def apply_rr_command(self, cmd, mp, cfx_robot, **kwargs): + zd = rr_zone_to_fanuc(cmd_get_arg(cmd,"fine_point"),cmd_get_arg(cmd,"blend_radius")) + sd = rr_speed_to_fanuc(cmd_get_arg(cmd,"tcp_velocity"), cmd_get_extended(cmd,"tcp_velocity_units", None)) + rt = rr_target_to_fanuc(cmd_get_arg(cmd,"tcp_pose"), cfx_robot, + cmd_get_extended(cmd,"joint_position", None), cmd_get_extended(cmd,"joint_units", []), cmd_get_extended(cmd,"confdata", None), + *get_common_args(kwargs)) + mp.moveJ(*([rt] + sd + [zd])) + +class MoveLCommandConv: + rr_types = ["experimental.robotics.motion_program.MoveLCommand"] + freeform_names = ["MoveL","MoveLCommand","experimental.robotics.motion_program.MoveLCommand"] + + def apply_rr_command(self, cmd, mp, cfx_robot, **kwargs): + zd = rr_zone_to_fanuc(cmd_get_arg(cmd,"fine_point"),cmd_get_arg(cmd,"blend_radius")) + sd = rr_speed_to_fanuc(cmd_get_arg(cmd,"tcp_velocity"), cmd_get_extended(cmd,"tcp_velocity_units", None)) + rt = rr_target_to_fanuc(cmd_get_arg(cmd,"tcp_pose"), cfx_robot, + cmd_get_extended(cmd,"joint_position", None), cmd_get_extended(cmd,"joint_units", []), cmd_get_extended(cmd,"confdata", None), + *get_common_args(kwargs)) + mp.moveL(*([rt] + sd + [zd])) + +class MoveCCommandConv: + rr_types = ["experimental.robotics.motion_program.MoveCCommand"] + freeform_names = ["MoveC","MoveCCommand","experimental.robotics.motion_program.MoveCCommand"] + + def apply_rr_command(self, cmd, mp, cfx_robot, **kwargs): + zd = rr_zone_to_fanuc(cmd_get_arg(cmd,"fine_point"),cmd_get_arg(cmd,"blend_radius")) + sd = rr_speed_to_fanuc(cmd_get_arg(cmd,"tcp_velocity"), cmd_get_extended(cmd,"tcp_velocity_units", None)) + rt = rr_target_to_fanuc(cmd_get_arg(cmd,"tcp_pose"), cfx_robot, + cmd_get_extended(cmd,"joint_position"), cmd_get_extended(cmd,"joint_units", []), cmd_get_extended(cmd,"confdata", None), + *get_common_args(kwargs)) + rt2 = rr_target_to_fanuc(cmd_get_arg(cmd,"tcp_via_pose", None), cfx_robot, + cmd_get_extended(cmd,"joint_via_position", None), cmd_get_extended(cmd,"joint_via_units", []), cmd_get_extended(cmd,"confdata_via", None), + *get_common_args(kwargs)) + mp.moveC(*([rt2, rt] + sd + [zd])) + +class WaitTimeCommandConv: + rr_types = ["experimental.robotics.motion_program.WaitTimeCommand"] + freeform_names = ["WaitTime", "WaitTimeCommand", "experimental.robotics.motion_program.WaitTimeCommand"] + + def apply_rr_command(self, cmd, mp, **kwargs): + mp.waittime(cmd_get_arg(cmd, "time")) + +#FANUC Setup Commands + +_command_convs = dict() +_freeform_command_convs = dict() + +conv_types = [ + MoveAbsJCommandConv, + MoveJCommandConv, + MoveLCommandConv, + MoveCCommandConv, + WaitTimeCommandConv +] + +def _init_convs(): + for c in conv_types: + c_inst = c() + for x in c_inst.rr_types: + _command_convs[x] = c_inst + for y in c_inst.freeform_names: + _freeform_command_convs[y] = c_inst + +_init_convs() + +class OptionalCommandException(Exception): + def __init__(self, message): + super().__init__(message=message) + +def get_command_conv(cmd): + if cmd.datatype == "experimental.robotics.motion_program.FreeformCommand": + conv = _freeform_command_convs.get(cmd.data.command_name, None) + if conv is None: + if cmd.data.optional: + raise OptionalCommandException(f"Optional command {cmd.data.command_name}") + else: + assert False, f"Unknown command {cmd.data.command_name}" + return conv + else: + conv = _command_convs.get(cmd.datatype, None) + assert conv is not None, f"Unknown command {cmd.datatype}" + return conv + +def apply_rr_motion_command_to_mp(cmd, mp, **kwargs): + conv = get_command_conv(cmd) + conv.apply_rr_command(cmd, mp, **kwargs) + +def get_extended_int(rr_mp, name, default_value = None): + if rr_mp.extended is None: + return default_value + val = rr_mp.extended.get(name, default_value) + if val is None: + return default_value + if hasattr(val, "data"): + val = val.data[0] + return int(val) + +def rr_motion_program_to_fanuc(rr_mp, robot, motion_group): + + # Robot structure for computing confdata.cfx from joint seed + P_cfx_w = np.copy(robot.P) + P_cfx_w[:,0] = 0.0 + P_cfx_w[:,5:7] = 0.0 + cfx_robot = rox.Robot(robot.H, P_cfx_w, robot.joint_type) + + setup_args = dict() + if rr_mp.motion_setup_commands is not None: + for setup_cmd in rr_mp.motion_setup_commands: + #with suppress(OptionalCommandException): + conv = get_command_conv(setup_cmd) + conv.add_setup_args(setup_cmd, setup_args) + + if rr_mp.extended is not None: + first_cmd_num_rr = rr_mp.extended.get("first_command_number", None) + if first_cmd_num_rr is not None: + setup_args["first_cmd_num"] = int(first_cmd_num_rr.data) + motion_group = get_extended_int(rr_mp, "motion_group", motion_group) + uframe = get_extended_int(rr_mp, "uframe", 1) + utool = get_extended_int(rr_mp, "utool", 1) + mp = fanuc_exec.TPMotionProgram(**setup_args) + for cmd in rr_mp.motion_program_commands: + #with suppress(OptionalCommandException): + apply_rr_motion_command_to_mp(cmd, mp, robot=robot, cfx_robot=cfx_robot, motion_group=motion_group, uframe=uframe, utool=utool) + + return mp + +def is_rr_motion_program_multimove(rr_mp): + if rr_mp.extended is None: + return False + groups =rr_mp.extended.get("groups", None) + if groups is None: + groups = rr_mp.extended.get("tasks", None) + if groups is None: + return False + + if groups.datatype == "string": + return False + assert groups.datatype in ("int32[]", "uint32[]", "double[]", "varvalue{list}"), "Invalid groups type" + return len(groups.data) > 1 + +def get_rr_motion_program_task(rr_mp, default_task = 1): + if rr_mp.extended is None: + return default_task + groups =rr_mp.extended.get("groups", None) + if groups is None: + groups = rr_mp.extended.get("tasks", None) + if groups is None: + return default_task + + if groups.datatype in ("string", "int32[]", "uint32[]", "double[]"): + return groups.data + else: + assert groups.datatype == "varvalue{list}" + assert len(groups) == 1, "Multiple tasks not expected" + if groups.data[0].datatype == "string": + return int(groups.data[0].data) + elif groups.data[0].datatype in ("int32[]", "uint32[]", "double[]"): + return groups.data[0].data[0] + else: + assert False, "Invalid task type" + +def rr_motion_program_to_fanuc2(program, robots): + if (is_rr_motion_program_multimove(program)): + return rr_multimove_motion_program_to_fanuc(program, robots) + task = get_rr_motion_program_task(program) + + robot_ind = int(task)-1 + + rox_robot = robots[robot_ind] + mp = rr_motion_program_to_fanuc(program, rox_robot, task) + return mp, False, task + +def rr_multimove_motion_program_to_fanuc(program, robots): + motion_programs = [program] + multi_programs = program.extended.get("multi_motion_programs", None) + assert multi_programs is not None, "Invalid multimove motion program" + assert multi_programs.datatype == "varvalue{list}", "Invalid multimove motion program" + for mp in multi_programs.data: + assert mp.datatype == "experimental.robotics.motion_program.MotionProgram", "Invalid multimove motion program" + motion_programs.append(mp.data) + + groups = program.extended.get("groups", None) + assert groups is not None, "Invalid multimove motion program" + if groups.datatype in ("int32[]", "uint32[]", "double[]"): + tasks = [int(x+1) for x in groups.data] + elif groups.datatype == "varvalue{list}": + tasks = [int(x.data[0])+1 for x in groups.data] + else: + assert False, "Invalid multimove motion program" + + programs = [] + for i in range(len(motion_programs)): + robot_ind = int(tasks[i])-1 + rox_robot = robots[robot_ind] + programs.append(rr_motion_program_to_fanuc(motion_programs[i], rox_robot, tasks[i])) + + return programs, True, tasks diff --git a/src/fanuc_motion_program_exec/robotraconteur/experimental.robotics.motion_program.robdef b/src/fanuc_motion_program_exec/robotraconteur/experimental.robotics.motion_program.robdef new file mode 100644 index 0000000..ebf51b9 --- /dev/null +++ b/src/fanuc_motion_program_exec/robotraconteur/experimental.robotics.motion_program.robdef @@ -0,0 +1,196 @@ +service experimental.robotics.motion_program + +stdver 0.10 + +import com.robotraconteur.device +import com.robotraconteur.geometry +import com.robotraconteur.robotics.joints +import com.robotraconteur.robotics.tool +import com.robotraconteur.robotics.payload +import com.robotraconteur.robotics.robot +import com.robotraconteur.action +import com.robotraconteur.datetime + +using com.robotraconteur.device.Device +using com.robotraconteur.device.DeviceInfo +using com.robotraconteur.robotics.joints.JointPositionUnits +using com.robotraconteur.geometry.Pose +using com.robotraconteur.robotics.tool.ToolInfo +using com.robotraconteur.robotics.payload.PayloadInfo +using com.robotraconteur.robotics.robot.RobotInfo +using com.robotraconteur.action.ActionStatusCode +using com.robotraconteur.datetime.TimeSpec3 +using com.robotraconteur.robotics.robot.RobotOperationalMode +using com.robotraconteur.robotics.robot.RobotControllerState + +enum MotionProgramRobotCapabilities + unknown = 0, + queuing = 0x01, + current_command_feedback = 0x02, + queued_command_feedback = 0x04, + motion_recording = 0x08, + motion_program_preemption = 0x10, + motion_program_mode_select = 0x20, + preempt_number_feedback = 0x40 +end + +enum MotionProgramRobotStateFlags + unknown = 0, + error = 0x1, + fatal_error = 0x2, + estop = 0x4, + estop_button1 = 0x8, + estop_button2 = 0x10, + estop_button3 = 0x20, + estop_button4 = 0x40, + estop_guard1 = 0x80, + estop_guard2 = 0x100, + estop_guard3 = 0x200, + estop_guard4 = 0x400, + estop_software = 0x800, + estop_fault = 0x1000, + estop_internal = 0x2000, + estop_other = 0x4000, + estop_released = 0x8000, + enabling_switch = 0x10000, + enabled = 0x20000, + ready = 0x40000, + homed = 0x80000, + homing_required = 0x100000, + communication_failure = 0x200000, + motion_program_mode_enabled = 0x10000000, + motion_program_running = 0x20000000 +end + +struct RobotPose + field Pose tcp_pose + field double[] joint_position_seed + field JointPositionUnits{list} joint_units +end + +struct MoveAbsJCommand + field double[] joint_position + field JointPositionUnits{list} joint_units + field double tcp_velocity + field double tcp_acceleration + field double blend_radius + field bool fine_point + field varvalue{string} extended +end + +struct MoveJCommand + field RobotPose tcp_pose + field double tcp_velocity + field double tcp_acceleration + field double blend_radius + field bool fine_point + field varvalue{string} extended +end + +struct MoveLCommand + field RobotPose tcp_pose + field double tcp_velocity + field double tcp_acceleration + field double blend_radius + field bool fine_point + field varvalue{string} extended +end + +struct MoveCCommand + field RobotPose tcp_via_pose + field RobotPose tcp_pose + field double tcp_velocity + field double tcp_acceleration + field double blend_radius + field bool fine_point + field varvalue{string} extended +end + +struct WaitTimeCommand + field double time + field varvalue{string} extended +end + +struct SetToolCommand + field ToolInfo tool_info + field varvalue{string} extended +end + +struct SetPayloadCommand + field PayloadInfo payload_info + field Pose payload_pose + field varvalue{string} extended +end + +struct FreeformCommand + field string command_name + field varvalue{string} command_args + field bool optional + field varvalue{string} extended +end + +struct MotionProgram + field varvalue{list} motion_setup_commands + field varvalue{list} motion_program_commands + field varvalue{string} extended +end + +struct MotionProgramCommandInfo + field string command_name + field string{list} command_struct_types + field string{list} freeform_command_names + field string description + field varvalue{string} extended +end + +struct MotionProgramRobotInfo + field RobotInfo robot_info + field MotionProgramCommandInfo{list} supported_setup_commands + field MotionProgramCommandInfo{list} supported_motion_commands + field uint32 motion_program_robot_capabilities + field varvalue{string} extended +end + +struct MotionProgramStatus + field ActionStatusCode action_status + field int32 current_command + field int32 queued_command + field int32 current_preempt + field uint32 recording_handle +end + +struct MotionProgramRobotState + field TimeSpec3 ts + field uint64 seqno + field RobotOperationalMode operational_mode + field RobotControllerState controller_state + field uint64 motion_program_robot_state_flags + field int32 current_command + field int32 queued_command + field int32 current_preempt +end + +struct MotionProgramRecordingPart + field double[] time + field int32[] command_number + field double[*] joints + field string{list} column_headers +end + +object MotionProgramRobot + implements Device + property DeviceInfo device_info [readonly,nolock] + property RobotInfo robot_info [readonly,nolock] + property MotionProgramRobotInfo motion_program_robot_info [readonly,nolock] + function MotionProgramStatus{generator} execute_motion_program(MotionProgram program, bool queue) + function MotionProgramStatus{generator} execute_motion_program_record(MotionProgram program, bool queue) + function void preempt_motion_program(MotionProgram program, uint32 preempt_number, uint32 preempt_cmdnum) + function MotionProgramRecordingPart{generator} read_recording(uint32 recording_handle) + wire MotionProgramRobotState motion_program_robot_state [readonly,nolock] + function void enable_motion_program_mode() + function void disable_motion_program_mode() + function void clear_recordings() + function varvalue getf_param(string param_name) + function void setf_param(string param_name, varvalue value) + event param_changed(string param_name) +end \ No newline at end of file diff --git a/src/fanuc_motion_program_exec/robotraconteur/fanuc_motion_program_exec_robotraconteur.py b/src/fanuc_motion_program_exec/robotraconteur/fanuc_motion_program_exec_robotraconteur.py new file mode 100644 index 0000000..f05d089 --- /dev/null +++ b/src/fanuc_motion_program_exec/robotraconteur/fanuc_motion_program_exec_robotraconteur.py @@ -0,0 +1,187 @@ +import sys +import numpy as np +import argparse +import threading +from .. import fanuc_motion_program_exec_client as fanuc_client +import RobotRaconteur as RR +RRN=RR.RobotRaconteurNode.s +import RobotRaconteurCompanion as RRC +from RobotRaconteurCompanion.Util.InfoFileLoader import InfoFileLoader +from RobotRaconteurCompanion.Util.AttributesUtil import AttributesUtil +from RobotRaconteurCompanion.Util.TaskGenerator import SyncTaskGenerator +from RobotRaconteurCompanion.Util.RobDef import register_service_types_from_resources +from RobotRaconteurCompanion.Util.RobotUtil import RobotUtil +from ._motion_program_conv import rr_motion_program_to_fanuc2 + +import traceback +import time +import io +import random +import drekar_launch_process + +class MotionExecImpl: + def __init__(self, mp_robot_info, robot_ip, robot_username, robot_ip2, robot_username2): + + self.mp_robot_info = mp_robot_info + self._fanuc_client = fanuc_client.FANUCClient(robot_ip, robot_username, robot_ip2, robot_username2) + + self.device_info = mp_robot_info.robot_info.device_info + self.robot_info = mp_robot_info.robot_info + self.motion_program_robot_info = mp_robot_info + self._recordings = {} + + self.param_changed = RR.EventHook() + + self._robot_util = RobotUtil(RRN) + try: + self._rox_robots = [] + for chain_i in range(len(self.robot_info.chains)): + self._rox_robots.append(self._robot_util.robot_info_to_rox_robot(self.robot_info,chain_i)) + except: + traceback.print_exc() + raise ValueError("invalid robot_info, could not populate GeneralRoboticsToolbox.Robot") + + + def execute_motion_program(self, program, queue): + + assert queue is False, "Motion program queue not supported" + + fanuc_program, is_multimove, tasks = rr_motion_program_to_fanuc2(program, self._rox_robots) + + gen = ExecuteMotionProgramGen(self, self._fanuc_client, fanuc_program, is_multimove, tasks) + + return gen + + def execute_motion_program_record(self, program, queue): + + assert queue is False, "Motion program queue not supported" + + fanuc_program, is_multimove, tasks = rr_motion_program_to_fanuc2(program, self._rox_robots) + + gen = ExecuteMotionProgramGen(self, self._fanuc_client, fanuc_program, is_multimove, tasks, save_recording = True) + + return gen + + def read_recording(self, recording_handle): + robot_recording_txt = self._recordings.pop(recording_handle) + return RobotRecordingGen(robot_recording_txt) + + def clear_recordings(self): + self._recordings.clear() + + def get_param(self, param_name): + raise RR.InvalidArgumentException("Unknown parameter") + + def set_param(self, param_name, value): + raise RR.InvalidArgumentException("Unknown parameter") + + def enable_motion_program_mode(self): + pass + + def disable_motion_program_mode(self): + pass + + +class ExecuteMotionProgramGen(SyncTaskGenerator): + def __init__(self, parent, fanuc_client, motion_program, is_multimove, tasks, save_recording = False): + super().__init__(RRN, RRN.GetStructureType("experimental.robotics.motion_program.MotionProgramStatus"), 1, -1) + self._parent = parent + self._fanuc_client = fanuc_client + self._motion_program = motion_program + self._recording_handle = 0 + self._save_recording = save_recording + self._is_multimove = is_multimove + self._tasks = tasks + + def RunTask(self): + print("Start Motion Program!") + if not self._is_multimove: + robot_recording_data = self._fanuc_client.execute_motion_program(self._motion_program, self._save_recording) + else: + robot_recording_data = self._fanuc_client.execute_motion_program_coord(self._motion_program[0], self._motion_program[1], self._save_recording) + if self._save_recording: + recording_handle = random.randint(0,0xFFFFFFF) + self._parent._recordings[recording_handle] = robot_recording_data + self._recording_handle = recording_handle + print("Motion Program Complete!") + res = self._status_type() + res.action_status = self._action_const["ActionStatusCode"]["complete"] + res.recording_handle = self._recording_handle + return res + + +class RobotRecordingGen: + def __init__(self, robot_recording_txt ): + self.robot_rec_txt = robot_recording_txt + self.closed = False + self.aborted = False + self.lock = threading.Lock() + self._mp_recording_part = RRN.GetStructureType("experimental.robotics.motion_program.MotionProgramRecordingPart") + + def Next(self): + with self.lock: + if self.aborted: + raise RR.OperationAbortedException("Recording aborted") + + if self.closed: + raise RR.StopIterationException() + + ret = self._mp_recording_part() + if isinstance(self.robot_rec_txt,bytes): + self.robot_rec_txt = self.robot_rec_txt.decode('utf-8') + f = io.StringIO(self.robot_rec_txt) + column_headers = [x.strip() for x in f.readline().strip().split(',')] + data = np.genfromtxt(f,delimiter=',',skip_header=1) + + # All current paths expect to be within 10 MB limit + ret.time = (data[:,0].flatten().astype(np.float64))*1e-3 + # ret.command_number = data[:,1].flatten().astype(np.int32) + # TODO: prismatic joints + ret.joints = np.deg2rad(data[:,1:].astype(np.float64)) + ret.column_headers = column_headers + + self.closed = True + return ret + + def Abort(self): + self.aborted = True + + def Close(self): + self.closed = True + +def main(): + + parser = argparse.ArgumentParser(description="FANUC Robot motion program driver service for Robot Raconteur") + parser.add_argument("--mp-robot-info-file", type=argparse.FileType('r'),default=None,required=True,help="Motion program robot info file (required)") + parser.add_argument("--mp-robot-ip", type=str, default='127.0.0.1', help="robot controller ip address (default 127.0.0.1)") + parser.add_argument("--mp-robot-username",type=str,default='robot',help="robot controller username (default 'robot')") + parser.add_argument("--mp-robot2-ip", type=str, default=None, help="robot 2 controller ip address") + parser.add_argument("--mp-robot2-username", type=str, default='robot', help="robot 2 controller username") + + args, _ = parser.parse_known_args() + + RRC.RegisterStdRobDefServiceTypes(RRN) + register_service_types_from_resources(RRN, __package__, ["experimental.robotics.motion_program"]) + + with args.mp_robot_info_file: + mp_robot_info_text = args.mp_robot_info_file.read() + + info_loader = InfoFileLoader(RRN) + mp_robot_info, mp_robot_ident_fd = info_loader.LoadInfoFileFromString(mp_robot_info_text, "experimental.robotics.motion_program.MotionProgramRobotInfo", "device") + + attributes_util = AttributesUtil(RRN) + mp_robot_attributes = attributes_util.GetDefaultServiceAttributesFromDeviceInfo(mp_robot_info.robot_info.device_info) + + mp_exec_obj = MotionExecImpl(mp_robot_info,args.mp_robot_ip,args.mp_robot_username,args.mp_robot2_ip,args.mp_robot2_username) + + with RR.ServerNodeSetup("experimental.robotics.motion_program",59845,argv=sys.argv): + + service_ctx = RRN.RegisterService("mp_robot","experimental.robotics.motion_program.MotionProgramRobot",mp_exec_obj) + service_ctx.SetServiceAttributes(mp_robot_attributes) + + print("Press ctrl+c to quit") + drekar_launch_process.wait_exit() + +if __name__ == "__main__": + main() + From 095a64964824286ba046239a1bb54d6aefdd9cb4 Mon Sep 17 00:00:00 2001 From: John Wason Date: Thu, 21 Dec 2023 21:24:13 -0500 Subject: [PATCH 4/6] Add robot raconteur information to readme --- README.md | 39 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/README.md b/README.md index 7254ba7..1251370 100644 --- a/README.md +++ b/README.md @@ -264,6 +264,45 @@ with open("fanuc_log.csv","wb") as f: print(res.decode('utf-8')) ``` +# Robot Raconteur Service + +A Robot Raconteur service is available to allow a client to execute multi-move programs. The service +uses a standards track service definition `experimental.robotics.motion_program` that will provide interoperability +between multiple robot types. Due to the differences in the robot commands between controller implementations there +may be slight differences in the commands between robots, but in general the commands are very similar. + +When installing the module, include the `robotraconteur` option to get the required dependencies: + +```bash +python -m pip install fanuc-motion-program-exec[robotraconteur] +``` + +Start the service, specifying a robot info file: + +``` +fanuc-motion-program-exec-robotraconteur --mp-robot-info-file=config/FANUC_lrmate200id_motion_program_robot_default_config.yml +``` + +Optionally start using a module if the entrypoint does not work: + +``` +python -m fanuc_motion_program_exec_client.robotraconteur --mp-robot-info-file=config/FANUC_lrmate200id_motion_program_robot_default_config.yml +``` + +The following options are supported: + +* `--mp-robot-info-file=` - The info file that specifies information about the robot +* `--mp-robot-ip=` - The connection IP address of the robot. Defaults to `127.0.0.1` + for use with RoboGuide virtual controllers. Set `127.0.0.1` to the WAN IP address of the robot. +* `--mp-robot-username=` - The robot controller username. Defaults to "robot" +* `--mp-robot2-ip=` - Connection IP of second robot controller +* `--mp-robot2-username=` - Username for second robot controller + +Examples for a single robot and multi-move robots are in the `examples/robotraconteur` directory. The motion +programs make heavy use of `varvalue` types to allow for flexibility in the motion program contents. +The Python type `RR.VarValue` is used to represent the `varvalue` type. See the Robot Raconteur Python documentation +for more inforamtion on how `RR.VarValue` works and why it is necessary. + ## License Apache 2.0 License, Copyright 2022 Rensselaer Polytechnic Institute From c16aba40adcd15b54dfc254918e4b1167f7c5770 Mon Sep 17 00:00:00 2001 From: John Wason Date: Thu, 21 Dec 2023 21:25:22 -0500 Subject: [PATCH 5/6] Add entrypoint to pyproject.toml --- pyproject.toml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 0d33268..5591657 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -27,4 +27,7 @@ robotraconteur = [ ] [tool.setuptools.package-data] -"fanuc_motion_program_exec.robotraconteur" = ["*.robdef"] \ No newline at end of file +"fanuc_motion_program_exec.robotraconteur" = ["*.robdef"] + +[project.scripts] +fanuc-motion-program-exec-robotraconteur = "fanuc_motion_program_exec.robotraconteur.fanuc_motion_program_exec_robotraconteur:main" From 1e390b40e3e8413ddb2fa422886e09da21039c1f Mon Sep 17 00:00:00 2001 From: John Wason Date: Thu, 21 Dec 2023 21:28:07 -0500 Subject: [PATCH 6/6] Rename module back to fanuc_motion_program_exec_client --- examples/example.py | 2 +- examples/io_example.py | 2 +- examples/multi_coord_example.py | 2 +- examples/multi_example.py | 2 +- examples/read_joint_example.py | 2 +- pyproject.toml | 4 ++-- .../__init__.py | 0 .../fanuc_motion_program_exec_client.py | 0 .../robotraconteur/__init__.py | 0 .../robotraconteur/__main__.py | 0 .../robotraconteur/_motion_program_conv.py | 0 .../experimental.robotics.motion_program.robdef | 0 .../fanuc_motion_program_exec_robotraconteur.py | 0 tools/roboguide_base_adjust.py | 2 +- 14 files changed, 8 insertions(+), 8 deletions(-) rename src/{fanuc_motion_program_exec => fanuc_motion_program_exec_client}/__init__.py (100%) rename src/{fanuc_motion_program_exec => fanuc_motion_program_exec_client}/fanuc_motion_program_exec_client.py (100%) rename src/{fanuc_motion_program_exec => fanuc_motion_program_exec_client}/robotraconteur/__init__.py (100%) rename src/{fanuc_motion_program_exec => fanuc_motion_program_exec_client}/robotraconteur/__main__.py (100%) rename src/{fanuc_motion_program_exec => fanuc_motion_program_exec_client}/robotraconteur/_motion_program_conv.py (100%) rename src/{fanuc_motion_program_exec => fanuc_motion_program_exec_client}/robotraconteur/experimental.robotics.motion_program.robdef (100%) rename src/{fanuc_motion_program_exec => fanuc_motion_program_exec_client}/robotraconteur/fanuc_motion_program_exec_robotraconteur.py (100%) diff --git a/examples/example.py b/examples/example.py index d3567fb..a90b166 100644 --- a/examples/example.py +++ b/examples/example.py @@ -1,4 +1,4 @@ -from fanuc_motion_program_exec import * +from fanuc_motion_program_exec_client import * tp = TPMotionProgram() diff --git a/examples/io_example.py b/examples/io_example.py index 3027fb4..cfdbe8f 100644 --- a/examples/io_example.py +++ b/examples/io_example.py @@ -1,4 +1,4 @@ -from fanuc_motion_program_exec import * +from fanuc_motion_program_exec_client import * client = FANUCClient('192.168.0.1') client.set_ioport('DOUT',10,True) diff --git a/examples/multi_coord_example.py b/examples/multi_coord_example.py index 29852ab..689f5ba 100644 --- a/examples/multi_coord_example.py +++ b/examples/multi_coord_example.py @@ -1,4 +1,4 @@ -from fanuc_motion_program_exec import * +from fanuc_motion_program_exec_client import * tp_lead = TPMotionProgram() tp_follow = TPMotionProgram() diff --git a/examples/multi_example.py b/examples/multi_example.py index bb0723e..97d86e7 100644 --- a/examples/multi_example.py +++ b/examples/multi_example.py @@ -1,4 +1,4 @@ -from fanuc_motion_program_exec import * +from fanuc_motion_program_exec_client import * tp1 = TPMotionProgram() tp2 = TPMotionProgram() diff --git a/examples/read_joint_example.py b/examples/read_joint_example.py index 2498389..da866f1 100644 --- a/examples/read_joint_example.py +++ b/examples/read_joint_example.py @@ -1,4 +1,4 @@ -from fanuc_motion_program_exec import * +from fanuc_motion_program_exec_client import * client = FANUCClient('192.168.0.1') diff --git a/pyproject.toml b/pyproject.toml index 5591657..03239de 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -27,7 +27,7 @@ robotraconteur = [ ] [tool.setuptools.package-data] -"fanuc_motion_program_exec.robotraconteur" = ["*.robdef"] +"fanuc_motion_program_exec_client.robotraconteur" = ["*.robdef"] [project.scripts] -fanuc-motion-program-exec-robotraconteur = "fanuc_motion_program_exec.robotraconteur.fanuc_motion_program_exec_robotraconteur:main" +fanuc-motion-program-exec-robotraconteur = "fanuc_motion_program_exec_client.robotraconteur.fanuc_motion_program_exec_robotraconteur:main" diff --git a/src/fanuc_motion_program_exec/__init__.py b/src/fanuc_motion_program_exec_client/__init__.py similarity index 100% rename from src/fanuc_motion_program_exec/__init__.py rename to src/fanuc_motion_program_exec_client/__init__.py diff --git a/src/fanuc_motion_program_exec/fanuc_motion_program_exec_client.py b/src/fanuc_motion_program_exec_client/fanuc_motion_program_exec_client.py similarity index 100% rename from src/fanuc_motion_program_exec/fanuc_motion_program_exec_client.py rename to src/fanuc_motion_program_exec_client/fanuc_motion_program_exec_client.py diff --git a/src/fanuc_motion_program_exec/robotraconteur/__init__.py b/src/fanuc_motion_program_exec_client/robotraconteur/__init__.py similarity index 100% rename from src/fanuc_motion_program_exec/robotraconteur/__init__.py rename to src/fanuc_motion_program_exec_client/robotraconteur/__init__.py diff --git a/src/fanuc_motion_program_exec/robotraconteur/__main__.py b/src/fanuc_motion_program_exec_client/robotraconteur/__main__.py similarity index 100% rename from src/fanuc_motion_program_exec/robotraconteur/__main__.py rename to src/fanuc_motion_program_exec_client/robotraconteur/__main__.py diff --git a/src/fanuc_motion_program_exec/robotraconteur/_motion_program_conv.py b/src/fanuc_motion_program_exec_client/robotraconteur/_motion_program_conv.py similarity index 100% rename from src/fanuc_motion_program_exec/robotraconteur/_motion_program_conv.py rename to src/fanuc_motion_program_exec_client/robotraconteur/_motion_program_conv.py diff --git a/src/fanuc_motion_program_exec/robotraconteur/experimental.robotics.motion_program.robdef b/src/fanuc_motion_program_exec_client/robotraconteur/experimental.robotics.motion_program.robdef similarity index 100% rename from src/fanuc_motion_program_exec/robotraconteur/experimental.robotics.motion_program.robdef rename to src/fanuc_motion_program_exec_client/robotraconteur/experimental.robotics.motion_program.robdef diff --git a/src/fanuc_motion_program_exec/robotraconteur/fanuc_motion_program_exec_robotraconteur.py b/src/fanuc_motion_program_exec_client/robotraconteur/fanuc_motion_program_exec_robotraconteur.py similarity index 100% rename from src/fanuc_motion_program_exec/robotraconteur/fanuc_motion_program_exec_robotraconteur.py rename to src/fanuc_motion_program_exec_client/robotraconteur/fanuc_motion_program_exec_robotraconteur.py diff --git a/tools/roboguide_base_adjust.py b/tools/roboguide_base_adjust.py index fa7353b..63e8a3e 100644 --- a/tools/roboguide_base_adjust.py +++ b/tools/roboguide_base_adjust.py @@ -8,7 +8,7 @@ import sys,os,time,traceback,platform,subprocess from general_robotics_toolbox import * -from fanuc_motion_program_exec import * +from fanuc_motion_program_exec_client import * class BaseGUI(QDialog): def __init__(self, parent=None):