Learning Fast, Tool aware Collision Avoidance for Collaborative Robots

TL;DR

This paper presents a real-time, tool-aware collision avoidance system for collaborative robots that adapts to changing tools and environments, outperforming traditional methods in dynamic settings.

Contribution

The work introduces a novel perception and control framework that accounts for tool variability and partial observability, enabling faster and more accurate collision avoidance.

Findings

Outperforms traditional collision avoidance methods in dynamic environments

Operates with sub-millisecond response time

Reduces computational cost by approximately 60%

Abstract

Ensuring safe and efficient operation of collaborative robots in human environments is challenging, especially in dynamic settings where both obstacle motion and tasks change over time. Current robot controllers typically assume full visibility and fixed tools, which can lead to collisions or overly conservative behavior. In our work, we introduce a tool-aware collision avoidance system that adjusts in real time to different tool sizes and modes of tool-environment interaction. Using a learned perception model, our system filters out robot and tool components from the point cloud, reasons about occluded area, and predicts collision under partial observability. We then use a control policy trained via constrained reinforcement learning to produce smooth avoidance maneuvers in under 10 milliseconds. In simulated and real-world tests, our approach outperforms traditional approaches (APF, MPPI) in dynamic environments, while maintaining sub-millimeter accuracy. Moreover, our system operates with approximately 60% lower computational cost compared to a state-of-the-art GPU-based planner. Our approach provides modular, efficient, and effective collision avoidance for robots operating in dynamic environments. We integrate our method into a collaborative robot application and demonstrate its practical use for safe and responsive operation.

Tables

Table 1. TABLE I: Reward and Cost Functions.

Rewards
1. Position tracking	$1.0-clip({|p_{EE}^{targ}-p_{EE}|}^2$, 0.0, 1.0)
2. Orientation tracking	$1.0-2.0\ast \arccos \left({\left(q_{EE}\cdot {\left(q_{EE}^{targ}\right)}^{-1}\right)}_w\right)$
3. Command smoothness 1	$-1.0\times {10}^{-3}{\left|\left|q_t^{des}-q_{t-1}^{des}\right|\right|}^2$
4. Command smoothness 2	$-1.0\times {10}^{-4}{\left|\left|q_t^{des}-2q_{t-1}^{des}+q_{t-2}^{des}\right|\right|}^2$
Costs
1. Collision	1.0 if the robot body collides
2. Tool region violation    (Protective mode only)	1.0 if any ESDF (corner pointi)
3. End-effector speed limit	1.0 if EE speed exceeds 0.5 $m/s$
4. Joint speed limit	1.0 if $\left|{\dot{q}}_{i,t}\right|>0.8\cdot \dot{q}{}_{i,\text{lim.}}$

Table 2. TABLE II: Collision rate (Coll.) and success rate (Succ.) for different methods under static and dynamic obstacle conditions. All values in %

Method		Static		0.2 m/s	0.4 m/s
Perception	Control	Succ.	Coll.	Coll.	Coll.
GT Pose	APF	100	0	2	18
	MPPI	100	0	6	56
Convex hull [11, 48]	APF	54	16	78	82
	MPPI	72	0	50	72
Learned SDF [18]	APF	96	4	8	38
	MPPI	100	0	16	65
Ours (Occupancy grid)	RL	100	0	6	36

Table 3. TABLE III: Comparison across different scene representations. All values in %

Method	Protective Mode			Engage Mode
	Succ.	Coll.	Tool Coll.	Succ.	Coll.
End‑to‑End	10.0	2.51	5.03	11.0	6.48
Autoencoder (AE)	47.7	0.86	1.27	59.3	1.68
Mapping + AE	54.1	1.06	2.26	72.8	2.44
Ours	67.3	0.42	1.31	82.5	0.82

Equations

$$V_{C_i}^{safe}(s_t)=c_i(s_t)+(1-c_i(s_t))\sum _{t^{\prime }>t}^T\mathbb{E}\left[{\gamma }^{t^{\prime }-t}{c}_i(s_{t^{\prime }})\right].$$

