TL;DR
CLOSE&SAFE is a novel trajectory repair framework that allows redundant robots to navigate unexpected obstacles while preserving as much of their original task as possible. By prioritizing null-space motion and searching for minimal detours specifically along the boundaries of configuration-space obstacles, it ensures the robot only deviates from its path when absolutely necessary and rejoins it at the earliest opportunity.
What Problem Does This Paper Address?
In many industrial and service tasks—like wiping a table, welding a seam, or scanning a surface—the path of the robot's tool is the task itself. If a robot encounters an unexpected obstacle (like a human hand or a misplaced jug), standard motion planners often discard the entire original trajectory in favor of a new, collision-free path. This results in "task loss": uncleaned spots, missed welds, or gaps in data.
Existing methods often struggle with this. Trajectory optimization might reshape parts of the path that weren't even blocked. Reactive "repulsion" methods can get stuck in local minima (like a U-shaped obstacle). The authors aim for maximal task preservation: the robot should follow the nominal path exactly wherever feasible and deviate only as much as the obstacle forces it to.
Methods
The authors propose a two-layer hierarchy called CLOSE&SAFE (Close to the intended task, Safe from obstacles):
- Null-space Redundancy Resolution: For robots with more than 6 degrees of freedom (like the 7-DOF Franka used in the paper), there are multiple joint configurations for a single end-effector pose. The first layer tries to find a "swivel" of the robot's joints that avoids the obstacle without moving the tool off the path.
- Localized C-Space Boundary Detour: If the path is truly blocked (irreducible interference), the system identifies the specific "blocked segment." It then performs a four-stage repair process:
- Corridor Sampling: It samples configurations in a narrow "corridor" around the blocked segment in the 7D configuration space (C-space).
- Boundary Roadmap: It filters these samples to keep only those near the boundary of the C-space obstacle and connects them into a graph.
- Least-Deviation Search: It uses A* to find a detour through this roadmap that minimizes task-space deviation.
- Outward Projection: It "pushes" the detour points away from the obstacle to ensure a safety margin and smooths the final trajectory.
Datasets
The method was evaluated using:
- Four Simulated Scenarios: Using a 7-DOF Franka robot performing cleaning and spray-painting tasks with various obstacles (vases, other robot arms).
- MotionBenchMaker: 800 narrow-passage problems were used for ablation studies to test the robustness of the C-space detour stage.
- Hardware Experiments: Real-world validation on a Franka Research 3 robot for cleaning and scanning tasks involving a mannequin and a jug.
Key Insights and Ideas
- Lexicographical Ordering over Weighting: Instead of a complex cost function where you "tune" the balance between safety and task accuracy, CLOSE&SAFE uses a strict hierarchy: Feasibility > Task Preservation > Minimal Detour. This eliminates the need to retune parameters for every new scene.
- The Boundary is the Key: The most efficient detour around an obstacle is often right against its edge. By focusing sampling on the C-space obstacle boundary, the algorithm finds tighter detours more quickly than general-purpose planners.
- Localized Repair: By isolating only the blocked segment for repair, the algorithm ensures that the rest of the trajectory—which might have been carefully authored or demonstrated—remains untouched.
Key Results
CLOSE&SAFE demonstrated superior performance against established baselines like TrajOpt and CollisionIK:
- Task Recovery: It recovered 52.4–78% of the nominal task across various scenarios. In "hard" cases with concave obstacles where baselines often stalled or collided, CLOSE&SAFE was the only method to consistently complete the full path.
- Precision: It maintained a very tight clearance band (within 4.5 mm of the safety margin) across all scenarios, whereas baselines either over-avoided (losing task area) or penetrated the obstacle.
- Hardware Success: On real hardware, it successfully navigated complex "pockets" of free space (e.g., between a mannequin's head and a raised arm) that would trap local reactive controllers.
Limitations and Future Work
- Spatial Focus: The current metric and repair focus on spatial preservation. While the path is re-timed using a minimum-jerk profile, it doesn't strictly preserve the original velocity or acceleration profile of a demonstration.
- Static Obstacle Assumption: The planning assumes obstacles are static during the detour calculation. While the system is fast enough (running at 25 Hz) to handle slowly changing environments, it isn't designed for highly dynamic obstacles like a moving ball.
- Empirical Parameters: A few parameters, such as the projection distance and sampling density, are still determined empirically. Future work could involve making these adaptive to the complexity of the C-space geometry.
Summary updated