style: format and lint

Author: juelg

extensions/rcs_realsense/src/rcs_realsense/camera.py

@@ -1,9 +1,9 @@
 import copy
 import logging
 import threading
-from time import sleep
 import typing
 from dataclasses import dataclass
+from time import sleep
 
 import numpy as np
 import pyrealsense2 as rs

extensions/rcs_ur5e/src/rcs_ur5e/hw.py

@@ -1,14 +1,16 @@
+import multiprocessing as mp
+import time
 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 rcs_ur5e import robotiq_gripper
 
+from rcs import common
+
 
 @dataclass(kw_only=True)
 class UR5eConfig(common.RobotConfig):
@@ -38,6 +40,7 @@ def __post_init__(self):
 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.
@@ -51,12 +54,13 @@ def _control_robot(shm_name: str, ip: str, stop_queue: mp.Queue, config_queue: m
         ur_receive = rtde_receive.RTDEReceiveInterface(ip)
 
         if not ur_control.isConnected():
-            raise ConnectionError(f"Could not connect to UR5e at {ip}.")
+            msg = f"Could not connect to UR5e at {ip}."
+            raise ConnectionError(msg)
 
         # 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
@@ -66,22 +70,14 @@ def _control_robot(shm_name: str, ip: str, stop_queue: mp.Queue, config_queue: m
         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
-        )
+        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
+        dt = 1.0 / 500.0  # 2ms
 
         while stop_queue.empty():
             if not config_queue.empty():
@@ -91,11 +87,11 @@ def _control_robot(shm_name: str, ip: str, stop_queue: mp.Queue, config_queue: m
 
             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: 
+            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
@@ -107,7 +103,7 @@ def _control_robot(shm_name: str, ip: str, stop_queue: mp.Queue, config_queue: m
                     target_q = joint_target_view.tolist()
 
                 # current_qd = ur_receive.getActualQd()
-                # required_qd = diff / dt 
+                # required_qd = diff / dt
                 # required_qdd = (required_qd - current_qd) / dt
 
                 # max_qdd = [1,1,1,1,1,1]
@@ -133,8 +129,8 @@ def _control_robot(shm_name: str, ip: str, stop_queue: mp.Queue, config_queue: m
                     robot_config.lookahead_time,
                     robot_config.gain,
                 )
-                
-            elif mode == CARTESIAN_MODE: 
+
+            elif mode == CARTESIAN_MODE:
                 rotvec = common.RotVec(np.array(cartesian_target_view[3:6]))
                 # print("Target View: ", cartesian_target_view)
                 a = common.Pose(quaternion=rotvec.as_quaternion_vector(), translation=cartesian_target_view[:3])
@@ -168,20 +164,21 @@ def _control_robot(shm_name: str, ip: str, stop_queue: mp.Queue, config_queue: m
                 )
 
             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():
+        if "ur_control" in locals():
             ur_control.servoStop()
             ur_control.stopScript()
             ur_control.disconnect()
-        if 'shm' in locals():
+        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
+
+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 = common.RL(urdf_path="/home/johannes/repos/rcs/assets/ur5e/urdf/ur5e.urdf")
         self._config = UR5eConfig()
@@ -202,33 +199,39 @@ def __init__(self, ip: str):
         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)
+        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)
+            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.daemon = True  # Kills process if main process exits
         self._robot_process.start()
 
         # Check for first update
-        while self._joint_state_shm[0]==-10:
+        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():
@@ -245,8 +248,7 @@ def get_cartesian_position(self) -> common.Pose:
         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
-        return pose
+        return common.Pose(rpy_vector=[0, 0, np.deg2rad(180)], translation=[0, 0, 0]).inverse() * pose
 
     def get_ik(self) -> common.IK | None:
 
@@ -275,42 +277,45 @@ def set_cartesian_position(self, pose: common.Pose) -> None:
         self._shm_buffer[self._offset_target_reached] = 0
         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")
+        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._shm_buffer[self._offset_target_reached] = 0
         self._joint_target_shm[:] = q
-        self._shm_buffer[self._offset_mode:self._offset_target_reached] = (JOINT_MODE).to_bytes(4, "little")
+        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,
         )
         if np.any((home < -2 * np.pi) | (home > 2 * np.pi)):
-            raise ValueError(f"Home position {home} is out of bounds.")
+            msg = f"Home position {home} is out of bounds."
+            raise ValueError(msg)
         print(f"Moving to home position: {home}")
         self._joint_target_shm[:] = home
-        self._shm_buffer[self._offset_mode:self._offset_target_reached] = (JOINT_MODE).to_bytes(4, "little")
+        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:
         pass
 
+
 class RobotiQGripper:  # (common.Gripper):
     def __init__(self, ip):
         self.gripper = robotiq_gripper.RobotiqGripper()
         try:
             self.gripper.connect(ip, 63352, socket_timeout=3.0)  # default port for Robotiq gripper
         except Exception as e:
-            raise RuntimeError(f"Failed to connect to Robotiq gripper at {ip}: {e}")
+            msg = f"Failed to connect to RobotiQ gripper at {ip}"
+            raise RuntimeError(msg) from e
         if not self.gripper.is_active():
             self.gripper.activate()
         print("Gripper Connection established.")