Connectivity-Aware Representations for Constrained Motion Planning via Multi-Scale Contrastive Learning

Abstract

The objective of constrained motion planning is to connect start and goal configurations while satisfying task-specific constraints. Motion planning becomes inefficient or infeasible when the configurations lie in disconnected regions, known as essentially mutually disconnected (EMD) components. Constraints further restrict feasible space to a lower-dimensional submanifold, while redundancy introduces additional complexity because a single end-effector pose admits infinitely many inverse kinematic solutions that may form discrete self-motion manifolds. This paper addresses these challenges by learning a connectivity-aware representation for selecting start and goal configurations prior to planning. Joint configurations are embedded into a latent space through multi-scale manifold learning across neighborhood ranges from local to global, and clustering generates pseudo-labels that supervise a contrastive learning framework. The proposed framework provides a connectivity-aware measure that biases the selection of start and goal configurations in connected regions, avoiding EMDs and yielding higher success rates with reduced planning time. Experiments on various manipulation tasks showed that our method achieves 1.9 times higher success rates and reduces the planning time by a factor of 0.43 compared to baselines.

Figures (5)

• Figure 1: Illustration of how different configuration selections in constrained motion planning result in varying levels of planning difficulty. (Left) Multiple inverse kinematic solutions for the start and goal pose. (Right) Connected configurations are easy to plan, configurations in narrow passages are difficult to plan, and disconnected configurations are infeasible.
• Figure 2: Overview of the proposed method. The pipeline consists of two stages: (i) multi-scale pseudo-label generation via manifold learning and clustering at different neighborhood scales, and (ii) multi-scale contrastive learning guided by these labels, where configurations sharing the same pseudo-label are pulled closer in the feature-space and those with different labels are pushed apart, resulting in a connectivity-aware representation.
• Figure 3: Multi-scale pseudo-label generation on a modified Swiss Roll dataset emulating constrained regions in robotic configuration spaces. (Top) pseudo-labels projected onto the original data. (Bottom) latent embeddings with clustering. Left to right: increasing nearest neighborhood (NN) scales from local to global connectivity.
• Figure 4: Experimental setup in simulation using Franka Panda robots across diverse manipulation tasks.
• Figure 5: Experimental setup in simulation using the Tocabi humanoid for dual-arm manipulation tasks.