diff --git a/single_ursim_control/config.py b/single_ursim_control/config.py index 84fe3d5..f364352 100644 --- a/single_ursim_control/config.py +++ b/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") \ No newline at end of file diff --git a/single_ursim_control/main.py b/single_ursim_control/main.py index ce98c0e..dc9d43e 100644 --- a/single_ursim_control/main.py +++ b/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")) diff --git a/single_ursim_control/plugins/__init__.py b/single_ursim_control/plugins/__init__.py index 6b32910..aa84740 100644 --- a/single_ursim_control/plugins/__init__.py +++ b/single_ursim_control/plugins/__init__.py @@ -1,4 +1,5 @@ from .abstract_plugin import AbstractCommand, AbstractCommandCompiler, AbstractPlugin from .sleep_plugin import SleepPlugin from .sync_plugin import SyncPlugin -from .wait_plugin import WaitPlugin \ No newline at end of file +from .wait_plugin import WaitPlugin +from .ur_rtde_plugin import URRTDEPlugin \ No newline at end of file diff --git a/single_ursim_control/plugins/ur_rtde_plugin.py b/single_ursim_control/plugins/ur_rtde_plugin.py new file mode 100644 index 0000000..032e071 --- /dev/null +++ b/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")