added servo control

Author: Johannes Hechtl

extensions/rcs_ur5e/src/rcs_ur5e/hw.py

@@ -1,13 +1,13 @@
 import typing
 from dataclasses import dataclass
-
+import multiprocessing as mp
+from multiprocessing.shared_memory import SharedMemory
+import time
 import numpy as np
 import rtde_control
 import rtde_receive
-
 from rcs import common
-
-from . import robotiq_gripper
+from rcs_ur5e import robotiq_gripper
 
 
 @dataclass(kw_only=True)
@@ -16,8 +16,9 @@ class UR5eConfig(common.RobotConfig):
     gain: float = 300.0
     max_velocity: float = 1.0
     max_acceleration: float = 1.0
-    blocking: bool = True
     async_control: bool = False
+    max_servo_joint_step: float = 0.05
+    max_servo_cartesian_step: float = 0.01
 
     def __post_init__(self):
         super().__init__()
@@ -29,80 +30,233 @@ def __post_init__(self):
         super().__init__()
 
 
-class UR5e:  # (common.Robot): # should inherit and implement common.Robot, but currently there is a bug that needs to be fixed
-    def __init__(self, ip: str):
-        # self.ik = None common.RL(urdf_path=urdf_path)
-        # ik = rcs.common.Pin(
-        #     robot_cfg.kinematic_model_path,
-        #     robot_cfg.attachment_site,
-        #     urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
-        # )
-        self._config = UR5eConfig()  # with default values
-        self._config.robot_platform = common.RobotPlatform.HARDWARE
+# Define the shared memory size and name outside the class
+SHM_SIZE = 4 + 1 + 48 + 48 + 48 + 48
+SHM_NAME = "ur5e_control_shm"
+
+NO_MODE = 0
+JOINT_MODE = 1
+CARTESIAN_MODE = 2
 
+def _control_robot(shm_name: str, ip: str, stop_queue: mp.Queue, config_queue: mp.Queue) -> None:
+    """
+    Control loop for the robot, running in a separate process.
+    This function is a helper and not part of the class.
+    """
+    robot_config = UR5eConfig()
+    try:
+        # Initialize robot interfaces
         # TODO(j.hechtl): this is currently blocking if connection fails
-        self.ur_control = rtde_control.RTDEControlInterface(ip)
-        self.ur_receive = rtde_receive.RTDEReceiveInterface(ip)
+        ur_control = rtde_control.RTDEControlInterface(ip)
+        ur_receive = rtde_receive.RTDEReceiveInterface(ip)
+
+        if not ur_control.isConnected():
+            raise ConnectionError(f"Could not connect to UR5e at {ip}.")
+
+        # Attach to the shared memory segment
+        shm = SharedMemory(name=shm_name)
+        data_buffer = shm.buf
+        
+        # Define offsets for each field in the shared memory buffer
+        offset_mode = 0
+        offset_target_reached = offset_mode + 4
+        offset_joint_target = offset_target_reached + 1
+        offset_cartesian_target = offset_joint_target + 48
+        offset_joint_state = offset_cartesian_target + 48
+        offset_cartesian_state = offset_joint_state + 48
+
+        # Create numpy views on the shared memory buffer
+        joint_target_view = np.ndarray(
+            (6,), dtype=np.float64, buffer=data_buffer, offset=offset_joint_target
+        )
+        cartesian_target_view = np.ndarray(
+            (6,), dtype=np.float64, buffer=data_buffer, offset=offset_cartesian_target
+        )
+        joint_state_view = np.ndarray(
+            (6,), dtype=np.float64, buffer=data_buffer, offset=offset_joint_state
+        )
+        cartesian_state_view = np.ndarray(
+            (6,), dtype=np.float64, buffer=data_buffer, offset=offset_cartesian_state
+        )
 