$$V_{C_i}^{safe}(s)=c_i(s)+(1-c_i(s))\mathbb{E}\left[\gamma V_{C_i}^{safe}(s^{\prime })\right]\in [0,1].$$

$$V_C^{safe}(s_t)=\max (V_{C_{body}}^{safe}(s_t),V_{C_{tool}}^{safe}(s_t))$$

$$J_R(\pi )=\mathbb{E}\left[\sum _{t=0}^{\infty }{\gamma }^{t}r(s_t,a_t,s_{t+1})\right],$$

$$\underset{\pi }{\max }{J}_R(\pi )\text{s.t.}\forall i\in \{1,\dots ,n\},J_{C_i}(\pi )\le {ϵ}_i,$$

Full text

I INTRODUCTION

The use of robotic manipulators in contact-rich, real-world applications is rapidly expanding, driven by recent advances in control strategies and hardware [1, 2]. These developments have broadened robot deployment into a wide range of settings, from industrial sites to in-home service applications. As robots become more integrated into everyday environments, ensuring safety is becoming critical—especially for collaborative and service robots that operate alongside humans and cannot be isolated behind protective barriers like industrial machines.

Most industrial systems rely on collision detection [3, 4] and compliance control [5, 6], responding only after a collision has occurred. While widely adopted [7], these approaches fall short in dynamic environments populated by humans. A common preventive measure is to define protective zones around the robot during the pre-impact phase [8]. Sensors like LiDAR or safety-rated scanners create ”red” zones to trigger safety stops and ”yellow” zones to reduce speed. However, these methods do not change trajectories according to obstacles and are limited because robots with high kinetic energy require large safety zones. Recent works have introduced more advanced perception systems capable of adapting trajectories in real time by measuring distances and maintaining safe separation [9, 10, 11, 12, 13, 14].

Existing approaches still face limitations in real-world applications. They often rely on fixed geometric parameters and cannot adapt to dynamic environments. They also overlook controlled contact, reducing effectiveness in applications like assembly and pick-and-place. Furthermore, their lack of robustness to occlusions and reliance on heuristics often increase system complexity.

In this work, we propose an adaptive collision avoidance system that provides two command modes for manipulator-environment interaction (see Fig. 1). Engage mode allows the tool (e.g., gripper) to make contact during active tasks, while Protective mode prevents all contact during transport phases. This tool-aware design enhances adaptability and enables safe and efficient operation in dynamic contact-rich settings.

I-A System Requirements

We define three key requirements for an effective collision avoidance system in real-world applications:

• •

Flexibility to Tool Settings: Robots use a wide range of End-effector (EE)s, so the system should adapt to varying tool shapes and operational modes in real time.

• •

Precision: The system should achieve high precision in nominal operations, ensuring consistent performance.

• •

Responsiveness and Reliability:

The system must operate with minimal delays to avoid unsafe conditions, remain interpretable, and remain stable under challenging conditions such as occlusions.

I-B System Overview

Considering the requirements above, we developed a collision avoidance system that combines the robustness of learning-based methods with the precision of a classical controller. An overview of our system is shown in Fig. 2. The user provides three inputs: the desired EE pose, the geometry of the tool attached to the robot and the interaction mode (Engage or Protective mode). In nominal conditions, the system solves Inverse Kinematics (IK) using a classical method. A learned safety critic continuously monitors the environment and evaluates the risk of future collisions. When the risk exceeds a defined threshold, the system switches to a Reinforcement Learning (RL) policy for reactive motion generation.

The first part of our system is an encoder-decoder model that processes point cloud data along with its previous occupancy prediction. The module has two outputs: occupancy grid and safety critic values [15] for both body and tool collision constraints (explained later).

