From 26fbab4f2ae097779fe6e3e002298024dd00681c Mon Sep 17 00:00:00 2001
From: Anton Tetov <anton@tetov.se>
Date: Tue, 8 Feb 2022 18:01:55 +0100
Subject: [PATCH] added move to conf and joint_mode, decorator for whitespace

---
 ur_py_ctl/__init__.py          |  2 -
 ur_py_ctl/__main__.py          | 13 +++--
 ur_py_ctl/send_script.py       | 20 ++++----
 ur_py_ctl/urscript_commands.py | 92 +++++++++++++++++++++++++++-------
 4 files changed, 89 insertions(+), 38 deletions(-)

diff --git a/ur_py_ctl/__init__.py b/ur_py_ctl/__init__.py
index 550c059..6c89e01 100644
--- a/ur_py_ctl/__init__.py
+++ b/ur_py_ctl/__init__.py
@@ -1,7 +1,5 @@
 import pathlib
 
-from .send_script import send_script  # noqa: F401,F402
-
 __version__ = "0.1.0"
 
 HERE = pathlib.Path(__file__).parent
diff --git a/ur_py_ctl/__main__.py b/ur_py_ctl/__main__.py
index ae66b4b..fadc256 100644
--- a/ur_py_ctl/__main__.py
+++ b/ur_py_ctl/__main__.py
@@ -1,16 +1,15 @@
 import argparse
-import pathlib
 
-from ur_py_ctl import send_script
+from ur_py_ctl.send_script import send_script as send_script_func
 
 if __name__ == "__main__":
 
     parser = argparse.ArgumentParser(description="Send program to UR")
-    parser.add_argument("path", type=pathlib.Path, help="script file")
+    # parser.add_argument("path", type=pathlib.Path, help="script file")
 
-    args = parser.parse_args()
+    # args = parser.parse_args()
 
-    if not args.path.exists():
-        raise FileNotFoundError()
+    # if not args.path.exists():
+    #    raise FileNotFoundError()
 
-    send_script()
+    send_script_func()
diff --git a/ur_py_ctl/send_script.py b/ur_py_ctl/send_script.py
index 7dc271b..544fbff 100644
--- a/ur_py_ctl/send_script.py
+++ b/ur_py_ctl/send_script.py
@@ -6,20 +6,20 @@ import socket
 import ur_py_ctl.urscript_commands as ur_cmd
 from ur_py_ctl import LOG_DIR
 
-SERVER_ADRESS = "192.168.10.11"
+SERVER_ADRESS = "192.168.101.102"
 SERVER_PORT = 30003
-UR_IP = "192.168.10.10"
+UR_IP = "192.168.101.101"
 UR_SERVER_PORT = 30002
 # TOOL_ANGLE_AXIS = [-68.7916, -1.0706, 100, 3.1416, 0.0, 0.0]
 
 # New tool angle
-TOOL_ANGLE_AXIS = [-26.25, 71.64, 125.2, 3.1416, 0.0, 0.0]
+TOOL_ANGLE_AXIS = [0, 0, 0, 0, 0.0, 0.0]
 
 # UTIL
 
-LOG_FILE = LOG_DIR / "send_script.log"
-
-logging.basicConfig(filename=LOG_FILE, filemode="a", level=logging.DEBUG)
+logging.basicConfig(
+    filename=LOG_DIR / "send_script.log", filemode="a", level=logging.DEBUG
+)
 
 
 def start_log() -> None:
@@ -41,7 +41,7 @@ def get_script() -> bytes:
 
     for i in range(10):
 
-        script += ur_cmd.movel(i * 0.2, 0, 0, 0, 0, 0, 50 / 1000, 5 / 1000)
+        script += ur_cmd.move_to_pose(i * 0.2, 0, 500, 0, 0, 0, v=50 / 1000, r=5 / 1000)
         script += ur_cmd.textmsg(f"Sending command number: {i}")
 
     script += f'\tsocket_open("{SERVER_ADRESS}", {SERVER_PORT})\n'
@@ -51,12 +51,10 @@ def get_script() -> bytes:
     script += "end\n"
     script += "program()\n\n\n"
 
-    script = script.encode()
-
-    return script
+    return script.encode()
 
 
-def send_script(script_path) -> None:
+def send_script() -> None:
     start_log()
 
     send_socket = socket.create_connection((UR_IP, UR_SERVER_PORT), timeout=2)
