Contact-Aware Safety in Soft Robots Using High-Order Control Barrier and Lyapunov Functions Publisher: IEEE Cite This

IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED SEPTEMBER, 2025 1 Contact-Aware Safety in Soft Robots Using High-Order Control Barrier and Lyapunov Functions Kiwan Wong1,2, Maximilian St¨ olzle1,2,3, Wei Xiao4,2, Cosimo Della Santina3, Daniela Rus∗,2, Gioele Zardini∗,1 Abstract—Robots operating alongside people, particularly in sensitive scenarios such as aiding the elderly with daily tasks or collaborating with workers in manufacturing, must guarantee safety and cultivate user trust. Continuum soft manipulators promise safety through material compliance, but as designs evolve for greater precision, payload capacity, and speed, and increasingly incorporate rigid elements, their injury risk resurfaces. In this letter, we introduce a comprehensive High-Order Control Barrier Function (HOCBF) + High-Order Control Lyapunov Function (HOCLF) framework that enforces strict contact force limits across the entire soft-robot body during environmental interactions. Our approach combines a differentiable Piecewise Cosserat-Segment (PCS) dynamics model with a convex-polygon distance approximation metric, named Differentiable Conservative Separating Axis Theorem (DCSAT), based on the soft robot geometry to enable real-time, whole-body collision detection, resolution, and enforcement of the safety constraints. By embedding HOCBFs into our optimization routine, we guarantee safety, allowing, for instance, safe navigation in operational space under HOCLF-driven motion objectives. Extensive planar simulations demonstrate that our method maintains safety-bounded contacts while achieving precise shape and task-space regulation. This work thus lays a foundation for the deployment of soft robots in human-centric environments with provable safety and performance. Index Terms—Modeling and Control for Soft Robots, Robot Safety, Soft Robot Applications I. INTRODUCTION DEPLOYING robots in human-centered environments, such as assisting workers in manufacturing or supporting older adults in everyday activities [1], demands not only demonstrable safety but also user confidence in the robot’s behavior. Traditional rigid collaborative manipulators address this need through increasingly sophisticated algorithms for Manuscript received: May 4, 2025; Revised August 6, 2025; Accepted September 24, 2025. This paper was recommended for publication by Editor Cecilia Laschi upon evaluation of the Associate Editor and Reviewers’ comments. ∗D. Rus and G. Zardini contributed equally as joint senior authors. 1Laboratory for Information and Decision Systems, Massachusetts Institute of Technology, Cambridge, MA 02139, USA {kiwan588, mstolzle, gzardini}@mit.edu.2Computer Science and Artificial Intelligence Laboratory, Massachusetts Institute of Technology, Cambridge, MA 02139, USA {weixy, rus }@mit.edu.3Cognitive Robotics, Delft University of Technology, Delft, 2628 CD, Netherlands {M.W.Stolzle, C.DellaSantina}@tudelft.nl.4Robotics Engineering Department, Worcester Polytechnic Institute, Worcester, MA 01609, USA [email protected]. The work by K. Wong was supported by The Hong Kong Jockey Club Scholarships; The work by M. St¨ olzle was supported under the European Union’s Horizon Europe Program from Project EMERGE - Grant Agreement No. 101070918, and by the Cultuurfonds Wetenschapsbeurzen 2024 and the Rudge (1948) and Nancy Allen Chair for his research visit to LIDS/Zardini Lab at MIT. Digital Object Identifier (DOI): see top of this page. Safe & Gentle Interaction Safe Soft Robot Applications Search & Rescue Medical Scenarios Contact is no longer avoided but controlled. Robots can move closer—safely—by controlling how hard they touch their environment. Fig. 1: Contact Safety-Aware Control of Soft Robots with HighOrder CBFs (HOCBFs) and High-Order CLFs (HOCLFs). Illustration of compliant contact control with safety bounds guaranteed by HOCBFs. By respecting contact force limits, the robot can intentionally engage with its surroundings without sacrificing safety. Task goals are shaped through HOCLFs, while constraint satisfaction is upheld by HOCBFs. This approach enables the secure use of soft robots in demanding settings—from search-and-rescue missions to delicate medical procedures. collision detection [2], impedance control [3], Model Predictive Control (MPC) [4], and, more recently, Control Barrier Functions (CBFs) [5], [6], and the successful integration of Lyapunov-based methods with reinforcement learning to ensure robotic safety [7]. Yet, perception errors or model inaccuracies can still expose users to hazardous impacts. Continuum soft manipulators offer a fundamentally different path to safety: instead of relying solely on software, they seek to embed safety directly through compliance [8]. However, material softness is not a panacea [9], [10]. As the field advances toward greater precision and functionality, emerging designs are expected to incorporate increased stiffness, exert larger forces and velocities [11], and adopt hybrid rigidsoft architectures [12]. Such developments reintroduce risks traditionally associated with rigid systems. Thus, mechanical compliance must be augmented with algorithmic guarantees that ensure real-time safety and foster user trust. Our approach embraces, rather than avoids, physical contact1[10], [13], [14], deviating from the predominant paradigm in rigid robotics [2], [15]. Instead of treating contact as a failure, we exploit the soft robot’s embodied intelligence [16]—its intrinsic physical coupling with the environment—to enhance robustness, stabilize deformation and motion, and adapt to external constraints, all while ensuring that every interaction 1Please note that we use “contact” and “collision” interchangeably. 2 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED SEPTEMBER, 2025 respects safety standards such as the injury severity thresholds set out in ISO/TS 15066:2016 [15]. To date, no method explicitly enforces upper bounds on the contact force or pressure applied across the entire surface of a soft robot, while accounting for the system’s inertia. Classical impedance and force control schemes cannot impose strict bounds [17], and CBF-based methods [12] address only self-contact avoidance. A recently published paper [10] adapts CBFs as a safety filter to constrain end-effector forces, but it neglects distributed body interactions and relies on a simplified template model that approximates the continuum bending behavior with an articulated chain of masses connected by prismatic joints [17] while neglecting important strains such as shear or elongation. Instead, Xu et al. [14] measures contact forces between the soft robot body and obstacles in the environment, but defines a kinematics-aware CBF instead of a dynamics-aware HOCBF; therefore, it neglects the dynamics of the system, which can, in turn, cause safety issues. To fill this research gap, we present an integrated control framework that imposes contact-force limits along the entire body of a soft manipulator based on differentiable dynamic strain models. Building on the well-established CBF+Control Lyapunov Function (CLF) framework [5]—specifically its high-order extension [18]—our method optimizes for a control objective encoded in an HOCLF while keeping the system’s trajectory inside a certified safe set as specified by HOCBF constraints by solving a constrained Quadratic Program (QP) online. The proposed HOCBF+HOCLF control scheme rests on two pillars: (i) a fully differentiable implementation of the Piecewise Constant Strain (PCS) [19], [20], and (ii) a fast, differentiable collision detection & resolution routine that represents the soft manipulator with convex-polygon approximations. In support of pillar (ii), we propose a new convex-polygon distance measure—Differentiable Conservative SAT (DCSAT)—that serves as a conservative, differentiable proxy for the standard Separating Axis Theorem (SAT) metric. Compared with recent differentiable SAT surrogates such as Smooth SAT (SSAT) [21], our approach (1) systematically underestimates the separation distance, yielding the conservative buffer required for formal safety guarantees, and (2) achieves an increase of roughly 1.5–3× in computational efficiency, enabling real-time, full-body collision checks. We then validate the proposed framework through extensive simulations in a planar setting. In summary, this letter (i) develops a principled HOCBFbased method to enforce global contact force constraints on soft robots, (ii) adapts the HOCBF+HOCLF framework for operational space regulation supporting navigation tasks, and (iii) presents DCSAT, a new, fast, and conservative differentiable collision-detection method for arbitrary convex geometries, improving the feasibility of real-time use for full-body safety guarantees. A video attachment is available on YouTube2and the code is open-sourced on GitHub3. 2https://youtu.be/ahUhVXiRDPE 3https://edu.nl/yekue II. BACKGROUND This section introduces the background necessary for introducing the methodology and, later, the baseline methods. For clarity and simplicity, we focus on the planar case throughout this letter, though the framework naturally generalizes to 3D scenarios. A. Soft Robotic Kinematics and Dynamics We model the soft robot kinematics using the PCS formulation [19], which approximates the continuous backbone by discretizing it into Nsegments, where each segment exhibits the spatially constant strain ξi∈R3. The robot configuration qis then defined as the deviation of these strains from their equilibrium values, yielding a generalized coordinate vector q∈R3N. Based on this kinematic model, the forward kinematics map χ= FK(q, s) : R3N×(0, L]→SE(2) returns the Cartesian position χ=θpxpy⊤at a given arc-length position salong the backbone, where θ∈[−π,π) is the planar orientation, px, pyare the xand y-positions, and L∈Ris the total arc length of the robot’s centerline. Differentiating FK(q, s)with respect to the configuration results in the Jacobian J(q, s) = ∂FK(q,s) ∂q∈R3×3N. The corresponding dynamics can be derived leveraging established multibody modeling procedures [22], resulting in the following equations of motion: M(q)¨ q+Cq,˙ q˙ q+G(q) | {z } Multibody dynamics +Kq +D˙ q | {z } Elast. and Diss. Forces =A(q)u | {z } Act. Model +τc |{z} Contact ,(1) where M(q),C(q,˙ q)∈R3N×3Nare the mass and Coriolis matrices, respectively, G(q)captures gravitational effects, and K,D∈R3N×3Nare the linear stiffness and damping matrices, respectively. The actuation matrix A(q)∈R3N×m maps the actuation/control input u∈ U ⊂ Rminto generalized torques and τc∈R3Naccounts for forces and torques generated by contact with the environment. Furthermore, J+ ο,M(q)∈R3N×οis the dynamically consistent pseudo-inverse of the Jacobian Jοthat maps into the operational space ο∈Rο[3], [17], [23], [24]. B. Model-Based Operational Space Controller A model-based setpoint regulator uο(q,˙ q) : R6N→∈ R3N that drives the system towards the desired operational space reference οdin exponential time can be designed based on the operational space dynamics [3], [17], [23], [24] uο(q,˙ q) = J⊤ ο(q)f+G(q), f=Kpeο+KiZeοdt+Kd˙ eο | {z } Operational Space PID +J+⊤ ο,M(Kq +D˙ q) | {z } Cancel. Elastic & Diss. Forces ,(2) where f∈Rοis a Cartesian force that serves as the control input in operational-space, eο(t) = οd(t)−ο(t)is the regulation error in operational space, and Kp,Ki,Kd∈Rο×ο are the corresponding PID gain matrices. C. High Order Control Barrier Functions and High Order Control Lyapunov Functions The soft robot dynamics can be expressed in control-affine form as an ODE: ˙ x=˙ q M−1(τc−C˙ q−G+Kq −D˙ q) | {z } f(x) +03N×m M−1A(q) | {z } g(x) u,(3) WONG et al.: CONTACT-AWARE SAFETY IN SOFT ROBOTS USING HIGH-ORDER CONTROL BARRIER AND LYAPUNOV FUNCTIONS 3 with the locally Lipschitz continuous dynamics functions f(x) : Rn→Rnand g:Rn→Rn×m, where x= q⊤˙ q⊤⊤∈Rnwith n= 6Nis soft robot state with the corresponding time derivative ˙ x∈RnIn many soft robotic control tasks, constraints specified in operational space do not yield an explicit dependence on the input uafter a single time derivative. This motivates the use of high-order extensions of CLFs and CBFs, where constraints are enforced on higherorder derivatives that expose the control input explicitly [18]. Given a function bwith relative degree r, one can recursively build an HOCBF and HOCLF as follows [18]. Definition 1 (High-Order Control Barrier Function [18]): Let b:Rn→Rbe r-times differentiable with relative degree r. Define recursively: ψ0:= b, ψi:= Lfψi−1+αi(ψi−1), i = 1, . . . , r−1, where each αiis class-K. Let Ci:= {x|ψi−1(x)≥0}. Then bis a HOCBF if there exists αr∈ K such that: sup u∈ULr fb+LgLr−1 fbu+O(b) + αr(ψr−1)≥0,∀x∈ r \ i=1 Ci. Definition 2 (High-Order Control Lyapunov Function): Let V: Rn→Rbe a differentiable function of relative degree r, and define: ϕ0:= V, ϕi:= Lfϕi−1+βi(ϕi−1), i = 1, . . . , r−1, with each βi∈ K. Then Vis a HOCLF if there exists βr∈ K∞ such that: inf u∈ULr fV+LgLr−1 fVu+O(V) + βr(ϕr−1)≤0,∀x=0n. Here, Lf,Lgdenote Lie derivatives along f,grespectively, and O(·)collects all lower-order Lie and time derivatives up to degree r−1. The HOCBF (1), and the HOCLF (2) can be integrated into a QP convex optimization problem: min u,δ∥u∥2 2+pδ2, s.t. Lr fb(x) + LgLr−1 fb(x)u+Ob(x)+αr(ψr−1(x)) ≥0, Lr fV(x) + LgLr−1 fV(x)u+O(V(x)) + βr(ϕr−1(x)) ≤δ. (4) To keep the QP feasible when several HOCBFs and HOCLFs contradict each other, we add a non-negative slack δ≥0with penalty p > 0. This slack relaxes safety constraints when strict enforcement is impossible due to the nominal input or mutual conflicts. Typically, HOCLFs capture performance objectives and HOCBFs safety and other constraints; assigning slack to lower-priority terms lets the controller trade performance for safety. III. HIGH-ORDER CONTROL BARRIER AND LYAPUNOV FUNCTION FOR ENVIRONMENT-AWARE CONTROL This section describes how we can design HOCBFs and HOCLFs for environment-aware soft robot control. Most importantly, we can ensure contact force limits, and with this, safety, by deploying HOCBFs while relying on HOCLFs for defining motion behavior and objectives. To enable this, we require access to differentiable algorithms that perform collision detection and resolution between the soft robot body and convex polygonal environment obstacles. We consider a planar soft robotic arm operating within a two-dimensional workspace W ⊂ R2, populated by nobs known convex obstacles Wobs ={O1,...,Onobs }. We also assume the robot is fully actuated, with m= 3N=n 2. A. Collision Detection To facilitate collision detection and spatial reasoning, the soft robotic arm, though continuously deformable, is approximated as a discrete chain of convex polygonal parts, as illustrated in Fig. 2. Specifically, the robot is segmented into Nsrpoly convex polygons R= (P1, . . . , PNsrpoly ), each defined by its vertices {vi,1,...,vi,ki}, where kiis the number of vertices of the ith part. These vertices are computed via the forward kinematics FK(q, s)defined earlier. Each polygon is represented as: Pi=nx∈R2|x= ki X j=1 αi,jvi,j,αi,j ≥0, ki X j=1 αi,j = 1o.(5) The entire robot is then R= (Pi)Nsrpoly i=1 . This definition now allows us to detect collisions between the Nsrpoly polygons approximating the robot body and the environment approximated by Nobs static convex polygons. In order to do so, we need to know the configuration-dependent distance hi,j(q) : R3N→Rbetween the ith soft robot’s part Piand the jth obstacle Oj. Here, a positive hi,j(q)indicates separation, while a negative value indicates penetration. This distance is provided by a polygon distance metric d(·,·)s.t. hi,j(q) = d(Pi(q),Oj), i = 1, . . . , Nsrpoly, j = 1, . . . , nobs, between the ith soft robot’s part Piand the jth obstacle Oj. For notational simplicity, we omit the explicit dependence on the configuration variable qand write hi,j in the remainder of this section. In principle, any differentiable distance function d(·,·)can be used. In Section IV-B, we present DCSAT, which is a differentiable, computationally efficient, and conservative version of the SAT algorithm. B. Collision Resolution The distance metric h(q)now allows us to resolve the collision, which means that we project the collision forces onto the soft robot dynamics and vice versa. In this work, we specifically, without loss of generality, use a linear springdamper model to capture the collision characteristics. Future work might explore more advanced contact models here. The collision force Fc∈R≥0is given as Fci,j (hi,j,˙ hi,j) = (0,if hi,j >0 −kchi,j −cc˙ hi,j,if hi,j ≤0 where kcthe contact stiffness, and ccthe damping coefficient. To recover differentiability, we approximate the collision dynamics using softplus-based smoothing with ε∈R≥0as Fci,j (hi,j,˙ hi,j) = kcln 1 + e−hi,j /ε+ccln 1 + e−˙ hi,j /ε. (6) This contact force can now easily be applied to both the environment and the soft robot by projecting it along the contact surface vector. Specifically, for a given surface normal nci,j ∈R2pointing from the obstacle to the soft robot body with ∥nci,j ∥2= 1 and a contact position pci,j ∈R2, the 4 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED SEPTEMBER, 2025 generalized contact torque onto the soft robot can be described by τc= Nsrpoly X i=1 Nobs X j=1 J⊤ ci,j (q)Fci,j (hi,j,˙ hi,j)nc∈R3N,(7) where Jci,j ∈R2×3Nis the positional Jacobian of the contact point on the surface of the soft robot that is: ˙ pc=Jx,y + diag(−1,1) pci,j −FKx,y(q, sci,j )⊤Jθ | {z } Jci,j (q) ˙ q(8) with sci,j ∈(0, L]the point on the backbone of the robot closest to the contact position pci,j and Jθ(q, sci,j )∈R1×3N and Jx,y(q, sci,j )∈R2×3Nthe orientation and positional rows of the forward kinematics Jacobian J(q, sci,j ), respectively. C. Ensuring Safety via High-Order CBFs Below, we introduce a set of relative-degree-two HOCBFs dedicated to maintaining safety. We start with an HOCBFs that is standard in rigid-robotics applications and ensures complete avoidance of contact between the robot and its environment. While effective, these constraints can sharply curb performance, encourage overly cautious behavior, and block truly collaborative human–robot interactions. To overcome such drawbacks, we highlight a HOCBF limiting the contact force to Fc,max ∈R>0. This approach allows controlled contact between the soft robot and its surroundings while guaranteeing that such contact remains safe [10]. Our formulation is inspired by extensive studies on injury-severity criteria for rigid collaborative robots [2] and by ISO/TS 15066:2016 [15], which specifies body-part–dependent force thresholds as proxies for injury risk. 1) Contact Avoidance HOCBF. To ensure safety with respect to a forbidden region A⊂W, we define a HOCBF based on the signed distance between the robot segment Piand A. Let rsafe ≥0be a prescribed safety margin. Using the previously defined smooth distance metric hi,A(q) = d(Pi(q),A), we define bi,A(x) = hi,A(q)2−r2 safe.(9) Then bi,A(x)≥0guarantees that Pimaintains a distance of at least rsafe ≥0from A. 2) Contact Force Limit HOCBF. For each obstacle Oj∈ Wobs, define bi,j(x) = Fc,max,j −Fci,j (hi,j,˙ hi,j),(10) where Fci,j (hi,j,˙ hi,j)≥0is the contact force, for example, stemming from a linear spring-damper contact model as defined in (6), and Fc,max,j is maximum allowable contact force in static settings as, e.g., defined in ISO/TS 15066:2016 [15]. This barrier function ensures that the contact force between the ith soft robot part and the jth obstacle remains below the threshold. D. Achieving Effective Motion Behavior via Higher-Order Control Lyapunov Functions With the barrier conditions established, the next step is to define motion objectives that encourage task completion. Although the HOCLF framework affords substantial flexibility in Non-Collision (Separation) Collision (Overlap) A B A B Fig. 2: Illustration of SAT Polygon Distance Metrics. Visualization of SAT-based polygon distance metrics used for collision detection between the soft robot body and convex polygonal obstacles. Specifically, we illustrate the definition of the signed distance hSAT(A, B) between convex polygons Aand B. The right panel shows projection intervals IA,a and IB,a along the maximizing axis amax in both separation (top) and overlap (bottom) scenarios. defining task objectives, this letter concentrates on operationalspace regulation; comparable objectives could also be formulated for trajectory tracking, configuration-space regulation and contact-force control. Operational Space Regulation HOCLF. Let pi(q)∈R2 be the Cartesian pose of the tip of the ith segment and pgoal ∈ R2the desired target. Then the HOCLF function Vtsr,i(x) = ∥pi(q)−pd i∥2 2(11) encourages convergence of the tip of the ith segment toward the target position. IV. DIFFERENTIABLE POLYGON DISTANCE METRIC For HOCBF constructions requiring an rth-order continuously differentiable distance metric, the classical SAT [25], despite its efficiency for convex polygons and widespread use over support mapping methods such as the Gilbert-JohnsonKeerthi (GJK) algorithm [26], is fundamentally unsuitable due to its reliance on non-smooth min and max operations. This lack of differentiability prevents its direct application in forceor distance-based high-order control formulations. To address this limitation, prior work has introduced a differentiable variant of SAT, named SSAT, that approximates these non-differentiable operations using multi-level smoothing techniques to achieve C∞continuity [21]. However, the SSAT overestimates the polygon separation distance, which can lead to a violation of the safety constraint in downstream applications. Alternatively, methods like DCOL [27] offer differentiable collision detection for convex primitives, but they cannot quantify penetration depth and are therefore unsuitable for collision resolution (e.g., computing the contact force). Similarly, the randomized smoothing approach [28] and the accelerated optimization-based method [29] focus on differentiable collision checking and computational speed-up, respectively, but do not return differentiable signed distance or WONG et al.: CONTACT-AWARE SAFETY IN SOFT ROBOTS USING HIGH-ORDER CONTROL BARRIER AND LYAPUNOV FUNCTIONS 5 penetration depth, which limits their use in differentiable contact dynamics. In this letter, we propose a new differentiable variant of SAT, coined DCSAT, which replaces the layered smoothing pipeline of SSAT [21] with a single LogSumExp (LSE) approximation while providing a conservation metric for calculating distance between convex polygons. DCSAT preserves the C∞differentiability required for high-order control while significantly simplifying implementation and reducing computational overhead. A. Definition of SAT Before defining our distance metrics, we briefly revisit the SAT framework, which forms the foundation of convex polygon–distance computations based on orthogonal projections. Definition 3 (Separating axis): Let A, B ⊂Rdbe convex sets. A unit vector a∈Rdis called a separating axis for Aand Bif the projections of the sets onto aare disjoint; that is, if either max x∈Aa⊤x<min y∈Ba⊤yor max y∈Ba⊤y<min x∈Aa⊤x. Theorem 1 (Separating Axis Theorem [25]): Two convex sets Aand Bin Rdare disjoint if and only if there exists a separating axis between them. Lemma 1 (Sufficiency of Edge Normals in R2): Let A, B ⊂ R2be convex polygons. Then it suffices to test for separation along the set of directions orthogonal to the edges of Aand B. Specifically, let S(A)and S(B)denote the sets of edge directions of Aand B, respectively. Define A=ℓ⊥:ℓ∈S(A)∪S(B), where ℓ⊥denotes the unit vector orthogonal to edge ℓ. Then Ais a complete set of candidate separating axes. Proof. By the Separating Axis Theorem (Theorem 1), if A and Bare disjoint, there exists a direction n∈S1such that their projections onto ndo not overlap: max x∈A n⊤x < min y∈B n⊤yor max y∈B n⊤y < min x∈A n⊤x. Since the support function of a convex polygon is piecewise linear and attains its extrema at vertices, any separating direction must be orthogonal to some edge of Aor B. Therefore, it suffices to test separation along directions in A. If no such direction yields separation, Aand Bmust intersect. By combining Theorem 1 and Lemma 1, separation testing—and more specifically, the computation of separating distances—can be reduced to a finite set of one-dimensional projections. These observations underlie the projection-based metrics (SAT, SSAT, DCSAT) used throughout this letter. Following by the above statements, we could derive the distance between two polygons A and B. Projections. Let Ai, i ∈ IA,Bj, i ∈ IBdenote the vertices of convex sets A, B, respectively, where IAand IBdenote the index sets of the vertices of A, B respectively. For each axis a∈ A, the scalar projections of polygon vertices Aifor i∈ IA, and Bjfor j∈ IB, onto aare defined as Ai,a =a⊤Ai and Bj,a =a⊤Bj. Per-axis distance. The signed separation (positive if separated, negative if overlapping) between the projected intervals along axis ais given by gSAT(a) = maxd1(a), d2(a).(12) where d1(a) = min j∈IB Bj,a −max i∈IA Ai,a, d2(a) = min i∈IA Ai,a −max j∈IB Bj,a, (13) Global metric. The overall SAT-based signed distance between polygons Aand Bis then defined as hSAT(A, B) = max a∈A gSAT(a).(14) This value is positive when the polygons are separated, zero if they touch, and negative when they overlap. B. Differentiable Conservative SAT (DCSAT) To enable safe and differentiable distance evaluation for use in HOCBF+HOCLF controllers, and resolve the underestimation of the polygon extents present in SSAT [21], we introduce the DCSAT metric, which provides a conservative estimate of the polygon separation/penetration distance at an increased computational efficiency compared to SSAT. Unlike SSAT, which smooths each intermediate geometric operation and is primarily designed for quadrilateral shapes, DCSAT operates directly on global signed separation distances. It applies a single LSE operation to obtain a C∞approximation of the SAT metric that naturally extends to arbitrary convex polygons. Furthermore, DCSAT consistently underestimates the true separation distance, ensuring that safety constraints enforced via CBFs remain valid even under model uncertainty or near-contact conditions. This makes it particularly suitable for collision-aware control of systems with complex geometries. Definition 4 (Differentiable Conservative SAT (DCSAT)): Let A, B ⊂R2be convex polygons, and let Adenote the set of separating axes. For each axis a∈ A, define the separation terms d1(a)and d2(a)as in (12). Let D={dm(a)|a∈ A, m ∈ {1,2}} . Then, the DCSAT distance is defined as hDCSAT(A, B) := 1 αmax log X d∈D eαmaxd!−log(2|A|) αmax . Lemma 2 (Bounds for DCSAT): −log(2|A|) αmax ≤hDCSAT(A, B)−hSAT(A, B)≤0. Proof. Let M= max D=hSAT(A, B), and define e:= 1 αmax log Pd∈D eαmaxd. Lower bound: One term in the sum is eαmaxM, so Xeαmaxd≥eαmaxM⇒e≥M ⇒hDCSAT(A, B)−M≥ −log(2|A|) αmax . Upper bound: Since |D| = 2|A|, we have Xeαmaxd≤2|A| · eαmaxM⇒e≤M+log(2|A|) αmax ⇒hDCSAT(A, B)−M≤0. Theorem 2 (Conservative Approximation via DCSAT): Let 6 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED SEPTEMBER, 2025 01234 x 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 y SAT Path α=5 α=10 α=50 (a) SSAT [21] vs. SAT 01234 x 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 y SAT Path α=5 α=10 α=50 (b) DCSAT (ours) vs. SAT Fig. 3: Qualitative Benchmarking of DCSAT. Comparison of zerolevel contours between smoothed / differentiable polygon distance metrics and the classical SAT. Specifically, we compare the position of the square polygon at a zero distance with the 8-sided polygon according to the respective distance metric. In both cases, we report the resulting contours for various sharpness parameters α, and the gray dashed curve represents the true zero-level set of the classical, but not differentiable SAT (i.e., the ground-truth), where the centroid of polygon Bis in contact with polygon A. e:= 1 αmax log X d∈D eαmaxd!, hDCSAT(A, B) := e−log(2|A|) αmax . Then: (a) hDCSAT(A, B)∈C∞, (b) hDCSAT(A, B)≤hSAT(A, B)for all A, B, (c) If hDCSAT(A, B)=0, then the true SAT distance satisfies 0≤hSAT(A, B)<log(2|A|) αmax .(15) Proof. (a) Since eis composed of exponential and logarithmic operations over a finite sum of smooth terms, it follows that e∈C∞. Subtracting a constant preserves smoothness, hence hDCSAT ∈C∞.(b) Follows directly from Lemma 2. (c) If hDCSAT(A, B) = 0, then by definition, e=log(2|A|) αmax . From Lemma 2, it follows that 0≤hSAT(A, B)≤log(2|A|) αmax , since hDCSAT ≤hSAT, with hDCSAT = 0. which confirms the conservative approximation of the real distance: if the smoothed metric DCSAT hits zero, the true SAT distance remains non-negative. The DCSAT procedure is summarized in Algorithm 1. Algorithm 1 Differentiable Conservative SAT (DCSAT) Require: Convex polygons A={Ai},B={Bj}; sharpness α>0 1: A←{ℓ⊥:ℓ∈S(A)∪S(B)} 2: for all a∈ A do 3: d1←minj(a⊤Bj)−maxi(a⊤Ai) 4: d2←mini(a⊤Ai)−maxj(a⊤Bj) 5: Add d1, d2to D 6: end for 7: h←1 αlog Pd∈D eαd−log(2|A|) α 8: return h TABLE I: Average runtime (RT) in milliseconds over 1,000 trials for a batch of 32 four-sided robot segment–obstacle polygon pairs, where each obstacle polygon has Nsides. All polygons are aligned to be just in contact. Speedup is measured relative to SSAT. Distance errors are computed as the mean of the deviation from the SAT metric normalized by the obstacle size. Additionally, we report the minimum and maximum observed across all pairs. NSSAT RT ↓DCSAT RT ↓SAT RT (ours) ↓Speedup ↑SSAT Err. [min, max] DCSAT Err. [min, max] 4 0.046 ms 0.031 ms 0.025 ms 1.51 -12.0% [-37.6%, -0.1%] -2.8% [-3.5%, -1.9%] 8 0.064 ms 0.041 ms 0.033 ms 1.56 -1.1% [-21.9%, +1.2%] -2.6% [-3.2%, -1.8%] 16 0.096 ms 0.053 ms 0.039 ms 1.81 +0.2% [-2.3%, +1.2%] -2.9% [-3.4%, -2.0%] 32 0.129 ms 0.064 ms 0.046 ms 2.00 +0.4% [-2.0%, +1.0%] -2.8% [-3.3%, -2.1%] 64 0.165 ms 0.076 ms 0.063 ms 2.17 +0.5% [-0.4%, +1.1%] -2.8% [-3.4%, -2.3%] TABLE II: Average runtime per segment–polygon pair (ms) with increasing batch size, the polygons have eight sides. Each value is averaged over 1000 trials. Speedup is measured relative to SSAT. Batch Size SSAT (ms) ↓DCSAT (ours) (ms) ↓Speedup ↑ 32 0.0708 0.0463 1.53 64 0.0946 0.0514 1.84 128 0.1330 0.0653 2.04 256 0.2356 0.0985 2.39 C. Benchmarking DCSAT While the original SSAT formulation [21] was designed specifically for rectangles or axis-aligned quadrilaterals, its underlying principle—approximating projection half-extents using smooth absolute value functions—can be generalized to arbitrary convex shapes. To enable a fair comparison across general geometries, we implemented this natural extension and evaluated it against our proposed DCSAT and the classical SAT [25]. Fig. 3 visualizes the zero-level contours of both smooth metrics relative to the classical SAT boundary. Notably, DCSAT consistently generates a conservative underestimation of the true separation distance, as verified by the consistently negative values in the DCSAT error column of Table I, ensuring that the smoothed constraint remains valid under model error or near-contact conditions. In contrast, SSAT [21] tends to overestimate the true separation at low sharpness levels, as can be seen in the positive values in the SSAT error column of Table I, which can compromise safety-critical guarantees in barrier-based control. Although both smoothed variants are implemented efficiently in JAX, DCSAT achieves comparable or better computational performance in large batch collision detections compared to SSAT [21] despite its global formulation. As shown in Table I, it maintains fast execution across polygon sizes while preserving full C∞smoothness. To evaluate scalability, we benchmark the runtime of our method (DCSAT) against SSAT with increasing batch sizes (Table II). Each batch contains multiple robot segment–obstacle polygon pairs, where each obstacle polygon has N= 8. Results show that DCSAT consistently outperforms SSAT in runtime, and the relative speedup grows with the batch size, from 1.53× at batch size 32 to 2.39× at batch size 256. Taken together, these results suggest that DCSAT offers a robust and practical surrogate to classical SAT for use in applications that require differentiability and a conservative approximation of the polygon separation distance. WONG et al.: CONTACT-AWARE SAFETY IN SOFT ROBOTS USING HIGH-ORDER CONTROL BARRIER AND LYAPUNOV FUNCTIONS 7 V. EXPERIMENTS A. Baseline Methods a) Safety Unaware HOCLF.: This baseline captures the typical approach found in current soft-robotic control research [22]: controlling the system without explicitly enforcing safety constraints. Here, we drop the HOCBF terms from the QP and retain only the HOCLF component—specifically, the Operational-Space Regulation HOCLF—within the QP constraints. b) Contact Avoidance Artificial Potential Field.: The purpose of this baseline is the represent the scenario that is widespread in the rigid robotics literature (e.g., collision avoidance [2]) - the aim to fully avoid contact - in this case via application of a repulsive Artificial Potential Field (APF) approach [30]. We adopt an integral-free (i.e., Ki= 0) variant of operational-space impedance controller from (2) to operate on positions of the soft robot segment tips with ο=p⊤ 1,...,p⊤ N⊤∈R2N. Combined with the repulsive artificial potential force frep, the resulting control law is: u=uο+ Nsrpoly X i=1 Nobs X j=1 J⊤ ci,j frepi,j , frepi,j =   krep h−1 i,j −r−1 safe h2 i,j nci,j ,if hi,j ≤rsafe 02,if hi,j > rsafe . (16) Please note that the proportional term of uοin (2) corresponds to force stemming from an attractive APF. c) Contact Avoidance HOCBF+HOCLF.: The purpose of this baseline is similar to the last one, but instead of relying on an artificial potential field to avoid contact and a classic operational space controller for incorporating the task objective, we rely here on the HOCBF+HOCLF framework by combining a Contact Avoidance HOCBF with a Operational Space Regulation HOCLF. d) Contact Force-Limit HOCBF Filter.: In this baseline, we replace the HOCLF objectives with a nominal operationalspace controller whose commands are filtered for safety by solving the QP with Contact Force Limit HOCBF constraints. Specifically, we employ uο(x)from (2), where the operationalspace is defined as ο=p⊤ 1,...,p⊤ N⊤∈R2N, representing the positions of the segment tips. B. Implementation & Simulation Details We build on the CBFpy [31] package that offers an easy-to-use and high-performance implementation of (highorder) CBF+CLF in JAX while leveraging analytical gradients obtained via autodifferentiation. Our simulations consider a planar, two-segment PCS soft robot (N= 2) implemented in the JSRM package [20]. Each segment is 0.13 m long with a backbone radius of 0.02 m; the elastic modulus is 2 kPa and the shear modulus is 1 kPa. The actuation matrix is set to the identity, A(q) = I3N. No state or input bounds are enforced. The closed-loop ODE is integrated with a numerical solver implementing Tsitouras’ 5/4 method. For the contact model, we choose kc= 3000 N/m, cc= 0 and set the contact force limit to Fc,max,j = 15 N and, in contact-avoidance scenarios, impose a minimum safety clearance of rsafe = 0.005 m, and a repulsive artificial potential field stiffness of krep = 3000 N/M. The εvalue for softplusbased smoothing force is 2×10−4. C. Navigation Results We implemented multiple scenarios to validate our framework. Among them, the search & rescue task is the most representative for comparing different baselines and is therefore highlighted in this letter (see supplementary materials for additional scenarios). Specifically, we compare the behavior of the Contact Force-Limit HOCBF+HOCLF control strategy against the baselines. We present the results in Fig. 4 and Fig. 5 as sequences of stills and time evolution plots, respectively. Indeed, the results demonstrate that the safety-unaware controller (1) generates high contact forces that are potentially unsafe, (2) the contact-avoidance baselines is not able to complete the task as it exhibits an overly conservative behavior and the soft robot is not able to exploit its embodied intelligence, (3) a model-based operational-space regulator with HOCBFbased safety filter is not able to complete the task as the control strategy is not able to resolve the conflicts between task objective and safety constraints, and (4) the proposed contactforce limit exhibits very good task performance while ensuring safety by restricting the maximum contact force. VI. CONCLUSION This letter introduces a HOCBFs+HOCLFs framework [18] for the control of soft robots, grounded in a differentiable PCS model [19], [20] and a novel differentiable and conservative polygon distance metric DCSAT. Our method allows (1) soft robots to embrace contact with the environment but ensure safety via contact force-limit HOCBFs evaluated along the entire soft robot body, (2) the flexible and expressive specification of control objectives, such as shape and endeffector regulation or object manipulation, via HOCLFs. The DCSAT metric offers a C∞approximation of the classical SAT, yielding conservative signed distances with reduced computational overhead compared to existing baseline methods that overestimate the distance, leading to potentially unsafe behavior. Simulated experiments in navigation scenarios validate the framework’s ability to maintain geometric safety and guide soft robots toward task objectives. Current limitations of the work include the focus on planar settings, the simplistic contact model, and the reliance on accurate proprioception and exteroception. REFERENCES [1] A. K. Hall, U. Backonja et al., “Acceptance and perceived usefulness of robots to assist with activities of daily living and healthcare tasks,” Assistive Technology, 2019. [2] S. Haddadin, Towards safe robots: approaching Asimov’s 1st law. Heidelberg: Springer Berlin, 2013, vol. 90. [3] O. Khatib, “A unified approach for motion and force control of robot manipulators: The operational space formulation,” IEEE Journal on Robotics and Automation, vol. 3, no. 1, pp. 43–53, 1987. [4] A. Pupa and C. Secchi, “Efficient iso/ts 15066 compliance through model predictive control,” in 2024 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2024, pp. 17 358–17 364. [5] A. D. Ames, X. Xu et al., “Control barrier function based quadratic programs for safety critical systems,” IEEE Transactions on Automatic Control, vol. 62, no. 8, pp. 3861–3876, 2016. 8 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED SEPTEMBER, 2025 Fig. 4: Search & Rescue Sequence of Stills. Sequence of stills for the system evolution in the search & rescue scenario for five different control paradigms. Contact interaction states are visualized with color cues: blue indicates no contact, green denotes safe contact, and red highlights contact above the maximum allowable force. The green cross denotes the task-space goal for this task. Safety Unaware HOCLF: Only optimizing for the task objective encoded in the HOCLF and disregarding safety constraints. Contact Avoidance Artificial Potential: Fully avoiding contact, which is the common paradigm in rigid robotics, using an artificial potential field [30], Contact Avoidance HOCBF+HOCLF: Fully avoiding contact using the HOCBF+HOCLF framework. Contact Force-Limit HOCBF Filter: Embracing contact with the environment while ensuring that the contact forces remain within safe bounds via an HOCBF-based safety filter applied to a nominal operational-space controller [17], [23], [24]. Contact Force-Limit HOCBF+HOCLF (proposed): Also ensuring that the contact forces remain within safe bounds, but instead of a safety filter, directly solving a QP with HOCBF and HOCLF constraints. [6] F. Ferraguti, M. Bertuletti et al., “A control barrier function approach for maximizing performance while fulfilling to iso/ts 15066 regulations,” IEEE Robotics and Automation Letters, vol. 5, no. 4, pp. 5921–5928, 2020. [7] M. Han, Y. Tian et al., “Reinforcement learning control of constrained dynamic systems with uniformly ultimate boundedness stability guarantee,” Automatica, vol. 129, p. 109689, 2021. [8] D. Rus and M. T. Tolley, “Design, fabrication and control of soft robots,” Nature, vol. 521, no. 7553, pp. 467–475, 2015. [9] M. St¨ olzle, N. Pagliarani et al., “Soft yet effective robots via holistic co-design,” arXiv preprint arXiv:2505.03761, 2025. [10] A. K. Dickson, J. C. P. Garcia et al., “Safe autonomous environmental contact for soft robots using control barrier functions,” IEEE Robotics and Automation Letters, vol. 10, no. 11, pp. 11 283–11 290, 2025. [11] D. A. Haggerty, M. J. Banks et al., “Control of soft robots with inertial dynamics,” Science robotics, vol. 8, no. 81, p. eadd6864, 2023. [12] Z. J. Patterson, W. Xiao et al., “Safe control for soft-rigid robots with self-contact using control barrier functions,” in 2024 IEEE 7th International Conference on Soft Robotics (RoboSoft). IEEE, 2024, pp. 151–156. [13] P. Rao, O. Salzman, and J. Burgner-Kahrs, “Towards contact-aided motion planning for tendon-driven continuum robots,” IEEE Robotics and Automation Letters, 2024. [14] F. Xu, X. Kang, and H. Wang, “Hybrid visual servoing control of a soft robot with compliant obstacle avoidance,” IEEE/ASME Transactions on Mechatronics, vol. 29, no. 6, pp. 4446–4455, 2024. 0 2 4 6 8 10 12 14 16 Time (s) 0 20 40 60 80 100 120 140 Contact Force (N) Safety Unaware HOCLF Contact Avoid. Artif. Potential Contact Avoid. HOCLF+HOCBF Contact Force-Limit HOCBF Filter Contact Force-Limit HOCLF+HOCBF Unsafe Zone (15N) (a) Contact force evolution 0 2 4 6 8 10 12 14 16 Time (s) 0.00 0.05 0.10 0.15 0.20 0.25 0.30 0.35 0.40 Regulation Error (m) Safety Unaware HOCLF Contact Avoid. Artif. Potential Contact Avoid. HOCLF+HOCBF Contact Force-Limit HOCBF Filter Contact Force-Limit HOCLF+HOCBF (b) Regulation error evolution Fig. 5: Search & Rescue Time Evolution. The contact force value, regulation error value across the robot for different controllers during asearch & rescue scenario. Refer to the caption of Fig. 4 for more details about the experiment and the considered controllers. [15] I. Standard, “Iso/ts 15066: 2016: Robots and robotic devices– collaborative robots,” International Organization for Standardization: Geneva, Switzerland, 2016. [16] G. Mengaldo, F. Renda et al., “A concise guide to modelling the physics of embodied intelligence in soft robotics,” Nature Reviews Physics, vol. 4, no. 9, pp. 595–610, 2022. [17] C. Della Santina, R. K. Katzschmann et al., “Model-based dynamic feedback control of a planar soft robot: trajectory tracking and interaction with the environment,” The International Journal of Robotics Research, vol. 39, no. 4, pp. 490–513, 2020. [18] W. Xiao, C. A. Belta, and C. G. Cassandras, “High order control lyapunov-barrier functions for temporal logic specifications,” in 2021 American Control Conference (ACC). IEEE, 2021, pp. 4886–4891. [19] F. Renda, F. Boyer et al., “Discrete cosserat approach for multisection soft manipulator dynamics,” IEEE Transactions on Robotics, vol. 34, no. 6, pp. 1518–1533, 2018. [20] M. St¨ olzle, D. Rus, and C. Della Santina, “An experimental study of model-based control for planar handed shearing auxetics robots,” in Experimental Robotics. Cham: Springer Nature Switzerland, 2024, pp. 153–167. [21] N. Takasugi, M. Kinoshita et al., “Real-time perceptive motion control using control barrier functions with analytical smoothing for sixwheeled-telescopic-legged robot tachyon 3,” in 2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2024, pp. 6802–6809. [22] C. Della Santina, C. Duriez, and D. Rus, “Model-based control of soft robots: A survey of the state of the art and open challenges,” IEEE Control Systems Magazine, vol. 43, no. 3, pp. 30–65, 2023. [23] M. St¨ olzle, S. S. Baberwal et al., “Guiding soft robots with motorimagery brain signals and impedance control,” in 2024 IEEE 7th International Conference on Soft Robotics (RoboSoft). IEEE, 2024, pp. 276–283. [24] M. St¨ olzle, “Safe yet precise soft robots: Incorporating physics into learned models for control,” Dissertation (TU Delft), Mechanical Engineering, Delft University of Technology, 09 2025. [25] W. Bittle, “Sat (separating axis theorem),” https://dyn4j.org/2010/01/sat/, 2010, accessed: 2025-04-29. [26] E. G. Gilbert, D. W. Johnson, and S. S. Keerthi, “A fast procedure for computing the distance between complex objects in three-dimensional space,” IEEE Journal of Robotics and Automation, vol. 4, no. 2, pp. 193–203, 1988. [27] K. Tracy, T. A. Howell, and Z. Manchester, “Differentiable collision detection for a set of convex primitives,” in 2023 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2023, pp. 3663–3670. [28] L. Montaut, Q. Le Lidec et al., “Differentiable collision detection: a randomized smoothing approach,” in 2023 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2023, pp. 3240–3246. [29] L. Montaut, Q. L. Lidec et al., “Collision detection accelerated: An optimization perspective,” in RSS 2022-Robotics: Science and Systems, 2022. [30] O. Khatib, “The potential field approach and operational space formulation in robot control,” in Adaptive and Learning Systems: Theory and Applications. Springer, 1986, pp. 367–377. [31] D. Morton and M. Pavone, “Safe, task-consistent manipulation with operational space control barrier functions,” arXiv preprint arXiv:2503.06736, 2025, accepted at IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Hangzhou, 2025.