The safety critic value estimates the likelihood of future constraint violations (collisions), based on Srinivasan et al. [15]. It outputs a scalar value between 0 and 1. When the safety critic value is below a threshold (0.8 in our implementation), the RL policy output is used as an initial guess for an iterative IK method for high-precision. Conversely, when the critic indicates a high risk, the policy output is directly used for reactive collision avoidance.

The policy uses the latent space of the encoder as its input. This state encapsulates the spatial information necessary for effective collision avoidance [16]. The collision avoidance policy operates at a frequency of 50 Hz and generates joint position target residuals.

I-C Contributions

Our key advancements are:

• •

Fast and Adaptive Collision Avoidance:

Our system adapts in real time to user inputs and robustly handles dynamic obstacles. Our learned models adapts their outputs based on tool geometry and modes. Its low computational cost enables fast, safe execution of contact-rich tasks.

• •

Seamless Integration with High Accuracy: To balance responsiveness and precision, a fast but less accurate RL policy is activated only under high collision risk.

• •

Robust Perception: Our learned perception model removes the need for explicit robot filtering [11], mapping [17], and hand-crafted occlusion heuristics. This results in a simpler implementation and enables reliable operation in dynamic, partially observable environments.

We validate our system through real-world experiments and qualitative evaluations. Our approach achieves high precision and reliability while operating an order of magnitude faster than state-of-the-art methods [13, 18, 19].

II RELATED WORK

We summarize related works in the areas relevant to our research.

II-A Learned Scene Representation

Our approach aligns with recent works that employ neural networks to process raw sensor data for scene representation [16, 20, 21, 22, 23]. These methods utilize encoder-decoder architectures to process exteroceptive measurements and use the resulting latent vectors as inputs for policy networks. Hoeller et al.[16] used a 3D Convolutional Neural Network (CNN) to handle noisy point clouds, producing a latent representation for navigation and terrain reconstruction. Similarly, Kulkarni et al.[20] developed a Deep Collision Encoder (DCE) to extract collision information from noisy depth images for quadrotors using an architecture inspired by variational autoencoders.

These works demonstrate that processing sensor measurements with neural networks to build implicit environmental maps, rather than relying on conventional mapping, leads to highly responsive and robust systems.

II-B Collision Avoidance

Traditional approaches to safe motion planning focus on finding collision-free geometric paths using methods such as Rapidly-exploring Random Tree (RRT) [24] and Artificial Potential Field (APF) [25]. Subsequent research has explored Trajectory Optimization (TO) techniques, like CHOMP [26].

Recent research frames robot navigation as a global optimization problem. To handle complex environments with multiple possible solutions, sampling-based approaches are employed. For example, Curobo [13] combines sampling-based planning and TO for robotic manipulators.

These model-based methods generally perform well in fully observable environments where obstacles and dynamics are predictable. However, they struggle in dynamic, partially observable environments. Occlusions and the need for fast replanning make reliable collision avoidance challenging in real-world settings.

Recently, data-driven approaches, particularly those using RL, have emerged as alternatives. These methods allow robots to learn safe maneuvers by quickly responding to dynamic environment [27, 28, 20, 29].

II-C Safe Reinforcement Learning

Safe RL is a well-established research area, with various approaches differing in their architectural frameworks [30]. One widely used approach is the Conditional Value at Risk (CVaR) formulation [31, 32, 33]. This method focuses on maximizing the expected return in the lower tail of the return distribution, making it effective for addressing rare but critical events. However, we opted not to adopt this approach due to the complexity involved in implementing distributional critics and tuning risk parameters [34], leaving it as a consideration for future work.

In this work, we adopt the Constrained Markov Decision Process (CMDP) framework for its simplicity in implementation and deployment, leveraging its similarity to the widely used PPO algorithm. The CMDP framework [35] offers a structured and efficient approach to incorporating constraints into RL. By defining constraints as cost functions, CMDPs allow the optimization of policies that balance safety and performance [36, 37]. These methods efficiently manage multiple constraints. Kim et al.[38] and Lee et al.[39] demonstrated the utility of CMDPs in training safe locomotion policies for quadrupedal robots on rough terrain.

