Disentangled Iterative Surface Fitting for Contact-stable Grasp Planning

Overview

While surface fitting algorithms based on geometric compatibility optimization offer high flexibility, they do not sufficiently account for whether the aligned surfaces actually lead to a stable grasp. Specifically, achieving a stable grasp requires the ability to generate contact forces that can fully counteract external forces and torques (known as force-closure property). However, by focusing solely on geometric alignment, these methods fail to consider fundamental factors necessary for generating contact forces, such as the appropriate spatial relationship between the hand and the object. As a result, even if the surfaces are geometrically well-aligned, a spatial gap can form between the hand and the object, preventing actual contact from being established, or leading to an unstable distribution of contact points.

To address this issue, it is essential not only to align surfaces based on geometric compatibility but also to ensure that the hand and object surfaces are spatially well-aligned, allowing contact to be potentially established. We refer to this spatial alignment, which facilitates contact, as contact stability (Fig.1).

Proposed Framwork

In this study, we propose a novel surface fitting-based grasp planning algorithm that incorporates contact stability alongside geometric compatibility, which we call Disentangled Iterative Surface Fitting (DISF). From the perspective of contact stability, we explicitly integrate CoM alignment into the optimization process, drawing inspiration from the observation that, as mentioned earlier, humans naturally align their hand’s CoM with that of the object to enhance grasp stability. To achieve this, we leverage another key insight from human grasping behavior-that different pose parameters are optimized sequentially rather than simultaneously-and disentangle the overall grasp pose optimization into the following three sequential stages: (1) rotation optimization to align contact normals, (2) translation refinement for CoM alignment, and (3) gripper aperture adjustment to optimize contact point distribution. Our disentangled optimization framework preserves the advantages of flexible geometric compatibility evaluation while systematically enhancing contact stability through CoM alignment. The overview of our DISF framework is shown in Fig.2.

Contact Surface Optimization

The grasp planning problem with antipodal grippers can be modeled as a contact surface optimization problem which maximizes the grasp quality $Q$ by optimizing the rotation and translation parameter $(\mathbf{R}, \mathbf{t})$ as well as the fingertip displacement $\delta d$ from the original gripper width, given a specific set of contact surfaces between the fingertip $\mathcal{S}^f$ and object $\mathcal{S}^o$. This optimization can be demonstrated in Fig.3.

Simulation Experiment

Grasp Quality Evaluation

To validate the effectiveness of the proposed method, we evaluated the grasp quality for ten YCB objects using both the conventional geometric compatibility measures and the contact stability measure via CoM misalignment introduced in this study. As an evaluation metric for geometric compatibility, we used the following weighted measure, which combines the surface distance and the contact normal misalignment.

The results are shown in Fig.4. The Top part represents the geometric compatibility error, while the Bottom part represents the CoM alignment error. For geometric compatibility evaluation, the sampling-based approach (CMA-ES) and the proposed DISF exhibit lower errors, whereas the conventional VISF consistently shows higher errors. An interesting phenomenon is that VISF, which is supposed to optimize geometric compatibility, performs worse than the other two methods.

Grasp Success Rate Evaluation

To evaluate the feasibility of the planned grasps, we performed grasp executions in the MuJoCo physics simulator and assessed the grasp success rate for each object.

The below table presents the success and failure rates of grasp executions in the simulation experiments. In the columns for each method, a ✅ indicates success, while a horizontal bar (-) indicates failure. The “Correspondence” column shows the total number of correspondence pairs used for surface fitting optimization. The bottom rows report the overall grasp success rate for each method across all objects and the average planning time in milliseconds.

This table summarizes the grasp execution results. It can be observed that the proposed DISF method outperforms the baseline methods, highlighting the importance of CoM alignment in ensuring stable distribution of contact points for successful grasps.

Planned Grasp Execution by Proposed DISF

Success Cases

Failure Cases

Supplementary Materials

BibTeX citation

    @inproceedings{disf_IAS19_2025,
  author       = {Tomoya Yamanokuchi, Alberto Bacchin, Emilio Olivastri, Takamitsu Matsubara, Emanuele Menegatti},
  title        = {Disentangled Iterative Surface Fitting for Contact-stable Grasp Planning},
  booktitle    = {*** under review ***},
}