Close-proximity multi-arm manipulation requires collision models that are both geometrically accurate and differentiable enough for real-time optimization. Classical geometry checkers provide reliable distances but are difficult to use inside gradient-based model predictive control, while conservative proxy models can restrict tightly coupled motion. We present PI-UDF, a physics-informed unified differentiable framework for body-to-body collision distance prediction between articulated robots. PI-UDF combines analytical forward kinematics with learnable link-geometry embeddings and a shared residual network to predict pairwise inter-arm distances directly from robot configurations. To improve safety-critical fidelity, we combine quota-driven boundary mining with an asymmetric boundary-crossing penalty that emphasizes false-safe sign errors near the collision boundary. The learned distance field is integrated into nonlinear MPC as a differentiable inter-arm clearance term. We validate the framework on a real dual-Franka platform through high-speed close-proximity 14-DoF dual-arm swapping, sustained single-arm dynamic evasion, and dynamic-evasion planning configurations with frozen, predicted, and target-switching treatments of the moving arm. Hardware experiments and offline Drake/FCL replay show that PI-UDF provides a differentiable inter-arm clearance estimate suitable for closed-loop collision-aware collaborative robot motion generation.
Figures & tables
Fig. 1 : Overview of the PI-UDF framework. (a) Physics-informed Body-to-Body SDF provides differentiable inter-arm clearance and usable gradients for optimization. (b) The Boundary-Crossing Penalty (BCP) emphasizes false-safe boundary violations while keeping conservative safe-state errors less costly. (c) The learned distance field is used as an inter-arm clearance term in NMPC for close-proximity motion generation. Representative frames during close-proximity dual-arm crossing and subsequent separation are shown.
Fig. 2 : Overview of the PI-UDF architecture. Given an inter-arm link pair (i,j) , analytical forward kinematics computes the world-frame link poses 0T1,i(q1) and 0T2,j(q2) , from which the relative transform Tij(q) is constructed and vectorized as vpose . The fixed link geometries are represented by robot-specific learnable embedding-table lookups e1,i=E(1)[i] and e2,j=E(2)[j] , forming the implicit geometry descriptor vgeo . A shared residual regressor fuses both streams to predict the pairwise signed distance d^ij .
Fig. 3 : Configuration-level distance-zone distribution. Each retained dual-arm configuration is assigned to a zone by its minimum inter-arm signed clearance D⋆(q)=min(i,j)∈Pdij⋆(q) . Under the same 100k retained-configuration budget, uniform q -sampling is dominated by far-field configurations, whereas our quota-driven active mining shifts the retained set toward penetration and near-field configurations. The link-pair-level quota mechanism used to construct this retained set is described in Sec. II-C.
Model
Params (M) ↓
Train Time ↓
RMSE ↓
SC-MAE ↓
FNR ↓
AUC ↑
(Trainable / Frozen)
(min)
(cm)
(cm)
(%)
PairwiseNet (Uniform, 100 ep)
0.04 / 0.81
≈270.0
8.70
18.33
100.00
0.9221
PairwiseNet (Active, 50 ep)
0.04 / 0.81
138
4.80
2.38
16.47
0.9888
PairwiseNet (Active, 75 ep)
0.04 / 0.81
202.5
3.08
2.27
7.48
0.9950
PI-UDF (Ours; Active+BCP, 50 ep)
0.11 / 0.00
27.8
3.32
2.33
6.83
0.9948
TABLE I : System-Level Performance and Computational Efficiency. Evaluated on the Static Safety-Critical Test Set ( N=110,000 ). Best results are bold ; second-best are underlined . PI-UDF time includes end-to-end training of its embeddings and regressor. PairwiseNet times report distance-regressor training with a frozen geometry encoder and exclude the one-time encoder-pretraining cost. All reported times are measured on the same workstation.
Fig. 4 : Static Prediction Density. Probability density (KDE) of distance predictions on the static safety-critical set. PI-UDF captures the ground-truth penetration boundary ( d≤0 ), whereas PairwiseNet (Original) shifts toward false-safe values, reflecting poor near-field/penetration field pre-training.
Fig. 5 : Continuous Dynamic Trajectory Tracking. Raw predicted minimum clearances D^(qt) are compared with the Drake/FCL reference D⋆(qt) during the high-speed swapping trajectory.
Variant
Backbone
Act.
Loss
Params
RMSE
SC-MAE
FNR
Rg
Lat.
(cm) ↓
(cm) ↓
(%) ↓
( × 10 -3 ) ↓
(ms)
PI-UDF (MLP)
MLP (128 → 64 → 1)
ReLU
MSE
18,465
4.027
2.354
7.89
12.74
0.048
PI-UDF (MSE)
ResNet ( w=128,b=3 )
SiLU
MSE
109,345
3.442
2.283
7.29
2.31
0.209
PI-UDF (Small)
ResNet ( w=64,b=2 )
SiLU
BCP †
21,921
4.220
2.588
7.71
2.85
0.108
PI-UDF (ReLU)
ResNet ( w=128,b=3 )
ReLU
BCP †
109,345
3.635
2.278
7.52
9.83
0.196
PI-UDF (Base) ⋆
ResNet ( w=128,b=3 )
SiLU
BCP †
109,345
3.316
2.328
6.83
2.14
0.177
TABLE II : Ablation study of architectural and training-loss choices. Regression metrics (RMSE, SC-MAE, FNR) are evaluated on the Static Safety-Critical Test Set ( N=110,000 ), while gradient roughness Rg is a model-level gradient diagnostic along a standardized trajectory. ↓ : lower is better. The best result in each column is boldfaced . † BCP = Boundary-Crossing Penalty; ‡ FT = Fine-tuned on boundary-region samples; HNM = Hard Negative Mining. Note: Latency is measured on the Intel i7-12700 CPU (Sec. III-B1 ) using a batch size of B=64 single-pair queries. Reported values are wall-clock averages over 200 runs after 20 warmup iterations.
Fig. 6 : Gradient smoothness along an SE(3) collision trajectory (link pair L4–L5). Top: Predicted distance d^ . Middle: Directional derivative. ReLU exhibits piecewise-constant staircase artifacts, whereas SiLU remains C∞ -smooth. Bottom: Gradient roughness ∣Δ(∇d^)∣ . While ReLU’s mean roughness is 1.95× higher, its peak spikes are 4.28× more severe ( 0.075 vs. 0.017 ) and occur deep within the collision region ( t=0.72 ). These severe discontinuities can degrade local linearizations and quasi-Newton curvature estimates.
Fig. 7 : RMSE–FNR trade-off across ablation variants (bottom-left is preferred); bubble area scales with parameter count. FT improves both metrics over Base, whereas HNM further lowers FNR with a small RMSE increase.
Fig. 8 : High-speed close-proximity dual-arm swapping. (a) Representative frames from the executed sequence showing the system configurations. (b) Raw PI-UDF minimum D^(qreal) and offline Drake/FCL clearance D⋆(qreal) evaluated on the executed two-arm trajectory. (c) TCP speeds and per-arm max joint-speed ratios.
Fig. 9 : Raw logged PI-UDF clearance D^(qt) and offline Drake/FCL replay D⋆(qt) along the logged sustained dynamic-evasion trajectory.
Mode
Planning configuration
Min. clr. [cm]
Below dsafe
D^/D⋆
D^/D⋆
React.-F
frozen R2 + fixed target
9.16 / 9.68
3 / 1
Pred.-F
future R2 + fixed target
12.22 / 13.93
0 / 0
Pred.-TS
future R2 + target switching
14.09 / 15.91
0 / 0
TABLE III : Dynamic-evasion planning configurations and clearance outcomes.
Fig. 10 : Raw logged PI-UDF clearance D^(qt) is compared with offline Drake/FCL clearance D⋆(qt) evaluated at the same current logged configurations.
In teleoperation, the human operator typically controls only the end-effector pose, which often leads to self-collisions of the manipulator and collisions with environmental obstacles, since joints and links are not controlled individually. A common strategy to mitigate this issue is to enhance the operator's input using optimal-control-based trajectory planning. As derivative-based solvers require differentiable constraints, existing approaches either approximate robots and obstacles with spheres, reducing geometric accuracy, or approximate derivatives, degrading convergence and increasing computation times. We address these limitations by adapting a recent formulation of differentiable collision-avoidance constraints, based on duality in convex optimization, to the teleoperation setting. The robot is approximated with capsules and the environment with polytopes. We compare the resulting trajectory planning method against state-of-the-art techniques in simulation with varying numbers of obstacles and evaluate it on a UR5e manipulator in a real-world teleoperation test. Results show that our approach achieves lower computation times while enabling more accurate obstacle modeling, leading to smoother and collision-free end-effector teleoperation.
Max Grobbel, Tristan Schneider, Daniel Flögel +1
FZI - Forschungszentrum Informatik, Karlsruhe, Germany · Department of Electrical Engineering, Karlsruhe Institute of Technology, Karlsruhe, Germany
This paper presents a hierarchical density-based model predictive control framework for safe collaborative manipulation by multiple quadrupedal robots. The framework enables a team of robots to push a shared object to a desired pose using only the initial and goal poses, without requiring a precomputed reference trajectory. A centralized box-level MPC optimizes contact forces while enforcing a control-density constraint for goal convergence and obstacle avoidance. Each robot then solves its own distributed robot-level whole-body MPC, under a stated shared-information assumption, to track its moving contact location while accounting for static obstacles and the time-varying positions of neighboring robots. The approach is evaluated in MuJoCo using whole-body contact dynamics for two and three Unitree Go2 quadrupeds collaboratively pushing rigid objects through narrow passages. Comparisons with matched Control Barrier Function and RRT* based tracking baselines demonstrate the effectiveness of the proposed density-based formulation for push-only, force- and torque-coupled manipulation tasks. Implementation videos are available at https://jaggu2606.github.io/go2-density-mpc-pushing/
Jagannath Prasad Sahoo, Sriram S. K. S. Narayanan, Umesh Vaidya
Center for Artificial Intelligence and Robotics, Indian Institute of Technology Mandi, India · Department of Mechanical Engineering, Clemson University, Clemson, SC 29630, USA
Humanoid locomotion in highly confined environments requires navigating dense environmental obstacles and complex self-collision bounds while maintaining multi-contact dynamic feasibility. Traditional trajectory optimizers frequently struggle in these restricted spaces, as navigating the large collision space with splines on particle abstractions is insufficient and leads to poor local minima. To address this, we propose a three-stage whole-body planning framework that formulates kinematic path planning directly over kinematically reachable rigid-body volumes. By integrating differentiable collision avoidance into a reachability-constrained formulation, our framework synthesizes volume-informed guides that reliably guide a full-order trajectory optimizer over long horizons. We show that these optimized plans serve as high-quality references to train a residual reinforcement learning policy for robust online execution. We validate our approach on the Unitree G1 humanoid across three benchmark testbeds exceeding NIST emergency response standards, achieving restricted confinement ratios (Cr<1.5). Our framework generates feasible trajectories across 12-to-18-second tasks with complex foot and hand contacts where standard baselines fail, while the learned policy successfully tracks these plans under extensive domain randomization in physics simulation.
Carlos Gonzalez, Luis Sentis
Department of Aerospace Engineering and Engineering Mechanics, The University of Texas at Austin, TX 78712, USA.