fix: resolve critical safety and concurrency issues

maniunicon/core/robot.py

@@ -472,7 +472,10 @@ def run(self):
         self.robot_interface.stop()
         self.robot_interface.disconnect()
         if self.state_thread is not None:
-            self.state_thread.join()
+            # Use timeout to prevent indefinite blocking
+            self.state_thread.join(timeout=5.0)
+            if self.state_thread.is_alive():
+                print("Warning: State receiver thread did not terminate within timeout")
 
     def stop(self):
         """Stop the controller process and all threads."""

maniunicon/policies/gello.py

@@ -107,7 +107,24 @@ def _clip_joint_positions(
             Clipped joint positions
         """
         clipped_positions = target_positions.copy()
-        return clipped_positions  # temp disable
+
+        # Apply joint position limits if configured
+        if self.joint_limits is not None:
+            if "min" in self.joint_limits and self.joint_limits["min"] is not None:
+                min_limits = np.array(self.joint_limits["min"])
+                clipped_positions = np.maximum(clipped_positions, min_limits)
+            if "max" in self.joint_limits and self.joint_limits["max"] is not None:
+                max_limits = np.array(self.joint_limits["max"])
+                clipped_positions = np.minimum(clipped_positions, max_limits)
+
+        # Apply velocity limits to prevent sudden large movements
+        if current_positions is not None and self.joint_velocity_limit > 0:
+            delta = clipped_positions - current_positions
+            max_delta = self.joint_velocity_limit * self.dt
+            delta = np.clip(delta, -max_delta, max_delta)
+            clipped_positions = current_positions + delta
+
+        return clipped_positions
 
     def sync_state(self):
         """Sync the robot state with shared storage."""

maniunicon/robot_interface/franka_fr3_franky.py

@@ -113,12 +113,13 @@ def connect(self) -> bool:
             return False
 
     def disconnect(self) -> bool:
-        """Disconnect from the UR5 robot."""
+        """Disconnect from the Franka robot."""
         try:
             if self.robot is not None:
                 del self.robot
-                del self.gripper
                 self.robot = None
+            if self.gripper is not None:
+                del self.gripper
                 self.gripper = None
 
             self.ik_solver = None
@@ -326,8 +327,8 @@ def clear_error(self) -> bool:
             return False
 
         try:
-            self.robot_c.stopScript()
-            self.robot_c.unlockProtectiveStop()
+            if self.robot is not None:
+                self.robot.recover_from_errors()
             self._error_state = False
             return True
         except Exception as e:

maniunicon/robot_interface/ur5_robotiq.py

@@ -101,13 +101,15 @@ def reset_to_init(self) -> bool:
         if self.robot_c is not None:
             print("init!")
             self.robot_c.servoStop()
+            init_qvel = self.config.get("init_qvel", self.velocity)
             self.robot_c.moveJ(
                 self.config["init_qpos"],
-                self.config["init_qvel"],
+                init_qvel,
                 1.4,
             )
             self.robot_c.stopJ()
-            self.gripper.open()
+            if self.gripper is not None:
+                self.gripper.open()
             self.gripper_state = np.array([0.0])  # Gripper open state
             print("init finished!")
             return True
@@ -117,9 +119,10 @@ def move_to_joint_positions(self, joint_positions: np.ndarray) -> bool:
         if self.robot_c is not None:
             print(f"moving to joint positions {joint_positions}")
             self.robot_c.servoStop()
+            init_qvel = self.config.get("init_qvel", self.velocity)
             self.robot_c.moveJ(
                 joint_positions,
-                self.config["init_qvel"],
+                init_qvel,
                 1.4,
             )
             self.robot_c.stopJ()
@@ -279,7 +282,7 @@ def is_error(self) -> bool:
             # You can add specific UR5 error checking here
             # For example, check robot mode, safety status, etc.
             return self._error_state
-        except:
+        except Exception:
             return True
 
     def clear_error(self) -> bool:
@@ -288,8 +291,9 @@ def clear_error(self) -> bool:
             return False
 
         try:
-            self.robot_c.stopScript()
-            self.robot_c.unlockProtectiveStop()
+            if self.robot_c is not None:
+                self.robot_c.stopScript()
+                self.robot_c.unlockProtectiveStop()
             self._error_state = False
             return True
         except Exception as e:

maniunicon/robot_interface/xarm6_robotiq.py

@@ -163,13 +163,13 @@ def get_state(self) -> RobotState:
             try:
                 joint_velocities = np.array(self.arm.get_joint_speed()[1])
                 joint_velocities = np.deg2rad(joint_velocities)  # Convert to rad/s
-            except:
+            except (AttributeError, IndexError, TypeError):
                 joint_velocities = np.zeros_like(joint_positions)
 
             # Get joint torques (XArm API might not provide this directly)
             try:
                 joint_torques = np.array(self.arm.get_joint_torque()[1])
-            except:
+            except (AttributeError, IndexError, TypeError):
                 joint_torques = np.zeros_like(joint_positions)
 
             # Get TCP pose using IK solver
@@ -294,7 +294,7 @@ def is_error(self) -> bool:
                 if state[1] in [4, 5, 6]:
                     return True
             return self._error_state
-        except:
+        except Exception:
             return True
 
     def clear_error(self) -> bool:

maniunicon/utils/shared_memory/shared_memory_queue.py

@@ -2,6 +2,7 @@
 import numbers
 from queue import Empty, Full
 from multiprocessing.managers import SharedMemoryManager
+from multiprocessing import Lock
 import numpy as np
 
 from maniunicon.utils.shared_memory.shared_memory_util import (
@@ -13,8 +14,11 @@
 
 class SharedMemoryQueue:
     """
-    A Lock-Free FIFO Shared Memory Data Structure.
+    A thread-safe FIFO Shared Memory Data Structure.
     Stores a sequence of dict of numpy arrays.
+
+    Note: This implementation uses locks to ensure thread-safety for
+    concurrent multi-producer/multi-consumer access patterns.
     """
 
     def __init__(
@@ -27,6 +31,10 @@ def __init__(
         write_counter = SharedAtomicCounter(shm_manager)
         read_counter = SharedAtomicCounter(shm_manager)
 
+        # Create locks for thread-safe access
+        self._write_lock = Lock()
+        self._read_lock = Lock()
+
         # allocate shared memory
         shared_arrays = dict()
         for spec in array_specs:
@@ -86,67 +94,71 @@ def clear(self):
         self.read_counter.store(self.write_counter.load())
 
     def put(self, data: Dict[str, Union[np.ndarray, numbers.Number]]):
-        read_count = self.read_counter.load()
-        write_count = self.write_counter.load()
-        n_data = write_count - read_count
-        if n_data >= self.buffer_size:
-            raise Full()
-
-        next_idx = write_count % self.buffer_size
-
-        # write to shared memory
-        for key, value in data.items():
-            arr: np.ndarray
-            arr = self.shared_arrays[key].get()
-            if isinstance(value, np.ndarray):
-                arr[next_idx] = value
-            else:
-                arr[next_idx] = np.array(value, dtype=arr.dtype)
-
-        # update idx
-        self.write_counter.add(1)
+        with self._write_lock:
+            read_count = self.read_counter.load()
+            write_count = self.write_counter.load()
+            n_data = write_count - read_count
+            if n_data >= self.buffer_size:
+                raise Full()
+
+            next_idx = write_count % self.buffer_size
+
+            # write to shared memory
+            for key, value in data.items():
+                arr: np.ndarray
+                arr = self.shared_arrays[key].get()
+                if isinstance(value, np.ndarray):
+                    arr[next_idx] = value
+                else:
+                    arr[next_idx] = np.array(value, dtype=arr.dtype)
+
+            # update idx
+            self.write_counter.add(1)
 
     def get(self, out=None) -> Dict[str, np.ndarray]:
-        write_count = self.write_counter.load()
-        read_count = self.read_counter.load()
-        n_data = write_count - read_count
-        if n_data <= 0:
-            raise Empty()
+        with self._read_lock:
+            write_count = self.write_counter.load()
+            read_count = self.read_counter.load()
+            n_data = write_count - read_count
+            if n_data <= 0:
+                raise Empty()
 
-        if out is None:
-            out = self._allocate_empty()
+            if out is None:
+                out = self._allocate_empty()
 
-        next_idx = read_count % self.buffer_size
-        for key, value in self.shared_arrays.items():
-            arr = value.get()
-            np.copyto(out[key], arr[next_idx])
+            next_idx = read_count % self.buffer_size
+            for key, value in self.shared_arrays.items():
+                arr = value.get()
+                np.copyto(out[key], arr[next_idx])
 
-        # update idx
-        self.read_counter.add(1)
-        return out
+            # update idx
+            self.read_counter.add(1)
+            return out
 
     def get_k(self, k, out=None) -> Dict[str, np.ndarray]:
-        write_count = self.write_counter.load()
-        read_count = self.read_counter.load()
-        n_data = write_count - read_count
-        if n_data <= 0:
-            raise Empty()
-        assert k <= n_data
-
-        out = self._get_k_impl(k, read_count, out=out)
-        self.read_counter.add(k)
-        return out
+        with self._read_lock:
+            write_count = self.write_counter.load()
+            read_count = self.read_counter.load()
+            n_data = write_count - read_count
+            if n_data <= 0:
+                raise Empty()
+            assert k <= n_data
+
+            out = self._get_k_impl(k, read_count, out=out)
+            self.read_counter.add(k)
+            return out
 
     def get_all(self, out=None) -> Dict[str, np.ndarray]:
-        write_count = self.write_counter.load()
-        read_count = self.read_counter.load()
-        n_data = write_count - read_count
-        if n_data <= 0:
-            raise Empty()
-
-        out = self._get_k_impl(n_data, read_count, out=out)
-        self.read_counter.add(n_data)
-        return out
+        with self._read_lock:
+            write_count = self.write_counter.load()
+            read_count = self.read_counter.load()
+            n_data = write_count - read_count
+            if n_data <= 0:
+                raise Empty()
+
+            out = self._get_k_impl(n_data, read_count, out=out)
+            self.read_counter.add(n_data)
+            return out
 
     def _get_k_impl(self, k, read_count, out=None) -> Dict[str, np.ndarray]:
         if out is None: