Suppressing C-space discontinuities during planning

Develop a strategy that suppresses large configuration-space discontinuities during constrained bimanual motion planning without sacrificing planning-tree density or planning speed, thereby improving trajectory success rates, particularly in cluttered environments.

Background

The proposed pipeline uses inverse kinematics to parameterize the bimanual constraint manifold and performs discontinuity detection and correction during post-processing. Large changes between adjacent inverse-kinematic solutions can nevertheless create configuration-space discontinuities that prevent the construction of smooth, executable trajectories. The current correction procedure may fail when the distance between configurations is large.

The unresolved issue is how to address these discontinuities during the planning stage itself, rather than only after a path has been found. Any solution must preserve the advantages of the current method—especially dense tree exploration and fast planning—while improving robustness in cluttered environments.