Implemented basics of UR-RTDE plugin

single_ursim_control/config.py

@@ -7,3 +7,4 @@ class Config:
     SYNC_DELAY = float(os.environ.get("SYNC_DELAY", 1.0))
     REDIS_URL = os.environ["REDIS_URL"]
     SYNC_TIMEOUT = os.environ.get("SYNC_TIMEOUT", None)  # Wait infinity by default
+    ROBOT_ADDRESS = os.environ.get("ROBOT_ADDRESS")

single_ursim_control/main.py

@@ -2,7 +2,7 @@
 import os
 import sys
 from config import Config
-from plugins import SleepPlugin, SyncPlugin, WaitPlugin
+from plugins import SleepPlugin, SyncPlugin, WaitPlugin, URRTDEPlugin
 from plugin_repository import PluginRepository
 from program_executor import ProgramExecutor
 from http_server import ControllerHTTPServer
@@ -36,13 +36,20 @@ def main():
     compiler_repo.register_plugin(SleepPlugin)
     compiler_repo.register_plugin(SyncPlugin)
     compiler_repo.register_plugin(WaitPlugin)
+    compiler_repo.register_plugin(URRTDEPlugin)
 
     # Example code:
     compiler_repo.load_plugin("sleep")
     compiler_repo.load_plugin("sync")
     compiler_repo.load_plugin("wait")
+    compiler_repo.load_plugin("ur_rtde")
     program = []
     program.append(compiler_repo.get_compiler("sleep").compile(secs=2))
+    program.append(compiler_repo.get_compiler("moveJ").compile([5.7386425805573555, -0.536165146212658, 1.6278685933351111, -2.661452576366153, -1.5683528658421044, 1.0096729722787197], 1.0, 4.0))
+    program.append(compiler_repo.get_compiler("moveL").compile([-0.4, 0.1, -0.31, 3.142, 0, 0], 0.05, 0.75))
+    program.append(compiler_repo.get_compiler("moveL").compile([-0.4, 0.1, -0.24, 3.142, 0, 0], 0.05, 0.75))
+    program.append(compiler_repo.get_compiler("moveJ").compile([5.923472948343555, 0.032637657012293965, 0.2590068609959585, -0.2935643801854462, -2.7157323161031766, 4.71238898038469], 1.0, 4.0))
+    program.append(compiler_repo.get_compiler("moveJ").compile([4.982042349817814, -0.5256931707006921, 1.620887276327134, -1.0993828958312282, -3.660653573132907, 5.271592472723674], 1.0, 4.0))
     program.append(compiler_repo.get_compiler("wait").compile())
     program.append(compiler_repo.get_compiler("sleep").compile(secs=3))
     program.append(compiler_repo.get_compiler("sync").compile(nodes=2, name="test"))

single_ursim_control/plugins/__init__.py

@@ -2,3 +2,4 @@ from .abstract_plugin import AbstractCommand, AbstractCommandCompiler, AbstractP
 from .sleep_plugin import SleepPlugin
 from .sync_plugin import SyncPlugin
 from .wait_plugin import WaitPlugin
+from .ur_rtde_plugin import URRTDEPlugin

single_ursim_control/plugins/ur_rtde_plugin.py

