Unified Complementarity-Based Contact Modeling and Planning for Soft Robots

Soft robots are built to exploit safe, whole-body contact with their environment, making contact modeling and planning central to their potential impact. Yet most existing soft-robot work either ignores contact, treats it with simple penalties, or relies on high-fidelity FEM models that are too slow and restricted to small systems with simplified sticking-only interactions. As a result, we lack a scalable, physically consistent way to both simulate and plan contact-rich behaviors for continuum manipulators.

This work introduces a unified complementarity-based framework in which a single contact formulation underlies both forward simulation and trajectory optimization for a discretized soft arm. On the simulation side, we condition the resulting Linear Complementarity Problem (LCP) so it can be solved reliably even with many simultaneous contacts. On the planning side, we embed the same contact law in a Mathematical Program with Complementarity Constraints (MPCC) and use a kinematically guided warm-start to obtain dynamic manipulation trajectories that deliberately exploit contact.

We model a three-section pneumatically actuated soft arm under a piecewise-constant-curvature description and place convex contact disks along its backbone as potential contact candidates. At each time step, actuator-space dynamics are coupled to a velocity-level LCP contact formulation: normal impulses are complementary to normal relative velocity, and tangential impulses lie in a polyhedral approximation of the Coulomb friction cone, so separation, sticking, and sliding are all captured within a single algebraic model.

Dense contact disks introduce redundant constraints and scale disparities that render the raw LCP matrix singular or extremely ill-conditioned. We address this with a three-stage conditioning pipeline tailored to discretized soft bodies: inertial rank selection removes dynamically redundant contact rows, Ruiz equilibration balances the scales of all matrix blocks, and a lightweight Tikhonov regularization is applied to the normal blocks. The resulting conditioned LCP can be solved robustly by standard LCP solvers, enabling stable, high-fidelity simulation of whole-body, multi-contact interactions that are difficult for conventional physics engines.

Using the same complementarity-based contact law, we formulate ball-rotation tasks as an MPCC in which the optimizer jointly chooses robot actuation, ball motion, and contact forces over a horizon. Complementarity constraints enforce nonpenetration and Coulomb friction at each contact candidate, so the optimizer reasons explicitly about when the arm makes, maintains, or breaks contact with the ball.

Directly solving the full dynamic MPCC is challenging, so we introduce a kinematically guided warm-start. In the first stage, we remove the robot dynamics and treat the arm as a kinematic chain: we optimize over arm configurations, ball motion, and contact forces while preserving the same contact geometry and complementarity-based friction model. This kinodynamic problem produces a feasible contact schedule and ball rotation trajectory. In the second stage, we reintroduce the full soft-robot dynamics and solve the dynamic MPCC, initializing robot states, ball states, and contact impulses from the kinematic solution. This warm-start steers the solver toward dynamically feasible, contact-rich trajectories that achieve precise 90° ball rotations.