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

Code
Overview
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 deployment of our planner. Left: in a laboratory obstacle course composed of stacked and overlapping blocks with limited clearance, the robot performs agile base--arm reconfiguration while traversing tight passages (t1–t8).Right in an office environment, the robot follows a multi-goal route (with an intermediate waypoint) through narrow corridors and furniture-dense areas (t1–t12). Orange arrows visualize the direction of motion.

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. Extensive randomized high-density benchmarks and real-robot experiments demonstrate consistently superior success rates, trajectory quality, and solve times compared to strong baselines, enabling fast, safe, and reliable whole-body planning from naive initializations.

Trajectories Animation

Real-World Push T