III METHOD

We provide an overview of our system and describe each component in detail.

III-A Problem Definition

Figure 3A illustrates the target environment. We focus on 2.5D environments, where obstacles are primarily oriented in the vertical ($z$) direction with varying heights. We consider random static and dynamic obstacles, replicating typical scenarios where manipulators move around machinery, workpieces, and human workers.

Scenarios involving deep cavity insertion or complex, occluded shapes are excluded, as they exceed the sensing capabilities of fixed cameras and are beyond the scope of this work.

We consider various tool shapes attached to the EE by defining a tool region and simulating point cloud within the region as illustrated in Fig. 3B.

III-B Safety Critic

We follow the approach by Srinivasan et al. [15], which defines the discounted probability of future constraint violations. The optimal safety critic for the $i$-th constraint estimates the following expectation of the nominal controller:

[TABLE]

As specified in Table. I, the cost function $c_{i}$ is an indicator function in our setup. The cumulative discounted probability of violations is estimated using Bellman equation:

[TABLE]

We train it via value iteration during the encoder-decoder model training.

During the deployment, we aggregate safety critics to evaluate the overall collision risk. The maximum safety value between the robot body and the tool area is used as

[TABLE]

where $V^{safe}_{C_{body}}$ represents the value of the robot’s body, and $V^{safe}_{C_{tool}}$ represents that of the tool constraint (see III-C3).

III-C Constrained Policy Optimization

III-C1 Background

A control problem is typically modeled as a Markov Decision Process (MDP) in RL framework, which is defined by a tuple $(S,A,r,p,\mu)$. $S$ is the set of states, $A$ is the set of actions, $r:S\times A\times S\rightarrow\mathbb{R}$ is the reward function, $p:S\times A\times S\rightarrow[0,1]$ is the state transition probability and $\mu$ is the initial state distribution. The aim of RL is to find a policy $\pi:S\mapsto\mathcal{P}(A)$ that maximizes

[TABLE]

where $\gamma\in[0,1)$ is the discount factor. Here, $\mathbb{E}[\ldots]$ represents the average over a finite batch of samples. $s_{0}$ is sampled from an initial state distribution $\mu$ and trajectories are sampled using $\pi$.

This is extended into CMDP to handle physical constraints, augmented with a set $C$ of cost functions that capture constraint violations $\{c_{1},\ldots,c_{n}\}$ and corresponding limits $E=\{\epsilon_{1},\epsilon_{2},\ldots,\epsilon_{n}\}$ [40]. Each $c_{i}:S\times A\times S\rightarrow\mathbb{R}$ maps ($s_{t},a_{t},s_{t+1}$) to the cost of the transition. An optimal policy maximizes $J_{R}$, while keeping the discounted sum of future costs $c_{i}$ below their respective threshold $\epsilon_{i}$, yielding the constrained optimization problem:

[TABLE]

where $J_{C_{i}}(\pi)=\mathbb{E}\left[\sum_{t=0}^{\infty}\gamma^{t}c_{i}(s_{t},a_{t},s_{t+1})\right]$.

This framework has been shown to be effective for high-dimensional control problems in robotics in recent works [39, 38]. Following the previous works, we employ the Penalized Proximal Policy Optimization (P3O) algorithm.

III-C2 CMDP for collision avoidance

Here we define the action and observation spaces, as well as the reward and cost functions for our CMDP. The state transition probabilities follow the rigid body dynamics of the robot.

The action space is defined by the residual changes to the robot’s joint positions: $q_{t}^{des}=q_{t}+a_{t}$ where $a_{t}$ represents the action at time $t$ and $q_{t}$ represents current joint angles. Action distributions are modeled using a Gaussian distribution. The observation space includes the previous joint position history, the latent state from the voxel encoder, and the target EE pose, represented in Cartesian coordinates and euler angles in the base frame.

