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

TL;DR

This paper introduces Generalized Configuration Space Distance Fields (GCDF), a neural representation enabling fast, accurate collision reasoning for mobile manipulators in complex environments, facilitating efficient trajectory optimization.

Contribution

The paper extends CDF to GCDF for mobile manipulators with translational and rotational joints, providing a scalable, neural-based collision reasoning framework for trajectory optimization.

Findings

GCDF preserves Euclidean-like local distances.

Neural GCDFs support efficient GPU queries.

The optimization framework scales to thousands of constraints.

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.