Close&Safe: Maximal Task Preservation for Redundant Manipulators via C-Space Boundary Detour
Authors: Abhigyan Singh, Manthan Goyal, Fares Abu-Dakka, Ravi Prakash
Organizations: Indian Institute of Science (IISc), Bengaluru, India. · Indian Institute of Technology Ropar, Rupnagar, India. · New York University Abu Dhabi, Abu Dhabi, UAE.
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.
Figures & tables
Task Requirement
Trajectory Opt. [ 1 , 2 , 3 , 4 , 5 ]
Sampling-based [ 6 , 7 , 8 , 9 ]
Reactive Repulsion [ 10 , 11 , 12 ]
Per-instant IK [ 13 , 14 , 15 , 16 , 17 ]
Ours
Exact tracking where collision-free
∘
—
∘
∘
✓
Deviation confined to blocked segment
—
—
✓
✓
✓
Minimal task-space deviation
∘
—
—
∘
✓
Escapes concave C-space obstacles
∘
✓
—
—
✓
Fallback when redundancy exhausted
∘
✓
—
—
✓
TABLE I: Requirements imposed by a prescribed-trajectory manipulation task, and the extent to which each family of obstacle-avoidance methods satisfies them. Families are distinguished by decision horizon rather than solver type: per-instant IK and trajectory optimization are both optimization-based, and differ in whether avoidance is resolved at a single control instant or across the whole horizon. ✓ = satisfied by construction; ∘ = satisfied only conditionally, or traded off against a competing objective; — = not addressed by the formulation.
Definition (Symbol)
Value
C-obstacle boundary seed samples ( Ns )
100,000
Boundary-shell acceptance tolerance ( ϵ )
35 mm
Neighbors per node in the boundary road-map ( k )
14
Per-joint anisotropic C-space margin ( δ )
[1.45,0.90,0.75,1.10,0.40,0.85,0.40] rad
Swivel-manifold angular grid resolution ( G )
128
TABLE II: Hyperparameters used for the C-space avoidance repair pipeline, with their definitions and default values. These values are held fixed across every experiment unless that parameter is the subject of the ablation.
Fig. 2: The four benchmark scenarios. Top: executed trajectories rendered in the scene. Middle: minimum clearance to the obstacle set along the trajectory; the band below zero is penetration, so a curve entering it is inadmissible whatever its task preservation. Bottom: the checkpoints each method executed within tolerance, placed at their position along the nominal path, so a gap is task lost and its position says where it was lost. Lane colors in the bottom row are the colors used for the same methods in the rows above: nominal trajectory (red), our proposed Close&Safe (cyan), TrajOpt (green), CollisionIK (orange), Neural-JSDF QP (magenta), and Neural-JSDF MPPI (yellow). The nominal is drawn in red in the upper two rows as the reference and is in collision by construction. The hatched red region in the bottom row shows the trajectory violates the collision constraint.
Case 1 : Cleaning (vase)
Case 2 : Cleaning (arm)
Case 3 : Cleaning (vase + arm)
Case 4 : Spray painting
Method
TP (%)
Clr (mm)
TP (%)
Clr (mm)
TP (%)
Clr (mm)
TP (%)
Clr (mm)
Close&Safe (Ours)
68.4
+17.6
57.8
+13.1
52.4
+14.1
75.0
+13.7
TrajOpt [ 2 ]
54.4
+6.0
35.0 †
−55.4
27.2 †
−107.0
76.4
+5.4
Neural-JSDF (QP-IK) [ 15 ]
19.0 †
−27.5
22.0
+9.7
34.6
+2.2
75.8
+1.1
Neural-JSDF (MPPI) [ 15 ]
35.6
+3.5
23.4
+21.7
35.6
+1.6
38.0
+0.5
CollisionIK [ 13 ]
65.0
+24.1
22.4
+61.3
49.4
+31.2
75.2
+2.4
TABLE III: Task preservation ( TP , the percentage of the nominal stroke executed within tolerance; higher is better) and minimum signed clearance (positive = separation, negative = penetration) across the four benchmark scenarios. All baselines use hyperparameters obtained by per-scenario constrained Bayesian optimization (Optuna, 30 trials per method-scenario pair), maximizing TP subject to zero collision and to the Franka per-joint velocity, acceleration and jerk envelope. Entries marked † violate the collision constraint and are not admissible regardless of their TP .
Fig. 3: Case 2: Scanning on hardware. Top: the nominal sweep, from the back past the neck to the abdomen, with both arms lowered. Bottom: the same nominal executed with the left arm raised. The end-effector does not stall in the pocket between the head and the raised arm but passes around it and returns to the nominal, retaining 78% of the scan.
Seeds
Pass (%)
Median t (ms)
Mean t (ms)
1,000
87.0
9.9
18.4
2,500
92.5
7.1
12.5
5,000
96.2
5.8
9.8
10,000
99.0
7.4
10.6
25,000
99.9
15.0
18.2
100,000 ∗
100.0
30.9
36.0
TABLE IV: Effect of Sobol seed count on repair success and latency. ∗ Default configuration.
Property
10,000 seeds
100,000 seeds
Sobol pass rate
93.75%
100.00%
Pass rate (Mean ± SD)
98.62±2.59 %
100.00±0.00 %
Pass rate (Min / Max)
93.75% / 100.0%
100.0% / 100.0%
Median t (Mean ± SD)
8.5±1.9 ms
37.1±6.1 ms
TABLE V: Sensitivity to sampling stream (50 pseudo-random re-draws on 16 problems) against the deterministic Sobol set.
Department of Biomedical Engineering, National University of Singapore, Singapore 117583 · Peng Cheng Laboratory, Shenzhen 518057, China · School of Electrical and Electronic Engineering, Nanyang Technological University, Singapore 639798