diff --git a/ur_py_ctl/urscript_commands.py b/ur_py_ctl/urscript_commands.py
index 3f07af3..d147119 100644
--- a/ur_py_ctl/urscript_commands.py
+++ b/ur_py_ctl/urscript_commands.py
@@ -1,35 +1,91 @@
-def _add_whitespace(string: str) -> str:
-    return "\t" + string + "\n"
+from functools import wraps
 
 
-def set_tcp(x, y, z, ax, ay, az) -> str:
-    cmd = "set_tcp(p[{:.5f}, {:.5f}, {:.5f}, {:.5f}, {:.5f}, {:.5f}])".format(
-        x / 1000.0, y / 1000.0, z / 1000.0, ax, ay, az
-    )
-    return _add_whitespace(cmd)
+def _add_whitespace(func):
+    @wraps(func)
+    def wrapper(*args, **kwargs):
+        return f"\t{func(*args, **kwargs)}\n"
 
+    return wrapper
 
-def movel(x, y, z, ax, ay, az, speed, radius) -> str:
-    cmd = f"movel(p[{x:.5f}, {y:.5f}, {z:.5f}, {ax:.5f}, {ay:.5f}, {z:.5f}], v={speed:.3f}, r={radius:.3f})"  # noqa: E501
-    return _add_whitespace(cmd)
 
+class Motion(object):
+    LINEAR = 0
+    JOINT = 1
 
+
+def _get_func(func_name, urscript_args, urscript_kwargs=None):
+    args = urscript_args
+    if urscript_kwargs:
+        args += f", {urscript_kwargs}"
+
+    return f"{func_name}({args})"
+
+
+def _get_pose(x: float, y: float, z: float, ax: float, ay: float, az: float) -> str:
+    return f"p[{x:.5f}, {y:.5f}, {z:.5f}, {ax:.5f}, {ay:.5f}, {az:.5f}]"
+
+
+def _get_conf(j1, j2, j3, j4, j5, j6):
+    return f"[{j1:.5f}, {j2:.5f}, {j3:.5f}, {j4:.5f}, {j5:.5f}, {j6:.5f}]"
+
+
+def _get_move_kwargs(**kwargs) -> str:
+    ok_keywords = ("v", "a", "t", "r")
+
+    urscript_kwargs = []
+
+    for key in kwargs:
+        if key in ok_keywords:
+            urscript_kwargs.append(f"{key}={kwargs[key]:.5f]}")
+        else:
+            raise RuntimeError(f"Keyword name {key} not recognized.")
+
+    if len(urscript_kwargs) == 0:
+        return ""
+    else:
+        return " ,".join(kwargs)
+
+
+@_add_whitespace
+def set_tcp(*args) -> str:
+    return _get_func("set_tcp", _get_pose(*args))
+
+
+@_add_whitespace
+def move_to_pose(*pose_args, mode=Motion.JOINT, **kwargs):
+    func_name = "movel" if mode == Motion.LINEAR else "movej"
+
+    pose = _get_pose(*pose_args)
+    move_kwargs = _get_move_kwargs(**kwargs)
+
+    return _get_func(func_name, pose, move_kwargs)
+
+
+@_add_whitespace
+def move_to_conf(*conf_args, **kwargs):
+    conf = _get_conf(*conf_args)
+    move_kwargs = _get_move_kwargs(**kwargs)
+
+    return _get_func("movej", conf, move_kwargs)
+
+
+@_add_whitespace
 def textmsg(string: str) -> str:
-    cmd = f'textmsg("{string}")'
-    return _add_whitespace(cmd)
+    return f'textmsg("{string}")'
 
 
+@_add_whitespace
 def set_DO(pin: int, state: bool) -> str:
-    cmd = f"set_digital_out({pin:d}, {state})"
-    return _add_whitespace(cmd)
+    return f"set_digital_out({pin:d}, {state})"
 
 
+@_add_whitespace
 def sleep(seconds: int) -> str:
-    cmd = "sleep({})".format(seconds)
-    return _add_whitespace(cmd)
+    return "sleep({})".format(seconds)
 
 
+@_add_whitespace
 def popup(string: str) -> str:
     # Popup title not implemented, neither is error or warning flags
-    cmd = 'popup("{}")'.format(string)
-    return _add_whitespace(cmd)
+    return f'popup("{string}")'
-- 
GitLab