+        print("Robot control process started.")
+
+        dt = 1.0/500.0  # 2ms
+
+        while stop_queue.empty():
+            if not config_queue.empty():
+                robot_config = config_queue.get()
+            t_start = ur_control.initPeriod()
+            mode = int.from_bytes(data_buffer[offset_mode:offset_target_reached], "little")
+
+            joint_state = np.array(ur_receive.getActualQ())
+            joint_state_view[:] = joint_state
+            
+            cartesian_state = np.array(ur_receive.getActualTCPPose())
+            cartesian_state_view[:] = cartesian_state
+
+            if mode == JOINT_MODE: 
+                diff = joint_target_view - joint_state_view
+                if np.max(np.abs(diff)) < 0.01:
+                    data_buffer[offset_target_reached] = 1
+                if np.max(np.abs(diff)) > robot_config.max_servo_joint_step:
+                    diff = (robot_config.max_servo_joint_step / np.max(np.abs(diff))) * diff
+                    target_q = (joint_state_view + diff).tolist()
+                else:
+                    target_q = joint_target_view.tolist()
+                ur_control.servoJ(
+                    target_q,
+                    robot_config.max_velocity,
+                    robot_config.max_acceleration,
+                    dt,
+                    robot_config.lookahead_time,
+                    robot_config.gain,
+                )
+                
+            elif mode == CARTESIAN_MODE: 
+                diff = cartesian_target_view - cartesian_state_view
+                if np.max(np.abs(diff)) < 0.0025:
+                    data_buffer[offset_target_reached] = 1
+                if np.max(diff) > robot_config.max_servo_cartesian_step:
+                    diff = (robot_config.max_servo_cartesian_step / np.max(diff)) * diff
+                    target_pose = (cartesian_state_view + diff).tolist()
+                else:
+                    target_pose = cartesian_target_view.tolist()
+                ur_control.servoL(
+                    target_pose,
+                    robot_config.max_velocity,
+                    robot_config.max_acceleration,
+                    dt,
+                    robot_config.lookahead_time,
+                    robot_config.gain,
+                )
+
+            ur_control.waitPeriod(t_start)
+                
+    except Exception as e:
+        print(f"Robot control process encountered an error: {e}")
+        
+    finally:
+        print("Robot control process is shutting down...")
+        if 'ur_control' in locals():
+            ur_control.servoStop()
+            ur_control.stopScript()
+            ur_control.disconnect()
+        if 'shm' in locals():
+            shm.close()
+
+class UR5e: # (common.Robot): # should inherit and implement common.Robot, but currently there is a bug that needs to be fixed
+    def __init__(self, ip: str):
+        self._config = UR5eConfig()
         self._config.robot_type = common.RobotType.UR5e
-
-        if not self.ur_control.isConnected():
-            raise ConnectionError(f"Could not connect to UR5e at {ip}. Please check the IP address and connection.")
+        self._ip = ip
+
+        # Initialize shared memory and communication queues
+        self._shm = SharedMemory(name=SHM_NAME, create=True, size=SHM_SIZE)
+        self._shm_buffer = self._shm.buf
+        self._stop_queue = mp.Queue()
+        self._config_queue = mp.Queue()
+
+        # Define numpy views on the shared memory buffer
+        self._offset_mode = 0
+        self._offset_target_reached = self._offset_mode + 4
+        self._offset_joint_target = self._offset_target_reached + 1
+        self._offset_cartesian_target = self._offset_joint_target + 48
+        self._offset_joint_state = self._offset_cartesian_target + 48
+        self._offset_cartesian_state = self._offset_joint_state + 48
+
+        self._joint_target_shm = np.ndarray((6,), dtype=np.float64, buffer=self._shm_buffer, offset=self._offset_joint_target)
+        self._cartesian_target_shm = np.ndarray((6,), dtype=np.float64, buffer=self._shm_buffer, offset=self._offset_cartesian_target)
+        self._joint_state_shm = np.ndarray((6,), dtype=np.float64, buffer=self._shm_buffer, offset=self._offset_joint_state)
+        self._cartesian_state_shm = np.ndarray((6,), dtype=np.float64, buffer=self._shm_buffer, offset=self._offset_cartesian_state)
+
+        # Initialise with -10 to check
+        self._joint_state_shm[:] = -10
+
+        # Start the robot control process
+        self._robot_process = mp.Process(
+            target=_control_robot, 
+            args=(SHM_NAME, self._ip, self._stop_queue, self._config_queue)
+        )
+        self._robot_process.daemon = True # Kills process if main process exits
+        self._robot_process.start()
+
+        # Check for first update
+        while self._joint_state_shm[0]==-10:
+            print("Waiting for first robot state to arrive..")
+            time.sleep(1)
+        print("Robot Connection established.")
+
+
+    def __del__(self):
+        """Ensures resources are cleaned up when the object is garbage collected."""
+        self.stop_control_process()
+    
+    def stop_control_process(self):
+        """Safely stops the robot control process."""
+        if self._robot_process.is_alive():
+            self._stop_queue.put("STOP")
+            self._robot_process.join(timeout=5)
+            if self._robot_process.is_alive():
+                self._robot_process.terminate()
+            self._shm.close()
+            self._shm.unlink()
+            print("UR5e control process stopped and shared memory unlinked.")
 
     def get_cartesian_position(self) -> common.Pose:
