Unified Complementarity-Based Contact Modeling and Planning for Soft Robots

TL;DR

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 is introduced.

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.

Figures (4)

• Figure 1: Three-section pneumatically actuated soft robot used in this work. (a) Each section has three actuation units and intermediate plates (red) enforcing the piecewise constant curvature model. (b) Local frames along the backbone represent the robot's deformation. (c) Top, bottom, and intermediate plates are modeled as convex disks (light blue) serving as contact candidates.
• Figure 2: Forward simulation of the soft robot onto an inclined rigid box ($t \in [0, 2]$ s) with six intermediate disks per section as contact candidates, demonstrating that the framework supports an arbitrary number of disks. The robot free-falls from $t = 0$--$1$ s, then actuation ramps on sections 2 and 3 from $t = 1$--$2$ s.
• Figure 3: Scenario B: sequential contact and whole-body deformation of a soft robot interacting with two spherical obstacles under distal actuation.
• Figure 4: Trajectory optimization results for ball manipulation using the MPCC formulation. Each row corresponds to a $90^\circ$ rotation about a different axis: $x$ (top), $y$ (middle), and $z$ (bottom). Key frames from each optimized trajectory are shown. Coordinate axes: red ($x$), green ($y$), blue ($z$).