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

TL;DR

The paper tackles fast, safe full-body trajectory optimization for mobile manipulators in dense environments by introducing Generalized Configuration Space Distance Fields (GCDF), a weighted, neural implicit distance in configuration space that accounts for both base translation and arm orientation. It combines a theoretically grounded GCDF with a high-performance sequential convex optimization solver that queries a neural GCDF in parallel, enabling online constraint handling, sparse Jacobian exploitation, and online constraint updates for rapid replanning. Key contributions include the GCDF formulation with weighted eikonal and single-step projection guarantees, a scalable neural GCDF training pipeline that preserves gradients, and an open-source C++ solver that integrates with GPU-based GCDF queries. The framework achieves robust, non-conservative, whole-body motions in highly cluttered maps and real hardware, demonstrating practical applicability for long-horizon mobile manipulation tasks with dynamic or unknown environments.

Abstract

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.

Figures (12)

• Figure 1: Trajectory generation in complex environments for mobile manipulators using the proposed numerical optimization algorithm with neural Generalized Configuration Space Distance Fields. Starting from a trivial initial guess (the mobile base linearly interpolated between start and goal while the manipulator joints are all zeros, i.e., kept upright), the solver generates collision-free trajectories from scratch, exhibiting smooth and agile maneuvers that leverage whole-body coordination for safe obstacle avoidance. (a) Simulation results are shown from front and top views. (b) Real-world experiments in a similarly cluttered setup.
• Figure 2: Illustration of SDF (left) and CDF (right) for a query point $\bm p$ at configuration $\bm q$. SDF measures the task-space distance from $\bm p$ to the nearest point $\bm p'$ on the robot surface, as defined in \ref{['eq:sdf-cspace']}. In contrast, CDF measures the configuration-space (angular) distance from $\bm q$ to the nearest contact configuration $\bm q'$ on the zero-level set induced by $\bm p$, as defined in \ref{['eq:cdf']}.
• Figure 3: Illustration of GCDF values for a 1-DoF planar arm. The obstacle induces two symmetric contact configurations on the zero-level set, denoted by $\bm q_1$ and $\bm q_2$. For the unsigned GCDF, the distance function exhibits a spurious local maximum at $\bm q_0$, where the obstacle is equally distant from both arm sides, which can mislead gradient-based numerical solvers. Introducing the sign based on penetration removes this pathology, yielding smooth and consistent gradient directions toward the contact set.
• Figure 4: Construction of zero-level set for mobile manipulators. (a) For each workspace point $\bm p$ on the grid centered at the robot origin, we construct a representative subset $\tilde{\mathcal{Z}}(\bm p)\subset\mathcal{Z}(\bm p)$ by fixing the base translation and solving for contact configurations over the arm joints and base rotations. (b) For a new query point $\bm p$ with height $p_z$, we gather all grid points $\bm p_i$ on the same horizontal slice ($\bm p_{i,z}=\bm p_z$) in a neighborhood around $\bm p$. (c) We reconstruct the full zero-level set $\mathcal{Z}(\bm p)$ by taking the union of the precomputed subsets on that slice and compensating for their horizontal offsets, effectively translating the base while reusing the same arm contact configurations.
• Figure 5: Visualization of the Jacobian sparsity pattern induced by GCDF constraints, together with the dense-to-sparse mapping that assembles the full GCDF gradient vector queried from the implicit neural model into the sparse Jacobian.

Theorems & Definitions (10)

• Remark 1
• Definition 1: Generalized CDF
• Remark 2
• Theorem 1: Properties of GCDF
• proof
• Remark 3
• Remark 4
• Remark 5
• Remark 6
• proof