The reward and cost functions are outlined in Table I. The reward function encourages EE pose tracking while also regularizing the smoothness of the robot’s movements. The cost functions are defined as indicator functions.

III-C3 Variable Tool Region Constraint

The tool region, defined as a bounding box attached to the EE (Fig. 2A-ii), is randomized in each episode. We evaluate the Euclidean Signed Distance Function (ESDF) values at the eight corner points and flag a constraint violation if any value is less than half the voxel width.

III-D Model Implementation

III-D1 Architecture

The architecture is shown in Fig. 3. A voxelized point cloud is processed by three 3D CNN layers and concatenated with proprioceptive inputs–including joint state history and user commands–before passing through a Multi Layer Perceptron (MLP) to produce a latent vector. Joint states help filter out the robot’s own body and assess collision risk.

III-D2 Scenario Setup

All training data was generated in simulation using approximate robot and workspace geometry (Fig. 3C), without relying on precise CAD models.

To define a new setup, users have to specify the workspace dimensions (x, y, z), grid resolution, sampling ranges for target EE poses, and a range of tool region parameters, including size and offsets. Then the obstacles and tool bounding boxes are randomized each episode.

III-D3 Training

We first collect data for training the encoder–decoder and safety critic by running a nominal controller (differential IK) in simulation. Occlusion labels are assigned to grid cells where the ESDF value is less than half the grid resolution. These components are trained using supervised learning and value iteration.

The policy is then optimized using P3O [36], with the encoder–decoder and safety critic frozen to enable stable and efficient learning based on fixed perception and safety estimates in simulation.

IV EXPERIMENTAL RESULTS

We present real-world experiments and qualitative evaluations to validate each component of our system. See Supplementary video for our main result.

IV-A Experimental Setup

The collision avoidance behavior and task-space position tracking performance of our system are evaluated using the setup illustrated in Fig. 4A. We used the Indy7 robot, which is a collaborative robotic arm with six degrees of freedom and is equipped with a two-finger gripper. The depth camera, an Intel RealSense D435, was mounted on the side of the table, as depicted in the figure. The wrist camera in the picture is not used for collision avoidance.

To assess the system’s robustness in the presence of occlusions, we use a single point cloud source with a limited field of view (Realsense D435). This allows us to evaluate the system under realistic sensing constraints.

The robot operates within a workspace measuring 0.6 m ($x$-axis) by 1.0 m ($y$-axis) by 0.6 m ($z$-axis). Within this workspace, we construct a voxel grid with 0.05 m resolution.

IV-B Use-case Example: Pick-and-Place

Fig. 1 illustrates a pick-and-place system where our collision avoidance system is integrated. The supplementary video demonstrates this behavior in action. We use a finite state machine to switch between picking and placing operations. For grasp planning, we used ANY-grasp [41]. The Engage mode is activated when a grasp pose is detected by ANY-grasp. Protective mode is used otherwise. This setup demonstrates how our approach can be incorporated into existing pipelines.

IV-C Computation Time

Our system operates at 50 Hz. The average update time for each action is approximately 6 ms, enabling the system to scale for deployment at frequencies exceeding 100 Hz. All tests were performed on a desktop PC equipped with an RTX 3060 GPU (12 GB), an Intel i7-12700F CPU, and 32 GB of RAM.

Model inference averages 1.3 ms (SD 0.5 ms, max 4.0 ms), performing a single-pass evaluation of the encoder-decoder, policy, and safety critic. Other processes, including solving kinematics, communication, and observation computation, take 3.9 ms (SD 3.9 ms, max 19.0 ms). Point cloud processing—implemented with Warp [42]—runs in parallel and averages 7.9 ms (SD: 3.7 ms, max: 72.3 ms) for voxelization, occupancy computation, and raycasting.

IV-D Analyzing Adaptive Behavior

A static obstacle was placed at the center of the workspace to analyze obstacle avoidance behavior, following a setup similar to [43]. The robot was tasked with moving its EE from the left to an endpoint on the right.

