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

Suhyun Jeon; Yumin Lim; Woo-Jeong Baek; Hyeonseo Kim; Suhan Park; Jaeheung Park

cs.ROMar 26, 2026

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

Authors: Suhyun Jeon, Yumin Lim, Woo-Jeong Baek, Hyeonseo Kim, Suhan Park, Jaeheung Park

Organizations: Department of Intelligence and Information, Graduate School of Convergence Science and Technology, Seoul National University, Republic of Korea. · Artificial Intelligence Institute (AIIS), Seoul National University, Republic of Korea. · Department of Mechanical Engineering, Seoul National University, Republic of Korea. · Department of Robotics, Kwangwoon University, Republic of Korea. ·

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 & tables

Fig. 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.

Fig. 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.

Fig. 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.

Panel of Figure 4 — Fig. 4 : Experimental setup in simulation using Franka Panda robots across diverse manipulation tasks.

		CBiRRT / 100 scenes / 100 sec		Latent Motion / 100 scenes / 100 sec
Task	Method	Planning time (sec)	Success rate (%)	Planning time (sec)	Success rate (%)
Fixed Orientation	Random	$6.599\pm 11.533$	$92$	$2.944\pm 5.996$	$94$
	Joint-Space	$6.986\pm 8.744$	$\mathbf{100}$	$8.230\pm 16.891$	$99$
	Feature-Space (Ours)	$\mathbf{5.265\pm 5.221}$	$\mathbf{100}$	$\mathbf{1.808\pm 3.702}$	$\mathbf{100}$
Dual Manipulation	Random	$37.403\pm 34.414$	$31$	$4.700\pm 7.904$	$90$
	Joint-Space	$47.211\pm 26.268$	$41$	$2.510\pm 5.554$	$\mathbf{100}$

TABLE I : Comparison of start–goal selection method on Franka Panda across diverse manipulation tasks (100 scenes, 100 second time limit). Metrics are the average planning time (successful trials only) and the success rates.

Panel of Figure 6 — Fig. 5 : Experimental setup in simulation using the Tocabi humanoid for dual-arm manipulation tasks.

	Dual Manipulation		Dual Manipulation with Fixed Orientation
Method	Planning time (s)	Success rate (%)	Planning time (s)	Success rate (%)
Random	$7.351\pm 9.810$	$34$	$\mathbf{25.963\pm 25.951}$	$18$
Joint-Space	$8.372\pm 13.162$	$\mathbf{98}$	$35.778\pm 25.378$	$65$
Feature-Space (Ours)	$\mathbf{6.909\pm 8.584}$	$\mathbf{98}$	$38.341\pm 26.326$	$\mathbf{80}$

TABLE II : Comparison of start–goal selection method on the humanoid robot Tocabi using CBiRRT (100 scenes, 100 second time limit). Metrics are the average planning time (successful trials only) and the success rates.

Constrained Motion Planning

Constrained motion planning finds robot paths and controls that satisfy geometric, dynamic, safety, and task-specific requirements.

See all 188 papers

Induced Riemannian Metrics for Motion Planning with Constraints
Sep 22, 2026Phone Thiha Kyaw, Thomas Cohn, Miguel Angel Rogel Garcia +1Constrained Motion Planning Trajectory Optimization
Space & Terrestrial Autonomous Robotic Systems (STARS) Laboratory at the University of Toronto Institute for Aerospace Studies (UTIAS), Toronto, Ontario, Canada, M3H 5T6
Vectorizing Projection in Manifold-Constrained Motion Planning for Real-Time Whole-Body Control
Apr 14, 2026Shrutheesh R Iyer, I-Chia Chang, Andrew Z. Liu +2Motion Planning Constrained Motion Planning
Department of Computer Science, Purdue University · School of Mechanical Engineering, Purdue University
Manifold-constrained Hamilton-Jacobi Reachability Learning for Decentralized Multi-Agent Motion Planning
Nov 5, 2025Qingyi Chen, Ruiqi Ni, Junyoung Kim +1Multi-Robot Motion Planning Constrained Motion Planning
Department of Computer Science at Purdue University, West Lafayette, IN 47907, USA.

Sep 22, 2026J/K move · Enter open · S save

Induced Riemannian Metrics for Motion Planning with Constraints

Phone Thiha Kyaw, Thomas Cohn, Miguel Angel Rogel Garcia, Jonathan Kelly

Space & Terrestrial Autonomous Robotic Systems (STARS) Laboratory at the University of Toronto Institute for Aerospace Studies (UTIAS), Toronto, Ontario, Canada, M3H 5T6

Constrained Motion Planning Trajectory Optimization Motion Planning Sampling-Based Motion Planning

Open paper AskPDF arXiv

In constrained motion planning problems, task and loop-closure constraints restrict a robot's motion to a curved, lower-dimensional submanifold of its configuration space. Planners measure path length with a metric, which sets the cost of moving in each direction. Under the Euclidean metric, this cost is the same everywhere, whereas under a general Riemannian metric, such as the kinetic-energy metric, the cost can vary with direction and configuration. Existing methods often describe the submanifold either implicitly, as a constraint level set, or explicitly, through a parameterization. The implicit representation is typically combined with the Euclidean metric of the configuration space, and the explicit representation with the parameter domain, so the path length that a planner minimizes depends on the representation. Instead, we measure path length with the induced metric, which the submanifold inherits from a Riemannian metric on the configuration space. The implicit and explicit representations yield the same induced metric, expressed in different coordinates, and hence the same geometry. This result holds for any Riemannian metric on the configuration space, not only the Euclidean one. The choice of metric is therefore independent of the choice of representation. Using this result, we extend planning under a Riemannian metric from unconstrained spaces to constraint submanifolds by applying the induced metric in both a sampling-based planner and a trajectory optimizer. For an explicit representation, the induced metric also accounts for the distortion that the parameterization introduces. In experiments on a bimanual manipulation setup with two Franka arms under end-effector task constraints, we compare the Euclidean and kinetic-energy metrics.

Abstract

Constrained Motion Planning

Explore similar work

Induced Riemannian Metrics for Motion Planning with Constraints

Vectorizing Projection in Manifold-Constrained Motion Planning for Real-Time Whole-Body Control

Manifold-constrained Hamilton-Jacobi Reachability Learning for Decentralized Multi-Agent Motion Planning