@@ -0,0 +1,160 @@
+import threading
+from typing import Dict, List
+
+from .abstract_plugin import AbstractCommand, AbstractPlugin, AbstractCommandCompiler
+import logging
+import rtde_control
+import rtde_receive
+
+from config import Config
+
+
+#
+# I have to admit, the abort() solution is not the most robust thing I've ever designed
+# But for this simple scenario, it's good enough
+#
+
+class CombinedRobotInterface:
+    USE_UPPER_RANGE_REGISTERS = False
+
+    def __init__(self, addr: str):
+        self._robot_c = rtde_control.RTDEControlInterface(addr)
+        self._robot_r = rtde_receive.RTDEReceiveInterface(
+            addr,
+            use_upper_range_registers=self.USE_UPPER_RANGE_REGISTERS
+        )
+
+    @property
+    def robot_c(self) -> rtde_control.RTDEControlInterface:
+        return self._robot_c
+
+    @property
+    def robot_r(self) -> rtde_receive.RTDEReceiveInterface:
+        return self._robot_r
+
+    def get_async_operation_progress(self) -> int:
+        # https://gitlab.com/sdurobotics/ur_rtde/-/blob/968b08e357f2e562e9ae156a3036283e23fa74f2/src/rtde_receive_interface.cpp#L42
+        register_offset = 24 if self.USE_UPPER_RANGE_REGISTERS else 0
+
+        # https://gitlab.com/sdurobotics/ur_rtde/-/blob/968b08e357f2e562e9ae156a3036283e23fa74f2/src/rtde_receive_interface.cpp#L632
+        return self.robot_r.getOutputIntRegister(register_offset + 2)
+
+        # This does not work.... lol
+        # The rtde_receive interface only allows you to use the user defined registers not the internal ones
+
+    def close(self):
+        self._robot_c.stopScript()
+        self._robot_c.disconnect()
+        self._robot_r.disconnect()
+
+
+class URRTDEMoveJCommand(AbstractCommand):
+
+    def __init__(self,
+                 logger: logging.Logger,
+                 robot: CombinedRobotInterface,
+                 pose: List[float],
+                 speed: float,
+                 acceleration: float):
+        # TODO: validations
+
+        self._logger = logger
+        self._robot = robot
+        self._pose = pose
+        self._speed = speed
+        self._acceleration = acceleration
+
+    def execute(self):
+        self._logger.debug(f"Moving robot arm: MoveJ to {self._pose} s: {self._speed} a: {self._acceleration}")
+        self._robot.robot_c.moveJ(self._pose, self._speed, self._acceleration)
+        self._logger.debug("Done moving")
+
+    def abort(self):
+        # TODO
+        pass
+
+    def describe(self) -> dict:
+        return {
+            "command": "moveJ",
+            "params": {
+                "pose": self._pose,
+                "speed": self._speed,
+                "acceleration": self._acceleration
+            }
+        }
+
+
+class URRTDEMoveLCommand(AbstractCommand):
+
+    def __init__(self,
+                 logger: logging.Logger,
+                 robot: CombinedRobotInterface,
+                 q: List[float],
+                 speed: float,
+                 acceleration: float):
+        # TODO: validations
+
+        self._logger = logger
+        self._robot = robot
+        self._q = q
+        self._speed = speed
+        self._acceleration = acceleration
+
+    def execute(self):
+        self._logger.debug(f"Moving robot arm: MoveL to {self._q} s: {self._speed} a: {self._acceleration}")
+        self._robot.robot_c.moveL(self._q, self._speed, self._acceleration)
+        self._logger.debug("Done moving")
+
+    def abort(self):
+        # TODO
+        pass
+
+    def describe(self) -> dict:
+        return {
+            "command": "moveL",
+            "params": {
+                "q": self._q,
+                "speed": self._speed,
+                "acceleration": self._acceleration
+            }
+        }
+
+
+class URRTDEMoveJCompiler(AbstractCommandCompiler):
+
+    def __init__(self, logger: logging.Logger, robot: CombinedRobotInterface):
+        self._logger = logger
+        self._robot = robot
+
+    def compile(self, pose: List[float], speed: float = 1.05, acceleration: float = 1.4) -> AbstractCommand:
+        return URRTDEMoveJCommand(self._logger, self._robot, pose, speed, acceleration)
+
+
+class URRTDEMoveLCompiler(AbstractCommandCompiler):
+
+    def __init__(self, logger: logging.Logger, robot: CombinedRobotInterface):
+        self._logger = logger
+        self._robot = robot
+
+    def compile(self, q: List[float], speed: float = 0.25, acceleration: float = 1.2) -> AbstractCommand:
+        return URRTDEMoveLCommand(self._logger, self._robot, q, speed, acceleration)
+
+
+class URRTDEPlugin(AbstractPlugin):
+    plugin_name = "ur_rtde"
+
+    def __init__(self):
+        self._logger = logging.getLogger("plugin").getChild("ur_rtde")
+
+        self._robot = CombinedRobotInterface(Config.ROBOT_ADDRESS)
+        logging.info(f"Connected to robot at {Config.ROBOT_ADDRESS}")
+
+    def load_compilers(self) -> Dict[str, AbstractCommandCompiler]:
+        return {
+            "moveL": URRTDEMoveLCompiler(self._logger, self._robot),
+            "moveJ": URRTDEMoveJCompiler(self._logger, self._robot)
+        }
+
+    def close(self):
+        self._robot.close()
+        logging.info(f"Disconnected from robot")