IV-D1 Interaction Mode Behaviors

Fig. 4B–D illustrate the trajectories executed by the robot in each mode. In Protective mode, the robot accounts for the tool region, resulting in longer detours to avoid potential collisions.

IV-D2 Safety Critic Values

Fig. 4E shows the cost values by the safety critic, calculated as the maximum of the body cost and the tool cost value. The controller switches to the RL policy when the cost value exceeds a predefined threshold (0.8) and reverts to the nominal controller once the cost decreases.

IV-D3 Estimated Collidable Region

Fig. 5 shows the raw point cloud and the estimated occupancy during the experiment in Fig. 4. When the gripper is at the left side, the table is significantly occluded (Fig. 5A,B-i). Our model remembers the measurements at the initial configuration and preserves the occupancy prediction.

Our model filters out measurements corresponding to the robot’s body and tool components, such as the gripper, camera, and cables. This capability is achieved by incorporating randomly simulated tool geometry during training.

IV-D4 Adaptation to Tool Shape

Fig. 6A-C depict the task-space position tracking performance in the $yz$ plane for three different EE configurations. Each target position was maintained for three seconds after convergence, and the average tracking error was computed. The experiment follows the setup shown in Fig. 4A, with errors measured via forward kinematics.

As collision avoidance is activated, higher tracking errors are observed near the table and obstacle. In contrast, open areas consistently exhibit errors around 0.01 mm. As larger tool areas were commanded, the high-error region expands (Fig. 6B and Fig. 6C). This demonstrates the system’s ability to adaptively predict potential collisions based on tool dimensions and behave accordingly.

IV-E Validation of the Control Strategy

Fig. 6D illustrates the performance of RL alone, where tracking errors in the safe area are significantly higher compared to ours (Fig. 6A). This highlights the inherent limitations of RL in achieving the precision offered by traditional IK methods.

We also compared our system to Curobo [13], a state-of-the-art GPU-accelerated TO framework (Fig. 6E). The results are similar: Curobo also reports no feasible solution (marked as N/A) in obstacle regions. In collision-free regions, our method achieves lower tracking error because of the refinement using iterative IK.

Our system requires fewer computational resources overall, as shown in Fig. 7. The average planning time for Curobo is 468 ms (SD 244 ms, max 1.36 s), whereas our method operates within 10 ms. In terms of memory usage, ours requires only 0.45 GB of GPU memory, compared to approximately 4.5 GB for Curobo when running with NvBlox. It is important to note that this does not include voxel grid mapping and other system components.

While global optimizers [13, 44] excel at long-horizon planning under full observability, our lightweight system is better suited for reactive, short-horizon collision avoidance—especially when computational resources are shared with high-level processes.

IV-F Evaluation in Dynamic Settings

To assess the reliability of our system, we evaluate its performance in dynamic environments with moving obstacles and compare it against widely used baseline methods.

IV-F1 Experimental Setup

As shown in Fig. 8A, the EE is commanded to move from right to left at a speed of 0.25 m/s. For the dynamic case, an obstacle is spawned on the left side of the workspace and moves at a fixed linear velocity toward the center of the EE target trajectory. In the static case, the obstacle is fixed at the center. Each method is evaluated over 50 trials.

All controllers operate at 50 Hz; to ensure consistent timing, the simulation pauses each step until computation is complete.

IV-F2 Baselines

We validated our control strategy against two baseline approaches with different perception methods:

• •

APF: A classical reactive path planning method [25].

• •

Model Predictive Path Integral** (MPPI)**: A sampling-based Model Predictive Control (MPC) method [13, 18, 45]. We followed STORM [19] implementation with a 0.5 s horizon and 4000 samples.

Traditional approaches often preprocess point clouds by clustering and approximating obstacles as ellipsoids or convex hulls [46, 11]. More recent work has adopted learned ESDF or collision check for improved robustness and reactivity [18, 47], which aligns with our learning-based approach.

