diff --git a/maniunicon/core/robot.py b/maniunicon/core/robot.py index 3949735..c2a759d 100644 --- a/maniunicon/core/robot.py +++ b/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.""" diff --git a/maniunicon/policies/gello.py b/maniunicon/policies/gello.py index fa0b68e..415941e 100644 --- a/maniunicon/policies/gello.py +++ b/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.""" diff --git a/maniunicon/robot_interface/franka_fr3_franky.py b/maniunicon/robot_interface/franka_fr3_franky.py index 40e9c00..d920ff6 100644 --- a/maniunicon/robot_interface/franka_fr3_franky.py +++ b/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: diff --git a/maniunicon/robot_interface/ur5_robotiq.py b/maniunicon/robot_interface/ur5_robotiq.py index c11d2cf..4fbbfe4 100644 --- a/maniunicon/robot_interface/ur5_robotiq.py +++ b/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: diff --git a/maniunicon/robot_interface/xarm6_robotiq.py b/maniunicon/robot_interface/xarm6_robotiq.py index da6ad67..0dae511 100644 --- a/maniunicon/robot_interface/xarm6_robotiq.py +++ b/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: diff --git a/maniunicon/utils/shared_memory/shared_memory_queue.py b/maniunicon/utils/shared_memory/shared_memory_queue.py index 54dd0b8..3a107b9 100644 --- a/maniunicon/utils/shared_memory/shared_memory_queue.py +++ b/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: diff --git a/maniunicon/utils/shared_memory/shared_memory_ring_buffer.py b/maniunicon/utils/shared_memory/shared_memory_ring_buffer.py index eac9f0f..7b7b15e 100644 --- a/maniunicon/utils/shared_memory/shared_memory_ring_buffer.py +++ b/maniunicon/utils/shared_memory/shared_memory_ring_buffer.py @@ -4,6 +4,7 @@ import numbers import time from multiprocessing.managers import SharedMemoryManager +from multiprocessing import Lock import numpy as np from maniunicon.utils.shared_memory.shared_ndarray import SharedNDArray @@ -15,8 +16,11 @@ class SharedMemoryRingBuffer: """ - A Lock-Free FILO Shared Memory Data Structure. + A thread-safe LIFO Shared Memory Ring Buffer. 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__( @@ -71,6 +75,9 @@ def __init__( ) timestamp_array.get()[:] = -np.inf + # Create lock for thread-safe write access + self._write_lock = Lock() + self.buffer_size = buffer_size self.array_specs = array_specs self.counter = counter @@ -125,45 +132,46 @@ def clear(self): def put( self, data: Dict[str, Union[np.ndarray, numbers.Number]], wait: bool = True ): - count = self.counter.load() - next_idx = count % self.buffer_size - # Make sure the next self.get_max_k elements in the ring buffer have at least - # self.get_time_budget seconds untouched after written, so that - # get_last_k can safely read k elements from any count location. - # Sanity check: when get_max_k == 1, the element pointed by next_idx - # should be rewritten at minimum self.get_time_budget seconds later. - timestamp_lookahead_idx = (next_idx + self.get_max_k - 1) % self.buffer_size - old_timestamp = self.timestamp_array.get()[timestamp_lookahead_idx] - t = time.monotonic() - if (t - old_timestamp) < self.get_time_budget: - deltat = t - old_timestamp - if wait: - # sleep the remaining time to be safe - time.sleep(self.get_time_budget - deltat) - else: - # throw an error - past_iters = self.buffer_size - self.get_max_k - hz = past_iters / deltat - raise TimeoutError( - "Put executed too fast {}items/{:.4f}s ~= {}Hz".format( - past_iters, deltat, hz + with self._write_lock: + count = self.counter.load() + next_idx = count % self.buffer_size + # Make sure the next self.get_max_k elements in the ring buffer have at least + # self.get_time_budget seconds untouched after written, so that + # get_last_k can safely read k elements from any count location. + # Sanity check: when get_max_k == 1, the element pointed by next_idx + # should be rewritten at minimum self.get_time_budget seconds later. + timestamp_lookahead_idx = (next_idx + self.get_max_k - 1) % self.buffer_size + old_timestamp = self.timestamp_array.get()[timestamp_lookahead_idx] + t = time.monotonic() + if (t - old_timestamp) < self.get_time_budget: + deltat = t - old_timestamp + if wait: + # sleep the remaining time to be safe + time.sleep(self.get_time_budget - deltat) + else: + # throw an error + past_iters = self.buffer_size - self.get_max_k + hz = past_iters / deltat + raise TimeoutError( + "Put executed too fast {}items/{:.4f}s ~= {}Hz".format( + past_iters, deltat, hz + ) ) - ) - - # write to shared memory - for key, value in data.items(): - if value is None: - continue - 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 timestamp - self.timestamp_array.get()[next_idx] = time.monotonic() - self.counter.add(1) + # write to shared memory + for key, value in data.items(): + if value is None: + continue + 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 timestamp (use the same time for consistency) + self.timestamp_array.get()[next_idx] = t + self.counter.add(1) def _allocate_empty(self, k=None): result = dict()