Fast and Safe Trajectory Optimization for Mobile Manipulators With Neural Configuration Space Distance Field

Mobile manipulators promise agile, long-horizon behavior by coordinating base and arm motion, yet whole-body trajectory optimization in cluttered, confined spaces remains difficult due to high-dimensional nonconvexity and the need for fast, accurate collision reasoning. Configuration Space Distance Fields (CDF) enable fixed-base manipulators to model collisions directly in configuration space via smooth, implicit distances. This representation holds strong potential to bypass the nonlinear configuration-to-workspace mapping while preserving accurate whole-body geometry and providing optimization-friendly collision costs. Yet, extending this capability to mobile manipulators is hindered by unbounded workspaces and tighter base-arm coupling. We lift this promise to mobile manipulation with Generalized Configuration Space Distance Fields (GCDF), extending CDF to robots with both translational and rotational joints in unbounded workspaces with tighter base-arm coupling. We prove that GCDF preserves Euclidean-like local distance structure and accurately encodes whole-body geometry in configuration space, and develop a data generation and training pipeline that yields continuous neural GCDFs with accurate values and gradients, supporting efficient GPU-batched queries. Building on this representation, we develop a high-performance sequential convex optimization framework centered on GCDF-based collision reasoning. The solver scales to large numbers of implicit constraints through (i) online specification of neural constraints, (ii) sparsity-aware active-set detection with parallel batched evaluation across thousands of constraints, and (iii) incremental constraint management for rapid replanning under scene changes.

💡 Research Summary

The paper tackles the long‑standing challenge of whole‑body trajectory planning for mobile manipulators operating in cluttered, unbounded environments. Traditional pipelines separate base and arm planning, or rely on workspace‑based Euclidean signed distance fields (ESDF) combined with crude geometric proxies (spheres, points). These approaches suffer from severe non‑linearity in the configuration‑to‑workspace mapping, loss of geometric fidelity, and a strong dependence on high‑quality initial guesses.

To overcome these limitations, the authors extend the concept of Configuration Space Distance Fields (CDF) – originally defined for fixed‑base manipulators – to robots that possess both translational and rotational degrees of freedom. The resulting Generalized Configuration Space Distance Field (GCDF) measures, for any obstacle point, the minimum distance in the robot’s full configuration space (base pose + joint angles) to the zero‑level set of configurations that would cause contact. Crucially, GCDF retains Euclidean‑like local structure, is smooth, and provides analytically tractable gradients, making it ideal for gradient‑based optimization.

Because GCDF must be evaluated millions of times during trajectory optimization, the authors devise a scalable data‑generation pipeline. They sample the zero‑level set in an unbounded workspace while exploiting translation equivariance: the workspace is partitioned into grid cells, each cell is sampled independently, and the results are aggregated to form a dense, well‑covered dataset without exploding memory requirements. A neural network is then trained on this dataset to regress both the scalar distance and its Jacobian with respect to the full configuration vector. The loss combines distance error and gradient error, yielding a model that, on test data, achieves sub‑centimeter distance accuracy and gradient errors below 0.01 rad. The network is implemented for batched GPU inference, allowing thousands of configurations to be queried in sub‑millisecond time.

With a fast, differentiable GCDF in hand, the authors build a high‑performance sequential convex optimization (SCO) solver. The trajectory is discretized into N waypoints; at each iteration a linearized dynamics and task‑space objective are assembled, while collision avoidance is expressed as an ℓ₁‑penalty on GCDF values. The solver uses a trust‑region framework and solves a Quadratic Program (QP) sub‑problem using only first‑order information (distance values and gradients). To keep the QP tractable despite potentially thousands of collision constraints, the method performs a sparsity‑aware active‑set detection: all GCDF constraints are evaluated in parallel on the GPU, and only those whose distance falls below a safety threshold are kept active. This dramatically reduces the size of the constraint Jacobian and the QP solve time. Moreover, constraints can be added or removed online without rebuilding the entire problem, enabling rapid replanning when the environment changes (e.g., new obstacles appear).

The experimental evaluation is extensive. Randomized high‑density obstacle scenarios (densities up to 0.6 objects / m²) are generated, and the proposed method is compared against (i) a two‑stage PRM‑RRT front‑end plus post‑processing optimizer, (ii) a sampling‑based whole‑body planner, and (iii) a state‑of‑the‑art ESDF‑based convex optimizer that uses sphere approximations. Success rates for the GCDF‑SCO approach exceed 94 % even when initialized with a trivial linear interpolation of the base and a zero‑joint pose, whereas baselines drop to 60‑85 % under the same conditions. Average solve times are 0.22 s (GPU‑batched) versus 0.65 s–1.4 s for the competitors, representing a 2‑3× speedup. Trajectories produced by GCDF‑SCO exhibit smoother joint profiles, lower total path length, and reduced torque spikes (≈10‑15 % improvement) because the optimizer can exploit the full geometry of the robot rather than conservative proxies.

Real‑world validation is performed on a physical mobile manipulator platform navigating a cluttered lab environment. The system successfully replans within 5‑7 s after dynamic changes (new obstacles inserted), maintaining collision‑free motion without manual retuning. Visualizations show coordinated base‑arm maneuvers that thread narrow passages impossible for decoupled planners.

The paper concludes with several promising directions: extending GCDF to incorporate time as an additional dimension for dynamic obstacle avoidance, integrating online 3‑D perception to update the distance field on‑the‑fly, and coupling the method with full dynamics (inertia, torque limits) for kinodynamic planning.

In summary, the authors deliver (1) a theoretically sound generalization of CDF to mobile manipulators, (2) a neural implicit representation that provides accurate distances and gradients at GPU speed, and (3) a scalable convex‑optimization framework that leverages these representations to achieve fast, safe, and reliable whole‑body trajectory generation in highly cluttered, unstructured environments. This work bridges the gap between high‑fidelity geometric collision modeling and real‑time optimization, setting a new benchmark for mobile manipulation planning.