-        ur_pose = self.ur_receive.getActualTCPPose()
-
-        trans = [ur_pose[0], ur_pose[1], ur_pose[2]]
+        ur_pose = self._cartesian_state_shm
+        trans = ur_pose[0:3]
         rotvec = common.RotVec(np.array(ur_pose[3:6]))
-
         pose = common.Pose(quaternion=rotvec.as_quaternion_vector(), translation=trans)
-
         pose = common.Pose(rpy_vector=[0, 0, np.deg2rad(180)], translation=[0, 0, 0]).inverse() * pose
-        print(f"Current: {pose.xyzrpy()}")
         return pose
 
     def get_ik(self) -> common.IK | None:
-        # TODO return ik object with values from robot
         return None
 
-    def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[6]], np.dtype[np.float64]]:  # type: ignore
-        return np.array(self.ur_receive.getActualQ())
+    def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[6]], np.dtype[np.float64]]:
+        return np.array(self._joint_state_shm)
 
     def get_parameters(self) -> UR5eConfig:
         return self._config
 
     def set_parameters(self, robot_cfg: UR5eConfig) -> None:
         self._config = robot_cfg
+        self._config_queue.put(robot_cfg)
 
     def get_state(self) -> UR5eState:
         return UR5eState
 
+    def set_cartesian_position(self, pose: common.Pose) -> None:
+        target_pose = (common.Pose(rpy_vector=[0, 0, np.deg2rad(180)], translation=[0, 0, 0]) * pose).rotvec()
+        self._cartesian_target_shm[:] = target_pose
+        self._shm_buffer[self._offset_mode:self._offset_target_reached] = (CARTESIAN_MODE).to_bytes(4, "little")
+        if not self._config.async_control:
+            while not self._shm_buffer[self._offset_target_reached]:
+                time.sleep(0.01)
+
+    def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[6]], np.dtype[np.float64]]) -> None:
+        self._joint_target_shm[:] = q
+        self._shm_buffer[self._offset_mode:self._offset_target_reached] = (JOINT_MODE).to_bytes(4, "little")
+        if not self._config.async_control:
+            while not self._shm_buffer[self._offset_target_reached]:
+                time.sleep(0.01)
+    
     def move_home(self) -> None:
         home = typing.cast(
             np.ndarray[tuple[typing.Literal[6]], np.dtype[np.float64]],
             common.robots_meta_config(common.RobotType.UR5e).q_home,
         )
-        # check if home position is between -2*pi and 2*pi
         if np.any((home < -2 * np.pi) | (home > 2 * np.pi)):
             raise ValueError(f"Home position {home} is out of bounds.")
         print(f"Moving to home position: {home}")
-        asynchronous = not self._config.blocking
-        self.ur_control.moveJ(home, self._config.max_velocity, self._config.max_acceleration, asynchronous)
+        self._joint_target_shm[:] = home
+        self._shm_buffer[self._offset_mode:self._offset_target_reached] = (JOINT_MODE).to_bytes(4, "little")
+        while not self._shm_buffer[self._offset_target_reached]:
+            time.sleep(0.01)
 
     def reset(self) -> None:
