A Complementarity-Based Framework for Soft-Robot Contact Modeling and Planning

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 linear complementarity problems (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 complementarity-based framework for soft-robot contact modeling and planning that uses a consistent contact formulation across forward simulation and trajectory optimization. We develop a robust 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 in simulation on contact-rich ball-manipulation tasks. In conclusion, the proposed framework provides a new foundation for contact modeling, simulation, and planning in soft robotics.