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.
Real-time collision avoidance for robotic manipulators requires fast reactions to unexpected obstacle motion and lookahead to avoid becoming trapped by near-future constraints. Full model predictive control can provide this foresight, but its online cost may grow quickly with horizon length, model fidelity, and the number of active geometric constraints. Conversely, horizon-free reactive methods are computationally efficient but can be short-sighted in dynamic clutter. We present a task-space receding-horizon controller that uses a short contact-consistent rollout to generate a terminal kinematic reference satisfying internal non-penetration constraints, then computes only the first input of a smooth minimum-acceleration transition toward that reference. Starting from a closed-loop inverse-kinematics regulation law, the rollout is performed with an iterative dynamics solver operating on inflated convex robot and obstacle geometries, so that robot-obstacle contacts, dynamic obstacle motion, and self-collisions can shape the terminal reference without requiring full constrained trajectory optimization. We analyze the contact-inactive closed loop and show local exponential task-space regulation under standard regularity assumptions. For contacts activated inside the rollout, we characterize the corresponding discrete updates and bound the effect of moving obstacles on regular operating sets. Simulations on a 40-DOF multi-chain system show that intermediate horizons balance anticipation, responsiveness, and computational cost. Hardware experiments on a 6-DOF platform demonstrate consistent sim-to-real behavior without accurate inertial parameter estimation, and comparisons against dynamic optimization fabrics and model predictive control (MPC) baselines show improved success rates in dynamic clutter while preserving solve times compatible with real-time execution in the tested regimes.
Mattia Penzotti, Marco Controzzi
Biorobotics Institute and the Department of Excellence in Robotics and AI, Scuola Superiore Sant’Anna, Pisa, Italy
Coding agents have emerged as a promising paradigm for robot manipulation: a language model writes the robot controller as a program, and agents built in this way now operate robots without robot-specific training. Whether this paradigm is also safe, however, has not been asked. We evaluate coding agents under a safety constraint, where each task pairs a manipulation goal with an obstacle the robot must not touch. The agent pursues the goal but collides with the obstacle in most cases, treating task completion as its sole objective. The agent reasons about the obstacle in its traces, and the prompt already forbids touching it, so neither perception nor instruction is at fault; the fault lies in the planning, where the stated constraint never becomes a priority. By decomposing manipulation into a route phase and a contact-rich moment, we locate the source of the failure. Along the route, the model cannot prioritize the safety constraint, having no notion of a clearing route and none of replanning once a chosen route becomes infeasible. At the contact, it is unaware that contact execution is bounded by the same constraint. To close this gap, we present SafeHarness, which equips the model with two obstacle-aware harnesses. Obstacle-aware route planning grounds the objects as bounding boxes and draws candidate routes over them as sequences of waypoints. The agent then plans a route in advance, verifies it, replans when necessary, and only then executes it. Obstacle-aware contact execution instead selects the contact position so that the contact itself avoids the obstacle. SafeHarness attains 81.2% task success and 91.9% collision avoidance with GPT-6-Astra, surpassing the previous SOTA by 13.7 and 23.0 points, and the same agent without harnesses by 31.2 and 57.5 points, respectively.
This paper proposes a task-oriented model predictive control (ToMPC) framework for safe and efficient robotic manipulation in open workspaces. The framework unifies collision-free motion and robot-environment interaction to address diverse scenarios. Additionally, it introduces task-oriented obstacle avoidance that leverages kinematic redundancy to enhance manipulation efficiency in obstructed environments. This complex optimization problem is solved by the alternating direction method of multipliers (ADMM), which decomposes the problem into two subproblems tackled by differential dynamic programming (DDP) and quadratic programming (QP), respectively. The effectiveness of this approach is validated in simulation and hardware experiments on a Franka Panda robotic manipulator. Results demonstrate that the framework can plan motion and/or force trajectories in real time, maximize the manipulation range while avoiding obstacles, and strictly adhere to safety-related hard constraints.
Xinyu Jia, Wenxin Wang, Jun Yang +2
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