Simultaneous Contact Selection and Planning for Contact-Rich Manipulation with Cascaded Optimization
Zhe Zhang, Xingrong Diao, Haoxiang Liang, Han Yang, Bi-Ke Zhu, Dandan Zhang, Jiankun Wang, Senior Member, IEEE · 2026
Read Simultaneous Contact Selection and Planning for Contact-Rich Manipulation with Cascaded Optimization by Zhe Zhang, Xingrong Diao, Haoxiang Liang, Han Yang, Bi-Ke Zhu, Dandan Zhang, Jiankun Wang, Senior Member, IEEE (2026) in the Tower of Babel, a free online library of public-domain works preserved in clean, searchable text. No sign-up. The full text is served as a plain-text file at a predictable URL, where text is present: https://antarctic-labs.com/tower-of-babel/arxiv-2605-27972.txt.
Introduction
Zhang and colleagues let robots pick where to touch an object and how to move it, in one joint plan. Their staged method first hunts for good grip spots with a stand-in contact model, then plots arm moves from those spots. Lab tests and real trials show varied, tough handling even when motion models err and sensing is noisy.
Catalog record
| CREATOR | Zhe Zhang, Xingrong Diao, Haoxiang Liang, Han Yang, Bi-Ke Zhu, Dandan Zhang, Jiankun Wang, Senior Member, IEEE |
|---|---|
| YEAR | 2026 |
| COLLECTION | PAPERS |
| FORMAT | TXT |
| FILE SIZE | 105794 |
| LICENSE | http://creativecommons.org/licenses/by/4.0/ |
| RIGHTS | CC BY |
| SOURCE | arXiv |
Source: https://arxiv.org/abs/2605.27972
Browse the full library · About the Tower of Babel
Full text
The opening of the work, from the library's hosted text:
Simultaneous Contact Selection and Planning for Contact-Rich Manipulation with Cascaded Optimization Zhe Zhang, Xingrong Diao, Haoxiang Liang, Han Yang, Bi-Ke Zhu, Dandan Zhang, Jiankun Wang, Senior Member, IEEE Abstract We propose an optimization-based framework for robust contact-rich manipulation. Recent contact-implicit methods enable online hybrid planning across contact modes, allowing closed-loop manipulation for a given target state and contact location sequence of the robot and object. However, most existing approaches lack the ability to autonomously reason and generate diverse contact location sequences and manipulation trajectories, i.e., active contact location selection, which limits their applicability to relatively simple tasks. Active contact location selection is challenging due to complementarity in contact dynamics and the sparse gradients, making the design of a unified framework for contact selection and planning difficult. To address these challenges, we introduce Simultaneous Contact Selection and Planning (SCSP), a cascaded optimization framework comprising Contact Selection Optimization (CSO) and Contact Planning Optimization (CPO). CSO leverages a surrogate contact model and discrete-continuous optimization to efficiently resolve the nonsmoothness and coupling in contact selection, enabling online global searching of optimal contact locations. CPO performs prior-guided contact planning by evaluating the reference contact locations produced by CSO and generating corresponding manipulation trajectories in real time for redundant manipulators. Extensive simulations and real-world experiments demonstrate that SCSP produces diverse manipulation behaviors and robust control under inaccurate dynamics and perceptual noise. We further validate the generalization of the framework on challenging manipulation tasks. Project website: https://sites.google.com/view/scsp-robot . Nomenclature p,\bm{p} A 3D point and its position vector. \theta,\bm{\psi} Angle and quaternion vector. \mathcal{O},\partial\mathcal{O} Object geometry and surface. \mathcal{R},\partial\mathcal{R} Robot geometry and surface. h Time interval. \bm{\lambda},\bm{\beta} Contact force vector and contact impulse vector. \bm{\phi} Contact distance vector. \bm{J} Contact Jacobian. \bm{n},\bm{t}_{i} Normal vector and i -th tangental vector. \bm{x}=[\bm{p},\bm{\psi}] Pose vector consists of position and orientation. \bm{q},\dot{\bm{q}},\ddot{\bm{q}} Robot configuration, velocity and acceleration. \bm{s} System state consists of object pose \bm{x}_{o} and \bm{q} . \bm{v} Velocity vector in Cartesian space. I Introduction Contact-Rich manipulation requires robots to intentionally make and break contact with objects to drive them toward desired states. Specifically, the robot needs to plan the contact sequence consisting of approaching trajectory, contact locations, contact forces, and transitions between different contact modes (i.e., contact, separation, sliding, and sticking). Such contact planning is challenging due to the combinatorial complexity of contact modes and the inherent nonsmoothness arising from the hybrid nature of contact dynamics. Early works adopted hierarchical frameworks by separating continuous-state planning and discrete contact mode searching [ 56 , 48 , 16 , 12 , 13 ] , or utilizing mixed-integer programs to handle the hybrid nature of contact [ 1 , 33 , 17 ] . However, this separation often suffers from poor scalability caused by the explosion of contact trajectories. Therefore, these methods offer limited manipulation strategies and poor robustness. Recent advances have been achieved through both learning-based methods and model-based methods. Learning-based methods [ 23 , 28 , 54 , 53 , 55 , 14 , 20 , 26 , 31 ] can capture the nonsmooth nature of contact dynamics through offline reinforcement learning (RL), thus efficiently handle complex contact-rich manipulation tasks that are difficult for model-based methods. However, they are often limited to generalization across diverse object types [ 23 , 28 , 54 ] or new task settings [ 55 , 20 , 26 , 14 ] . This constitutes a major bottleneck in developing foundation models for manipulation. DyWA [ 31 ] proposes a world-action model architecture and shows impressive results in 6D non-prehensile manipulation with generalization across diverse object geometries. However, a generalizable framework remains to be explored. Fig. 1 : SCSP enables autonomous reasoning for contact sequence selection and online generation of diverse manipulation trajectories, allowing it to be applied to a variety of challenging contact-rich manipulation tasks. The representative model-based method is contact-implicit trajectory optimization (CITO), which was first applied to locomotion [ 38 , 46 , 27 ] and later extended to manipulation. We mainly focus on Contact-Implicit Model Predictive Control (CIMPC), which solves the contact planning problem online. These methods leverage time-stepping contact dynamics [ 39 , 5 ] and incorporate a convexified contact model within trajectory optimization, enabling hybrid contact planning in a unified framework. CIMPC can compute control inputs autonomously based solely on the object’s current pose, goal pose, and dynamics information to establish and break contact with the object. Existing methods primarily focus on investigating different approximation formulations and solution strategies. For instance, C3 [ 7 ] locally linearizes the LCS model and solves it using various solvers. CQDC [ 36 ] proposed a convexified contact model significantly reducing computational complexity, and subsequent works [ 40 ] [ 21 ] have built upon this idea to design even more efficient algorithms. Furthermore, CF-MPC [ 22 ] derives a complementarity-free model to improve computational efficiency. Although such online hybrid planning has increased manipulation diversity and enabled these methods to accomplish more complex tasks, the lack of active contact selection remains a key bottleneck to their performance. These methods are largely limited to manipulating objects with simple geometries or operating under accurate dynamics and perceptions. Additionally, CIMPC is prone to converging to local optima when the robot’s initial guess is poor. Active contact selection is crucial in complex contact-rich manipulation, as it enables the generation of sufficiently diverse manipulation strategies to avoid local optima. However, most existing model-based methods lack this capability. Current approaches either rely on hand-crafted cost functions to guide contact or utilize a given reference trajectory as a contact prior for planning. This constrains their manipulation trajectories to a narrow distribution, making them prone to failure when facing out-of-distribution (OOD) tasks. [ 40 , 21 ] rely on high-quality prior knowledge of contact locations and are unable to perform a global search for the optimal contact location. [ 27 ] developed a novel optimization formulation named CRISP to achieve global planning of contact force and contact location, but it depends on explicit geometric representations of objects and is limited to 2D pushing tasks involving simple geometries. IDTO [ 25 ] removes the shooting phase from CIMPC and reformulates the problem as a trajectory optimization and inverse dynamics problem, enabling real-time computation on highly redundant robots. However, this method is limited to simple regular objects to ensure gradient continuity during optimization. STOCS-3D [ 52 ] enables contact planning for arbitrarily complex geometries by formulating CITO as a semi-infinite problem, but it is restricted to offline computation and has not been validated on robot manipulation tasks. [ 42 , 11 ] achieve 6D nonprehensile manipulation by sampling points across the object’s surface and evaluating parallel CIMPC instances to determine the optimal contact location, but they suffer from prohibitive computational costs. [ 35 ] uses a reference trajectory as prior guidance for contact selection, but it is unable to achieve autonomous reasoning. [ 49 ] explores the use of an RL policy to generate the optimal contact location and contact planning parameters, enabling online contact selection and planning. This work inspiring for our approach, but it still lacks validation of generalization across different topologies. Designing a unified planning framework for simultaneous contact selection and contact planning is challenging. The complementarity inherent in contact dynamics