Unified Complementarity-Based Contact Modeling and Planning for Soft Robots

TL;DR

This paper introduces a unified, physically consistent framework for modeling and planning contact-rich interactions in soft robots, addressing challenges like redundant constraints and ill-conditioning with a novel LCP-based approach.

Contribution

It develops a robust LCP model with a three-stage conditioning pipeline and a warm-start strategy for dynamic contact planning in soft robotics.

Findings

Effective contact modeling for soft robots using LCPs.

Improved trajectory optimization through contact with MPCC.

Enhanced robustness and efficiency in contact-rich manipulation tasks.

Abstract

Soft robots were introduced in large part to enable safe, adaptive interaction with the environment, and this interaction relies fundamentally on contact. However, modeling and planning contact-rich interactions for soft robots remain challenging: dense contact candidates along the body create redundant constraints and rank-deficient LCPs, while the disparity between high stiffness and low friction introduces severe ill-conditioning. Existing approaches rely on problem-specific approximations or penalty-based treatments. This letter presents a unified complementarity-based framework for soft-robot contact modeling and planning that brings contact modeling, manipulation, and planning into a unified, physically consistent formulation. We develop a robust Linear Complementarity Problem (LCP) model tailored to discretized soft robots and address these challenges with a three-stage conditioning pipeline: inertial rank selection to remove redundant contacts, Ruiz equilibration to correct scale disparity and ill-conditioning, and lightweight Tikhonov regularization on normal blocks. Building on the same formulation, we introduce a kinematically guided warm-start strategy that enables dynamic trajectory optimization through contact using Mathematical Programs with Complementarity Constraints (MPCC) and demonstrate its effectiveness on contact-rich ball manipulation tasks. In conclusion, CUSP provides a new foundation for unifying contact modeling, simulation, and planning in soft robotics.