-        self.ur_control.stopL()
-
-    def set_cartesian_position(self, pose: common.Pose) -> None:
-        asynchronous = not self._config.blocking
-        print(f"target: {pose.xyzrpy()}")
-        pose = common.Pose(rpy_vector=[0, 0, np.deg2rad(180)], translation=[0, 0, 0]) * pose
-        target_pose = pose.rotvec()
-        self.ur_control.moveL(target_pose, self._config.max_velocity, self._config.max_acceleration, asynchronous)
-
-    def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[6]], np.dtype[np.float64]]) -> None:  # type: ignore
-        asynchronous = not self._config.blocking
-        self.ur_control.moveJ(q.tolist(), self._config.max_velocity, self._config.max_acceleration, asynchronous)
-
+        pass
 
 class RobotiQGripper:  # (common.Gripper):
     def __init__(self, ip):
@@ -114,6 +268,7 @@ def __init__(self, ip):
         self.gripper.activate()
         if not self.gripper.is_active():
             raise RuntimeError("Failed to activate Robotiq gripper.")
+        print("Gripper Connection established.")
 
     def get_normalized_width(self) -> float:
         # value between 0 and 1 (0 is closed)

extensions/rcs_ur5e/src/rcs_ur5e/test_robot.py

@@ -4,7 +4,7 @@
 
 ROBOT_IP = "192.168.25.201"
 
-robot = UR5e(ROBOT_IP, "sth")
+robot = UR5e(ROBOT_IP)
 print(f"Robot joint positions: {robot.get_joint_position()}")
 print(f"Robot cartesian position: {robot.get_cartesian_position()}")
 print(f"Robot Parameters: {robot.get_parameters()}")

extensions/rcs_ur5e/src/rcs_ur5e/ur5e_env_cartesian_control.py

@@ -27,7 +27,7 @@
 def main():
     if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
         robot_cfg = UR5eConfig()
-        robot_cfg.blocking = True
+        robot_cfg.blocking = False
         env_rel = RCSUR5eEnvCreator()(
             robot_cfg=robot_cfg,
             control_mode=ControlMode.CARTESIAN_TQuat,
@@ -67,9 +67,9 @@ def main():
         env_rel.get_wrapper_attr("sim").open_gui()
 
     obs, info = env_rel.reset()
-    print(obs)
-    print(env_rel.unwrapped.robot.get_cartesian_position())  # type: ignore
-    print(env_rel.unwrapped.robot.get_joint_position())  # type: ignore
+    # print(obs)
+    # print(env_rel.unwrapped.robot.get_cartesian_position())  # type: ignore
+    # print(env_rel.unwrapped.robot.get_joint_position())  # type: ignore
 
     for _ in range(100):
         # act = {"joints": common.robots_meta_config(common.RobotType.UR5e).q_home, "gripper": 0}
@@ -80,9 +80,9 @@ def main():
         # sleep(1)
         for _ in range(10):
             # move 1cm in x direction (forward) and close gripper
-            act = {"tquat": [0.01, 0, 0.0087265, 0, 0, 0, 0.9999619], "gripper": 0}
+            act = {"tquat": [0.01, 0,0, 0.0087265, 0, 0, 0.9999619], "gripper": 0}
             obs, reward, terminated, truncated, info = env_rel.step(act)
-            print(obs)
+            #print(obs)
             # print(env_rel.unwrapped.robot.get_cartesian_position())  # type: ignore
             if truncated or terminated:
                 logger.info("Truncated or terminated!")

extensions/rcs_ur5e/src/rcs_ur5e/ur5e_env_joint_control.py

@@ -8,6 +8,7 @@
 
 import rcs
 from rcs import sim
+from rcs_ur5e.hw import UR5eConfig
 
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
@@ -19,8 +20,10 @@
 def main():
 
     if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
+        robot_cfg = UR5eConfig()
         env_rel = RCSUR5eEnvCreator()(
             control_mode=ControlMode.JOINTS,
+            robot_cfg=robot_cfg,
             ip=ROBOT_IP,
             camera_set=None,
             max_relative_movement=np.deg2rad(5),