Close&Safe: Maximal Task Preservation for Redundant Manipulators via C-Space Boundary Detour

Abhigyan Singh, Manthan Goyal, Fares Abu-Dakka, Ravi PrakasharXiv 2026 · arXiv

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):

  1. 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.
  2. 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:
  3. Corridor Sampling: It samples configurations in a narrow "corridor" around the blocked segment in the 7D configuration space (C-space).
  4. Boundary Roadmap: It filters these samples to keep only those near the boundary of the C-space obstacle and connects them into a graph.
  5. Least-Deviation Search: It uses A* to find a detour through this roadmap that minimizes task-space deviation.
  6. Outward Projection: It "pushes" the detour points away from the obstacle to ensure a safety margin and smooths the final trajectory.
The four stages of the CLOSE&SAFE detour process: sampling within a corridor, building a boundary roadmap, searching for the minimal deviation path, and projecting outward for safety.

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

Abstract

Contact-rich manipulation such as wiping, polishing and surface inspection specifies the task as a continuous end-effector trajectory rather than a goal pose. When an obstacle absent from the planning model intersects it, conventional planners return a collision-free path that may differ from the nominal throughout, discarding trajectory that was never obstructed. We present Close&Safe (Close to the intended task, Safe from obstacles), which decouples tracking from avoidance: the nominal is followed exactly wherever it remains feasible, and avoidance acts only on the interval where it is not, so tracking is hindered as little as the obstacle allows. On that interval we sample the configuration-space obstacle boundary within a corridor around the nominal, connect the samples into a roadmap, and search it for the detour that deviates least before rejoining the nominal at the earliest feasible configuration. We evaluate across diverse simulated obstacle configurations, physical Franka experiments, and ablations of key algorithmic components. The results demonstrate collision-free trajectory repair with localized task modification, while recovering 52.4-78% in simulation and on hardware.
BibTeX
@misc{singh2026close,
  author = {Abhigyan Singh and Manthan Goyal and Fares Abu-Dakka and Ravi Prakash},
  title = {{Close\&Safe: Maximal Task Preservation for Redundant Manipulators via C-Space Boundary Detour}},
  year = {2026},
  eprint = {2611.26031},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2611.26031},
}
Figure