For evaluation, we implemented two perception variants for the baselines: (i) point cloud obstacle approximation using convex hulls [11, 48] combined with Kalman filter-based centroid tracking, and (ii) a learned SDF model [18].

IV-F3 Result

Table II summarizes the performance of our method compared to baseline approaches under varying obstacle speeds. GT denotes access to ground-truth obstacle position and shape.

Our method achieves performance comparable to APF with learned SDF [18]. Across all scenarios, approaches using learned perception demonstrate higher robustness to dynamic changes and partial observability.

APF occasionally fails in static settings due to local minima and may exhibit oscillatory behavior caused by sudden, high repulsive forces. MPPI shows strong performance in static scenarios but struggles with dynamic obstacles. Despite running at high frequency without delay, its trajectory samples requires time to converge to collision-free trajectories, which limits responsiveness.

The convex hull approximation approach suffers significantly from partial occlusions, as illustrated in Fig. 8B. In such cases, the obstacle is underestimated in size, leading to increased collision rates.

IV-F4 Scalability

Traditional methods involving point cloud clustering and filtering [46, 11] have a per-cycle runtime that grows with the number of segmented obstacles. By contrast, our method and learned approaches using the whole scene [18, 47] maintain a constant runtime.

IV-G Effectiveness of the Learned Perception Model for RL

We evaluated our perception approach, which uses an encoder-decoder model to reconstruct collision regions, filter out noise, and remove robot and tool areas from the voxel grid.

IV-G1 Baselines

We compared three methods:

• •

End-to-End Training: Training directly from scratch using the same RL algorithm without pretraining.

• •

Pretrained Autoencoder (AE): A 3D CNN is pretrained to reconstruct the input voxel grid, and its latent representation is used as input to the policy.

• •

Mapping with Autoencoder (Mapping+AE): This approach explicitly filters out robot and tool points from the raw point cloud, constructs a voxel-based occupancy grid, and uses it as input to the policy network. We integrated Voxblox [17] into the simulation, allowing the policy to use the voxel map as external memory.

IV-G2 Results

The results, summarized in Table III, are evaluated in both control modes in simulation. In Protective mode, tool shape is randomized. The tool violation rate reflects the percentage of episodes where tool constraints were violated. Metrics are computed from 20,000 episodes, with waypoints sampled from free space (ESDF $>$ 0.1) in the workspace. Each episode runs for 10 seconds.

End-to-end training with P3O yields suboptimal performance, resulting in overly conservative behavior that reduces success rates and increases collision frequency.

While the pretrained encoder (AE) improves over end-to-end training, it still leads to lower success rates and higher collision rates compared to our method.

Although the Mapping+AE approach achieves a high success rate, it struggles with perception uncertainties such as noise and occlusions. A key failure mode we observed is that robot and tool filtering occasionally removes nearby obstacle points when they are too close to the robot, resulting in unexpected collisions.

V CONCLUSION

We presented a fast and tool-aware collision avoidance system for collaborative robots. A use case example, shown in Fig. 1, highlights its applicability to realistic scenarios involving collaborative robots under practical sensing constraints. By integrating constrained RL with a traditional differential IK approach, our system achieves both robustness and high accuracy. It seamlessly adapts to diverse tool shapes and interaction modes, ensuring safe and reliable operation while supporting real-time performance exceeding 100 Hz

Compared to baseline methods such as APF and MPPI, our system consistently achieves lower collision rates and comparable or higher success rates in dynamic environments. Experimental results further show robust performance with control latency under 10 ms and GPU memory usage of only 0.45 GB—over 10 times more memory-efficient and significantly faster than Curobo’s TO [13].

Future work will focus on integrating the system into contact-rich and complex manipulation frameworks such as ALOHA [49] to study high-level intelligence. Additionally, we aim to extend the system to mobile manipulators and multi-robot scenarios operating in variable workspaces.

ACKNOWLEDGMENT

We appreciate Dayoung Kim for helping camera setup and calibration.