## Abstract

In this work, we discuss the application of energy-based controller design for under-actuated soft robot manipulators. The continuous dynamics of the soft robot are modeled through the differential geometry of Cosserat beams. Using a finite-dimensional truncation, the system can be written as a reduced port-Hamiltonian model that preserves the passivity condition. Then, a model-based controller is introduced that produces a local minimizer of closed-loop potential energy for the desired end-effector configuration. The stabilizing control utilizes an energy-based approach and exploits the passivity of the soft robotic system. The effectiveness of the energy-based controller is demonstrated through extensive simulations of various soft robotic systems that share a resemblance with biology. All software and numerical studies are provided in an open-access SOROTOKI toolkit written in Matlab.

### Similar content being viewed by others

Avoid common mistakes on your manuscript.

## Introduction

The field of soft robotics is slowly growing as a prominent successor to conventional rigid robotics. Contrary to rigid robots, soft robots explore ‘*soft materials*’ that significantly enhance the robot’s dexterity, inherent safety, enable a rich family of motion primitives, and provide environmental robustness. By fully exploiting soft materials, soft robotics places the first steps towards achieving performance similar to biology [12, 16, 21]. In this work, we primarily focus on a subclass of soft robots called ‘*soft manipulators*’.

Although significant steps have been taken towards bridging biology and soft robotics, its innate infinite-dimensionality poses substantial challenges to modeling and control. To be more specific, soft robots theoretical allow for infinitely many degrees-of-freedom along their continuously deformable body. This renders them particularly suited for PDE models [15, 20, 33] rather than the conventional ODE for traditional robotics [22, 31]. Additionally, their actuation often employs distributed loads (e.g., pneumatics [16, 21] and tendons [32, 33]). Consequently, classical descriptions of rigid links and joints paired with local actuation are no longer viable nor physically representative. This paradigm shift calls for novel control-oriented modeling approaches tailored for hyper-flexible and under-actuated robotic systems.

In the last decade, the field of modeling for soft robotic systems has matured sufficiently and currently their applicability in model-based control is slowly feasible [14]. To highlight a few: reduced-order finite element models [15, 33, 34], constant and non-constant curvature approaches [13, 18], Cosserat-beam models [4, 27], and learning-based approaches [5]. The Piece-wise Constant Curvature (PCC) model—a popular method of state reduction that assumes piecewise constant strains along the soft robot’s body—has proven to be viable for modeling solution applicable to feedforward controllers [16], and more recently model-based feedback controllers [13, 18]. Nevertheless, the PCC approach has limitations. They do not originate from continuum mechanics and thus are only applicable in restrictive settings. Although computationally performance might surpass continuous models, due to intrinsic kinematic restrictions of the PCC models, they are unable to capture important continuum phenomena, like buckling, environmental interaction, wave propagation, or deflection due to gravity.

Cosserat-beam models, on the other hand, have shown to capture a wide range of continuum deformations. Cosserat models originate from continuum mechanical PDE description and thus allow a more accurate description of the hyper-flexible nature under large deformations. The computational dynamics of Cosserat beams have been extensively developed by [29] through Geometrically-Exact finite elements on the Lie group \({\text {SE}(3)}\); and recently, these models are slowly gaining popularity in the soft robotics community [4, 26, 27, 32]. Ultimately, the strong nonlinearities paired with the diligence to achieve biological performance encourages Cosserat models for control. Yet, compared to PCC, literature on model-based control for hyper-redundant soft robotic models, e.g., Cosserat model, is scarce but growing [2, 11].

In this work, we aim to highlight the capabilities of Cosserat models for model-based control by fully exploring its hyper-redundancy for end-effector setpoint regulation. Adopted from our prior work [9], a finite-dimensional modeling approach is proposed such that the continuous dynamics can be cast into a port-Hamiltonian (pH) structure. The Lagrangian modeling framework is adopted from [4] and [27], but modified to suit a pH-structure. Our approach has several advantages over prior PCC models [8, 10, 16, 18] : (i) addition of hyper-redundancy, and (ii) a common formalism with energy-based controller design. Exploit the pH structure, we propose an energy-shaping control law that ensures stabilization of the end-effector of the soft robot. Similar energy-based control strategies can be found in [2, 17, 23, 24, 28] but applied to rigid-body systems. As a study case, we consider a soft robot manipulator inspired by an octopus tentacle (see Fig. 1). With the ability to deform continuously and its distributed muscular system, it is ideal for illustrating the complex morphological motion present in soft robotics. Contrary to our previous work [10], we provide a fully open-source Matlab toolkit—called SOROTOKI [7]—and an in-depth analysis of the proposed strategies through extensive simulation analysis of several soft robotic systems. These tools allow researchers to easily developed various control architectures for a general class of soft robotic system. builds upon the previous work [9].

This work is organized as follows. “Dynamic Modeling” will detail a modeling approach for a general class of soft robot manipulators. In “Controller Design”, we propose an energy-shaping control strategy. Lastly, we show the effectiveness of energy-based controller through numerical simulation in “Simulation Study‑Cases”, followed by a brief conclusion in “Conclusion”.

## Dynamic Modeling

Throughout this work, we will explore Lie group theory and various notations that we briefly wish to introduce. We provided a nomenclature in Table 1. The following notations are adopted: the Lie group of rigid-body transformation about the origin of \(\mathbb {R}^3\) is denoted by \({\text {SE}(3)}\), whereas the group of homogeneous rotation is denoted by \({\text {SO}(3)}\). The tangent space at the identity is called the Lie algebra. The Lie algebra of \({\text {SE}(3)}\) and \({\text {SO}(3)}\) are denoted by \({\text {se}(3)}\) and \({\text {so}(3)}\), respectively. The mathematical operations related to the groups and their algebras, e.g., the adjoint actions, are also provided in the nomenclature 1.

### Cosserat Beam Theory

In Cosserat theory, slender deformable solids are modeled as elastic strings subjected to geometric finite-strain theory.Drawing an analogy with soft robotics, we model the soft robot as a one-dimensional spatial curve passing through the geometric center of the soft robot (see Fig. 1). A similar approach is followed in our previous work [9]. Given its spatial-temporal nature, we introduce a temporal variable \(t \in [0,\,T]\) with finite horizon time *T*, and a spatial variable \(\sigma \in [0,\,L]\) with *L* the undeformed length of the soft robot. For convenience, we denote \(\mathbb {T}= [0,\,T]\) and \(\mathbb {X}= [0,\,L]\). For each point on the backbone, we introduce a (mobile) coordinate frame. The homogeneous rotation related to these coordinate frames is given by the rotation matrix \(\varvec{\Phi }: \mathbb {X}\times \mathbb {T}\rightarrow {\text {SO}(3)}\), and their origin by the position vector \(\varvec{}{\gamma }: \mathbb {X}\times \mathbb {T}\rightarrow \mathbb {R}^3\).

Following the geometric approach [3, 4, 26, 27, 29], we may equivalently represent each coordinate frames that are rigidly attached to the continuous backbone of the soft robot by a parameterized space curve in \({\text {SE}(3)}\):

Now, an expression for the strain field \(\varvec{}{\xi }\) and velocity field \(\varvec{}{\eta }\) anywhere on the Cosserat beam can be found by exploring the differential geometry of the curve. To do so, we must introduce some smoothness criteria:

### Assumption 1

All control inputs, i.e., a distributed control wrench acting on the system, are considered to be sufficiently smooth for any instance \(t \in \mathbb {T}\) and \(\sigma \in \mathbb {X}\) such that the resulting parametrized backbone \(\varvec{g}(\sigma ,t) \in {\text {SE}(3)}\) is everywhere differentiable.

### Local Strain and Velocity

Following the works [4, 9, 26, 27], let \(\varvec{}{\Gamma } = (\kappa _1,\, \kappa _2, \kappa _3)^\top\) and \(\varvec{}{U} = (\nu _1,\, \nu _2,\, \nu _3)^\top\) be the torsion-curvature and elongation-shear strain vector, respectively. Then, an expression for strain field \(\varvec{}{\xi }(\sigma ,t)\) is obtained through spatial differentiation of \(\varvec{g}\):

Similarly, let \(\varvec{}{\Omega } = (\omega _1, \omega _2, \omega _3)^\top\) and \(\varvec{}{V} = (v_1,\,v_2,\, v_3)^\top\) be the angular and linear velocity vector, respectively. Then, an expression for velocity field \(\varvec{}{\eta }(\sigma ,t)\) is obtained through time differentiation of \(\varvec{}{g}\):

Since we assume \(\varvec{g}\) to be everywhere differentiable, we can derive a PDE for the continuous forward kinematics of the soft robot [4, 9, 27]:

where \({\textbf {ad}}_{(\cdot )}\) denotes the adjoint action on the Lie algebra (full derivation in Appendix 1). Drawing an analogy to rigid robotics, the expression in (4) may be seen as the forward kinematics for a serial chain manipulator with infinitely many links.

### Finite-Dimensional Reduction

Similar to finite element methods, we wish to find a finite-dimensional approximation of the strain field \(\varvec{}{\xi }(\sigma ,t)\) for all points on the material domain \(\mathbb {X}\). To do so, we assume the following [9]:

### Assumption 2

Assuming the strain field has a separable spatio-temporal nature, any entry of the strain vector field \(\varvec{}{\xi } = \left( \xi _1, \xi _2, \ldots , \xi _6 \right) ^\top\) can be written as an infinite expansion of the following form:

where \(\{\theta _{n}\}^\infty _{n=1}\) is the set of (orthogonal) basis functions \(\theta _{n}: \mathbb {X}\rightarrow \mathbb {R}\) together with modal coefficients \(q_{i,n}: \mathbb {T}\rightarrow \mathbb {R}\), and an intrinsic time-invariant strain \(\xi ^\circ _{i}: \mathbb {X}\rightarrow \mathbb {R}\). The basis functions \(\theta _{n}(\cdot )\) and the modal coefficients \(q_n(\cdot )\) are both smooth functions.

### Assumption 3

Given infinite expansion (5), the *k*-th order truncation for any entry of the strain field, defined as

converges uniformly on \(\mathbb {X}\) and \(\mathbb {T}\) as the index \(k \rightarrow \infty\). Moreover, we assume that uniform convergence holds for its partial derivatives \(\tfrac{\partial }{\partial t} [\varvec{}{\xi }]_k\) and \(\tfrac{\partial }{\partial \sigma } [\varvec{}{\xi }]_k\).

Accordingly, we can rewrite the *k*-th order truncation of the complete strain field as a linear matrix operation as follows

where \(\varvec{\Theta } \in \mathbb {R}^{6 \times 6k}\) is a sparse matrix-valued function whose columns are mutually orthonormal, the operator \(\otimes\) denotes the Kronecker product and the vector \(\varvec{}{q} \in \mathbb {R}^{6k}\) the collection of all time-variant modal coefficients related to the columns of \(\varvec{\Theta }\). Although a wide variety of bases are possible [4, 13], we have chosen a modified Legendre polynomial set:

with \(n \in \mathbb {Z}^+\) the polynomial degree. Please note now that the inner product between elements of the set of modified Legendre functionals \(\{\theta _n\}_{n = 1}^k\) satisfies \(\langle \theta _i, \theta _j \rangle _{\mathbb {X}} := \int _\mathbb {X}{\theta _i}{\theta _j} {\text {d}}\sigma = 0\) for \(i \ne j\), and 1 otherwise. An alternative option could be constructing the set of basis functions through the so-called ‘*snapshot decomposition method*’ using FEM-driven data [1, 15, 20].

### Finite-Dimensional Kinematics

Given the finite-dimensional truncation in (7), we can now find an expression for the finite-dimensional forward kinematics in terms of the generalized coordinates \(\varvec{}{q}\) and its velocities components \(\dot{\varvec{}{q}}\).

First, let us regard the configuration of the soft robot \(\varvec{g} \in {\text {SE}(3)}\). Recall that the spatial evolution of the curve is described by \(\partial \varvec{g}/\partial \sigma = \varvec{g} \varvec{}{\xi }^\wedge\), see Eq. (2). Given the initial condition \(\varvec{g}(0,\cdot ) = \varvec{g}_0\), an approximation of the continuously deformable soft robot can be obtained by partial integration over the spatial domain:

Please note that this nothing more than the reconstruction of the curve by integration of its tangent space along its spatial parameter \(\sigma\). Next, lets regard the velocity kinematics \(\varvec{}{\eta }(\sigma ,t)\) for any point \(\sigma\) on the backbone curve. Using the differential property of the adjoint action \(\varvec{{\textbf {ad}}}_{\varvec{}{\xi }} = -\partial /\partial \sigma [{\textbf {Ad}}_{g^{-1}}] {\textbf {Ad}}_{g}\) [22], we can rewrite the continuous forward kinematics in (4) as

Now, given the initial condition \(\varvec{}{\eta }(0,t) = \varvec{}{0}_6\) and the approximations \([\varvec{}{\xi }]_k\) and \([\varvec{g}]_k\), we can find an approximation to the velocity twist \(\varvec{}{\eta }\) by partial integration over space:

which naturally gives rise to the geometric Jacobian \([\varvec{J}]_k \in \mathbb {R}^{6\times 6k}\). The geometric Jacobian plays an important role in obtaining the Lagrangian form of the reduced-order dynamic model. Finally, to express the acceleration twist, we take the time-derivative of (11) leading to

which gives rise to the analytic expression of the time-derivative of the geometric Jacobian \(\dot{[\varvec{J}]}_k\) (see Appendix 2 for the derivation).

### Finite-Dimensional Dynamics

Here, we detail the dynamics of the Cosserat beam. A majority of the dynamic framework presented here is adopted from [4]; yet we introduce some modification to allow for the pH-structure. Earlier work of this extension can also be found in [9], but recapitulated here for to be self-contained. First, let us consider an infinitesimal slice of continuum body that is perpendicular to the backbone curve. The kinetic momenta of this infinitesimal slice are then given by \(\varvec{}{\mu }(\sigma ,t) = \varvec{\mathcal {M}}(\sigma ) \varvec{}{\eta }(\sigma ,t)\) in which \(\varvec{{\mathcal {M}}}(\sigma ) \in {\text {se}^*(3)} \times {\text {se}(3)} \cong \mathbb {R}^{6\times 6}\) denotes the symmetric (possibly space-varying) inertia tensor.

### Remark 1

For some soft robots, the inertia tensor \(\varvec{\mathcal {M}}\) may have an explicit dependency on space or time (or both). Nevertheless, for sake of simplicity, we limit ourselves to a diagonal invariant inertia tensor \(\varvec{\mathcal {M}} = \text {diag}\left\{ {\rho \varvec{I}_3, \rho \varvec{{\mathcal {J}}}} \right\}\) with line-density \(\rho > 0\) and the second moment of area \(\varvec{{\mathcal {J}}} \in {\text {so}^*(3)} \times {\text {so}(3)} \cong \mathbb {R}^{3\times 3}\).

Using the expression of the kinetic momenta \(\varvec{}{\mu }\) of the infinitesimal body, we can write the equation of motion for a particular slice at \(\sigma\) using the Newton-Euler equations:

where again \({\textbf {Ad}}_{(\cdot )}\) stands for the adjoint action, and \(\varvec{}{\mathcal {F}} = \varvec{}{\mathcal {F}}_c + \varvec{}{\mathcal {F}}_u + \varvec{}{\mathcal {F}}_g - \varvec{}{\mathcal {F}}_e\) the resultant wrench that is composed of the constraint wrench \(\varvec{}{\mathcal {F}}_c\), the input wrench \(\varvec{}{\mathcal {F}}_u\), and the potential wrenches due to gravity and visco-elasticity, \(\varvec{}{\mathcal {F}}_g\) and \(\varvec{}{\mathcal {F}}_e\), respectively. Further evaluation of (13) leads to

where we used the fact that \(\dot{{\textbf {Ad}}}_{\varvec{}{g}} ^{{-1}}= -{\textbf {ad}}_{\varvec{}{\eta }} {{\textbf {Ad}}}_{\varvec{}{g} ^{{-1}}}\). Before continuing, we introduce a slight modification to the relation above. Using the fact that \({\textbf {ad}}_{\varvec{}{\eta }} \varvec{}{\eta } = \varvec{}{0}_6\), we can introduce the vector \(\varvec{}{\mathcal {M}} {\textbf {ad}}_{\varvec{}{\eta }} \varvec{}{\eta }\) to (14) without affecting the dynamics. The importance of this modification originates from the preservation of passivity in the Lagrangian model, which is an important property in stability theorems for robotics. By substitution of the null vector, the equation of motion becomes

which is nothing more than the Newton–Euler equation for rigid-body motion on \(\mathbb {R}^3\). To introduce the (reduced-order) Cosserat kinematics and make the expression symmetric, we substitute (11) and (12) into (15) and pre-multiply by \([\,\varvec{}{J}\,]_k^\top\):

where \(\varvec{\mathcal {C}}_{(\cdot )} = -\varvec{\mathcal {C}}_{(\cdot )} ^\top := \varvec{\mathcal {M}} {\textbf {ad}}_{(\cdot )} - {\textbf {ad}}_{(\cdot )} ^\top \varvec{\mathcal {M}}\) is a skew-symmetric matrix. It is important to note that by pre-multiplication of the transpose Jacobian, we have eliminated the constraint wrenches, i.e., \([\,\varvec{J}\,]_k^\top \varvec{}{\mathcal {F}}_c = \varvec{}{0}_n\) [22]. Finally, the finite-dimensional dynamics of the deformable soft robot is found by spatial integration of (16) over the material domain \(\mathbb {X}\). The overall dynamics can be written in familiar Euler-Lagrangian (EL) form as follows

with the system matrices:

where \(\varvec{M}\) is the generalized inertia matrix, \(\varvec{C}\) the centripetal-Coriolis matrix, \(\varvec{N}\) a vector of generalized potential forces with \(\varvec{\mathcal {F}}_g = -{\textbf {Ad}}_{[\varvec{g}]_k}^\top \varvec{\mathcal {M}} \varvec{}{a}_g\) the external wrench acting on the body due to gravitational acceleration \(\varvec{}{a}_g\), and \(\varvec{}{\mathcal {F}}_e\) a vector of viscoelastic forces imposed by the stiffness matrix \(\varvec{K} \succ 0\) and damping matrix \(\varvec{}{D} \succ 0\). The system of matrices can then be efficiently recovered using a *Matrix-Differential* solver proposed in [10]. This leads to a fast evaluation of (17) as to allow for realtime simulation using precompiled mex files in Matlab. The vector \(\varvec{}{Gu}\) represents the distributed forces and torques generated by various kinds of the internal actuators (e.g., tendons or pneumatics). If \(\text {rank}(\varvec{G}) < \dim (\varvec{}{q})\), the system is considered to be under-actuated. Within the context of soft robotics, whose infinite-dimensional configuration space cannot be matched by a finite number of actuators, these systems are often intrinsically under-actuated. Following the procedures in finite elements and assuming linear visco-elasticity, the stiffness matrix and damping matrix are computed through spatial integration:

where \(\varvec{{\mathcal {K}}} \in {\text {se}^*(3)} \times {\text {se}(3)}\) and \(\varvec{{\mathcal {D}}} \in {\text {se}^*(3)} \times {\text {se}(3)}\) are the stiffness and damping tensor, respectively.

### Lemma 1

The inertia matrix \(\varvec{M}(\varvec{}{q})\) is a symmetric, positive definite, symmetric. and is uniformly bounded such that there exists positive constants \(\lambda ^{-} \le \lambda ^{+}\) such that \(\lambda ^{-} \varvec{I}_{n} \preceq \varvec{M}(\varvec{}{q}) \preceq \lambda ^{+} \varvec{I}_{n} < \infty\).

### Proof

Proof can be found in Spong et al. [31] \(\square\)

### Lemma 2

Given the inertia matrix \(\varvec{M}(\varvec{}{q})\) and the Coriolis matrix \(\varvec{C}(\varvec{}{q},\dot{\varvec{}{q}})\) as described by (18) and (19), respectively, it can be shown that the matrix \(\dot{\varvec{M}} - 2\varvec{C}\) is skew-symmetric.

### Proof

To show \(\dot{\varvec{M}} - 2\varvec{C}\) is skew-symmetric, we start by computing the time-derivative of the inertia matrix. For sake of clarity, lets abbreviate the geometric Jacobians matrices \([\varvec{J}]_k = \varvec{J}\) and \([\dot{\varvec{J}}]_k = \dot{\varvec{J}}\). Through chain differentiation, we find

Then, calculating \(\dot{\varvec{M}} - 2\varvec{C}\) leads to

Since the matrix \(\varvec{J}^\top \varvec{\mathcal {C}} \varvec{J}\) is skew-symmetric, the remainder of the proof is to show that the matrix \(\varvec{S} = \dot{\varvec{J}}^\top \varvec{\mathcal {M}} \varvec{J} - \varvec{J}^\top \varvec{\mathcal {M}} \dot{\varvec{J}}\) also satisfies skew-symmetry. Since the inertia tensor is symmetric \(\varvec{\mathcal {M}} = \varvec{\mathcal {M}}^\top\), we can easily show this holds true:

Therefore, the matrix \(\dot{\varvec{M}(\varvec{}{q})} - 2\varvec{C}(\varvec{}{q},\dot{\varvec{}{q}})\) is skew-symmetric for all \(\varvec{}{q},\dot{\varvec{}{q}} \in \mathbb {R}^n\) \(\square\)

In literature, Lemma 2 is often referred to as the passivity condition [22, 23, 31]. It implies that, in the absence of external dissipation, the total energy of the system (i.e., the Hamiltonian) is conserved. It is also worth mentioning that this condition does not necessarily hold true for all cases, only for particular computations of the Coriolis matrix \(\varvec{C}(\varvec{}{q},\varvec{}{\dot{q}})\) (e.g., through the Christoffel symbols).

### Port-Hamiltonian Formulation

In this section, the Lagrangian model in (17) is rewritten in port-Hamiltonian form similar to [9]. To this end, we define the generalized momenta \(\varvec{}{p} := \varvec{M} \dot{\varvec{}{q}}\). Then, the (reduced-order) Hamiltonian is given by \({\mathcal {H}}(\varvec{}{q},\varvec{}{p}) := \mathcal {K}(\varvec{}{q},\varvec{}{p}) + \mathcal {U}(\varvec{}{q})\) with \(\mathcal {K}= \frac{1}{2} \varvec{}{p}^\top \varvec{M}^{{-1}}\varvec{}{p}\) and \(\mathcal {U}(\varvec{}{q})\) the kinetic and potential energy of a reduced system, respectively.

Given the system’s Hamiltonian \({\mathcal {H}}\), it can be easily shown that generalized velocities can be written in terms of partial derivatives of the Hamiltonian function

where we denote \(\nabla _{\varvec{}{p}}(\cdot ) := \partial (\cdot )^\top /\partial \varvec{}{p}\) as the gradient w.r.t. the vector field \(\varvec{}{p}\). Similarly, we introduce \(\nabla _{\varvec{}{q}}(\cdot ) := \partial (\cdot )^\top /\partial \varvec{}{q}\). Note that \(\varvec{M}^{-1}\) is always exists due to the positive definiteness condition in Lemma 1. Similarly, we seek a differential description that relates the time evolution of \(\varvec{}{p}\) to a state-derivative of the Hamiltonian function. Applying the chain rule of differentiation to the generalized momenta:

Taking the partial derivative of the Hamiltonian \(\mathcal {H}\) w.r.t. the generalized coordinates \(\varvec{}{q}\), we find

To relate (29) and (30), we explore some structural properties in the Lagrangian model. To be more specific, we exploit the skew-symmetry condition as detailed in Lemma 2. According to the [31], if the matrix \(\dot{\varvec{M}} - 2\varvec{C}\) satisfies the passivity condition in Lemma 2, it can be shown that

Finally, by combining (28), (29), (30), and (31), we can show that the Lagrangian model in (17) can be equivalently rewritten as a port-Hamiltonian (pH) system:

The advantage of the port-Hamiltonian model over the standard EL structure in (17) is the general applicability to different physical domains and the common formalism with energy-based control, which we will explore further in the next section.

## Controller Design

Given the previous reduced-order dynamic model, our objective is to find a controller \(\varvec{}{u}\) that ensures \(\lim _{t\rightarrow \infty } \varvec{g}(L,t) = \varvec{g}_d\) in which \(\varvec{g}_d \in {\text {SE}(3)}\) denotes the desired configuration of the end-effector. To achieve the control objective, the main idea here is to reshape the potential energy function of the finite-dimensional system using a standard energy-shaping techniques which are common to port-controlled Hamiltonian models [24, 28].

### Energy-Shaping Controller

We adopted an energy-based control strategy akin to the work of [9, 17, 23, 24, 28]. Following a similar energy-shaping strategy, the model-based nonlinear controller takes the form:

where \(\varvec{G}^{{+}}= \bigl (\varvec{G}^\top \varvec{G}\bigr )^{{-1}}\varvec{G}^\top\) is the generalized inverse of the actuation map \(\varvec{G}\), and the desired Hamiltonian in closed loop \(\mathcal {H}_d = \frac{1}{2} \varvec{}{p}^\top \varvec{M}^{{-1}}\varvec{}{p} + \mathcal {U}_d\) that satisfies \(\text {argmin}_{\varvec{g}_L} \mathcal {U}_d = \varvec{g}_d\) with \(\varvec{g}_L = \varvec{g}(L,\cdot )\) the pose of the end-effector. Note that we purposefully omitted any damping injection as the system’s intrinsic visco-elastic damping is deemed sufficiently large to guarantee stability. Following the concept of a kinematic feedback controller [4, 6] that artificially mimic an elastic element between the end-effector and the desired configuration in \({\text {SE}(3)}\), we have choose the gradient of the desired potential energy as

where \(\lambda _1 >0\) is a proportional gain, \(\lambda _2 > 0\) a controller gain related to the damping of the pseudo-inverse, \(\varvec{}{\mathcal {F}}_u = k_p T_{SE(3)}(\varvec{{\mathcal {E}}})\varvec{{\mathcal {E}}}\) an artificial control wrench with the positive definite stiffness matrix \(k_p\), \(\varvec{{\mathcal {E}}} = \log _{{\text {SE}(3)}} ([\varvec{g}_L]_k ^{{-1}}\varvec{g}_d)\) where the mappings \(\log _{{\text {SE}(3)}}(\cdot )\) and \(T_{{\text {SE}(3)}}(\cdot )\) denote the logarithmic map and the tangent-space map, respectively. We refer the reader to [30] for the numerical computations of these geometric operations. The vector \(\varvec{}{{\mathcal {E}}}\) may be regarded as the geometric error between the homogeneous transformations \(\varvec{g}_d\) and \(\varvec{g}_L\) such that if \(\varvec{g}_d = \varvec{g}_L\) will simply yield \(\Vert \varvec{}{{\mathcal {E}}} \Vert _2 = 0\). Furthermore, the controller gains \(\lambda _1\) and \(\lambda _2\) can be tuned accordingly to tweak the desired transient behavior of the closed-loop system, similar to a classical PD controller.

## Simulation Study-Cases

### Model and Solver Setting

In this section, we detail the numerical simulations of the port-Hamiltonian model in (32) together with the energy-shaping controller in (33). For all numerical simulations, we consider a truncation degree of the finite-dimensional model is \(k = 8\).

Due to the partial differential nature, we have to employ a nested ODE routine to recover the trajectories for \(\varvec{}{q}\) and \(\varvec{}{p}\). First, we employ an implicit trapezoidal solver with a fixed stepsize of \(dt = 30\) ms to solve (32). At each time increment, we have to evaluate the dynamic matrices (18)–(22). To efficiently compute these dynamic entities, we solve the spatial integration problem over the material domain \(\mathbb {X}\) using a second-order Runge-Kutta solver. The stepsize for the spatial solver is \({\text {d}} \sigma = 5\) mm. All simulation examples and underlying source code are provided publicly on the SOROTOKI toolkit [7] written in Matlab. Here, the numerical integrations of the system matrices (18), (19), (21) and (22) are performed using a so-called Matrix-Differential solver (see [10]). The simulations are performed on a modern machine (Ryzen 7-5800H CPU @3.2GHz).

For the soft robotic simulations, we have chosen a linear isotropic Hookean material with shear constraints. Although hyper-elastic material models are possible, e.g, Neo-Hookean or Yeoh, it is considered out of the scope of this work (see [19, 25]). Given these material properties, the inertia tensor and the stiffness tensor become diagonal matrices:

where the (average) cross-sectional area, and \(\varvec{{\mathcal {J}}}\) the second moment of area for a disc with radius *R*. The damping tensor chosen as \(\varvec{{\mathcal {D}}} = \zeta \varvec{{\mathcal {K}}}\) with damping coefficient \(\zeta\). The stiffness and damping matrix are precomputed using (23) and (24).

### Soft Robot Manipulator Inspired by Octopus’ Tentacle

In the first study case, we consider a soft robotic arm that is loosely inspired by the tentacles of an octopus. To introduce the under-actuation typically present in soft robotics, we have chosen an actuation matrix \(\varvec{G} = \text {blkdiag}\left\{ {\varvec{I}_5, \varvec{O}_3} \right\}\) such that only the first five modes are actively controllable. The system properties can be shown in Table 2.

The soft robot is subjected to the energy-based controller in (33), where the control gains are tuned to produce a smooth transient: \(\lambda _1 = 0.01\) and \(\lambda _2 = 0.001\). The artificial spring stiffness is chosen as \(\varvec{k}_p = \text {blkdiag}\left\{ {0.01\cdot \varvec{I}_3, \varvec{I}_3} \right\}\). Lastly, the desired configuration of the end-effector is chosen as follows:

The numerical results of the closed-loop system are shown in Figs. 2 and 4. It is worth mentioning that these simulation run at \(\pm 60\) Hz real-time sampling frequency. Real-time bandwidth is determined by the ratio between finite horizon and CPU’s computation time, i.e., \(f \approx T/T_{\text {sim}}\). This ratio is affected most by spatial discretization.

Figure 2 shows the evolution of the continuous deformation along the soft robotic body, whereas Fig. 4 shows the modal coefficients \(\varvec{}{q}(t)\) and the spatial trajectory of the end-effector. As can be seen, the end-effector of the soft robot manipulator slowly converges to the desired set-point \(\varvec{g}_d \in {\text {SE}(3)}\). Although the control gains could be increased to promote a faster transient, it was observed that high gains lead to undesired (propagating) oscillations of the flexible structure. A possible solution might be to introduce negative damping to the controller Hamiltonian \(\mathcal {H}_d\), to overcome the soft robot’s structural damping.

For the second simulation run, we modified the control gains to highlight an interesting property of the proposed controller. To be more specific, we increase the controller gains to \(\lambda _1 = \lambda _2 = 0.1\). The numerical results for the increased controller gains are shown in Figs. 3 and 5. Although the control goal and the initial conditions are chosen identical, the soft robot converges to a different configuration—albeit, a shape with less ‘*complexity*’. The cause of less complicated bending patterns has two origins. First, increasing the control gains also artificially impacts the structural stiffness of the soft robot, resulting in a soft robot with higher perceived stiffness. Second, by increasing the stabilizing term \(\lambda _2\) in the damped Jacobian inverse (34), more weight is given towards finding a solution that also minimizes the join angles \(\Vert \varvec{}{q} \Vert _2\). As intrinsically, more energy is spent to excite higher-order modes in the basis \(\{\theta _n\}_{n=1}^k\), the energy-based controller will thus find a minimizer that accounts for the ordering of the shape basis, penalizing higher-order modes. This result indicates that the proposed controller can be effectively tuned alter the structural compliance of the soft robot; and thus could be implemented carefully to preserve ‘*softness*’.

### Multi-Link Soft Robot Inspired by the Elephant’s Trunk

In the second study case, we consider a two-link soft robot that is inspired by the trunk of an elephant. A similar soft robotic system is considered in [16], where mobility of the bio-inspired robotic system is achieved through a pneumatic-network distributed along the continuous body of the robot. Therefore, considering a six-bellow network, the actuation matrix takes the form:

where \(\{\varvec{}{\mathcal {F}}_n\}_{n=1}^6\) is a set of piece-wise constant wrench functionals related to the pneumatic actuation bellows distributed along the soft robotic body, and \(\varvec{}{u} = (u_1,\,u_2,\,u_3,\,u_4,\,u_5,\,u_6)^\top\) a vector of wrench amplitudes. The control input sets \(\{u_1,\ldots ,u_3\}\) relate to the first link and \(\{u_4,\ldots ,u_6\}\) to the second link of the robot. Given this input configuration, it also follows that \(\text {rank}(\varvec{G}(\varvec{}{q})) < \dim (\varvec{}{q})\) for all \(\varvec{}{q} \in \mathbb {R}^{6k}\), i.e., underactuated. The system and solver properties are given in Table 3; and we consider \(k=8\) spatial modes.

Again we apply the energy-based controller in (33) to the system, where the control gains are \(\lambda _1 = 5\) and \(\lambda _2 = 1\), while the artificial stiffness matrix \(\varvec{k}_p\) is kept identical to previous simulations. Lastly, the desired configuration of the end-effector is chosen as follows:

The numerical results of the closed-loop system are shown in Figs. 6 and 7; which could reach a real-time performance of \(\pm 26\) Hz. Figure 6 shows the continuous deformation along the soft robotic body, whereas Fig. 7 shows the trajectories of the modal coefficients \(\varvec{}{q}(t)\) and the spatial trajectory of the end-effector. As can be seen, the end-effector of the soft robot manipulator slowly converges to the desired set-point \(\varvec{g}_d \in {\text {SE}(3)}\), even when dealing with piece-wise distributed actuation loads applied to the continuous backbone. To describe the discontinuous actuation profiles, however, higher order modes are required as can be seen in Fig. 7. This might indicate there exist better tailored compact shape bases for this soft robotic system.

## Conclusion

The field of soft robotics is slowly maturing into a recognized subfield of robotics. Due to their intrinsic compliance, they allow for complex morphological motions that mimic animals in nature. Achieving similar performance to biology highlights the need for more accurate dynamic models and control strategies that fully exploit the hyper-redundant nature of soft robots. In this work, we provided a modeling framework for Cosserat beams that leads to a finite-dimensional system in a port-Hamiltonian structure. By exploiting the passivity, an energy-shaping controller was proposed that ensures the closed-loop Hamiltonian is minimal at the desired set-point. The numerical model was developed for a several bio-inspired soft robot (octopus’ tentacle and elephant’s trunk) with distributed control inputs. The key challenges here regarding both the model as the controller are their ability to capture the hyper-flexibility, deal with inherent under-actuation, and exploit its hyper-redundancy to achieve its control task. Given appropriate controller gains, the model-based controller yields smooth convergence of the soft robot’s end-effector while accounting for under-actuation. It was shown that by tuning the controller gains, the intrinsic stiffness of the soft body can be adapted, resulting in significantly different quasi-static joint solutions to the set-point problem. To some extent, the mobility of the Cosserat model paired with the energy-based control has a (close) resemblance to biological motion. There are, however, a few limitations to our approach. The strain parametrization of functional basis does not account for the geometry of the soft robot, meaning some systems require many spatial modes to *accurately* represent the true continuum dynamics. Second, regarding implementation, measuring these spatial modes in an experimental setting is difficult, and future research is required to find a suitable ‘*soft sensing*’ technique that (i) has limited impact on the dynamics, and (ii) accounts for the continuity of the elastic body. A possible solution might be the optimal placement of a network of distributed localized sensors, e.g., strain gauges or IMUs. Furthermore, the proposed controller is only suited for set-point regulation or slow-varying references. Exploring (fast)-dynamic control objectives will likely require more research. In particular, controllers that suppress natural resonances of continuum elastic body under fast motion. One could argue that this perhaps fights against the natural dynamics of the soft robot, yet such oscillations might be able to be explored for locomotion or soft manipulators throwing objects rather than traditional pick-and-place strategies.

Given these limitations, future work will focus on the following: (i) adding hyper-elasticity (ii) validating the controller experimentally, and (iii) constructing a set of basis functions through the so-called ’*snapshot decomposition method*’ using FEM-driven data. In particular, the latter two goals could be interesting to explore. Both advantages in FEM and Cosserat models, being accurate continuum deformations and computational efficiency; leading to a modeling strategy for *optimal* finite-dimensional state projection with the insightful structure of the passive and active joints.

## References

Astrid P, Weiland S, Willcox K, Backx T. Missing point estimation in models described by proper orthogonal decomposition. IEEE Trans Autom Control. 2008;53(10):2237–51.

Borja P, Dabiri A, Santina CD. Energy-based shape regulation of soft robots with unactuated dynamics dominated by elasticity. In: 2022 IEEE 5th international conference on soft robotics (RoboSoft). 2022. pp. 396–402.

Boyer F, Porez M, Leroyer A. Poincaré-cosserat equations for the lighthill three-dimensional large amplitude elongated body theory. Appl Robot. 2010;20(1):47–79.

Boyer F, Lebastard V, Candelier F, Renda F. Dynamics of continuum and soft robots: a strain parameterization based approach. IEEE Trans Rob. 2021;37(3):847–63.

Bruder D, Fu X, Gillespie RB, Remy CD, Vasudevan R. Data-driven control of soft robots using koopman operator theory. IEEE Trans Rob. 2021;37(3):948–61.

Bullo FMR. Proportional derivative (pd) control on the Euclidean group. In: Proceedings of European Control Conference. 1995. pp. 1091–1097.

Caasenbrood B. Sorotoki—an open-source soft robotics toolkit for matlab. https://github.com/BJCaasenbrood/SorotokiCode (2020).

Caasenbrood B, Pogromsky A, Nijmeijer H. Dynamic modeling of hyper-elastic soft robots using spatial curves. IFAC Proc Vol (IFAC-PapersOnline). 2020;50:9238–43.

Caasenbrood B, Pogromsky AY, Nijmeijer H. Energy-based control for soft manipulators using cosserat-beam models. In: International conference on informatics in control, automation and robotics—ICINCO. 2021. pp. 311–319.

Caasenbrood B, Pogromsky A, and Nijmeijer H. Control-oriented models for hyperelastic soft robots through differential geometry of curves. Soft Robot (in press). 2022.

Chang H-S, Halder U, Shih C-H, Tekinalp A, Parthasarathy T, Gribkova E, Chowdhary G, Gillette R, Gazzola M, Mehta PG. Energy shaping control of a CyberOctopus soft arm. 2020.

Choi W, Whitesides GM, Wang M, Chen X, Shepherd RF, Mazzeo AD, Morin SA, Stokes AA, Ilievski F. Multigait soft robot. Proc Natl Acad Sci. 2011;108(51):20400–3.

Della Santina C, Rus D. Control oriented modeling of soft robots: the polynomial curvature case. IEEE Robot Automat Lett. 2020;5(2):290–8.

Della Santina C, Duriez C, Rus D. Model based control of soft robots: a survey of the state of the art and open challenges. arXiv2021;1–69.

Duriez C. Control of elastic soft robots based on real-time finite element method. In: Proceedings—IEEE international conference on robotics and automation. 2013. pp. 3982–3987.

Falkenhahn V, Mahl T, Hildebrandt A, Neumann R, Sawodny O. Dynamic modeling of bellows-actuated continuum robots using the Euler–Lagrange formalism. IEEE Trans Rob. 2015;31(6):1483–96.

Franco E, Garriga-Casanovas A. Energy-shaping control of soft continuum manipulators with in-plane disturbances. Int J Robot Res. 2021;40(1):236–55.

Katzschmann RK, Della Santina CD, Toshimitsu Y, Bicchi A, Rus D. Dynamic motion control of multi-segment soft robots using piecewise constant curvature matched with an augmented rigid body model. In: Robosoft—IEEE international conference on soft robotics. 2019. pp. 454–461.

Kim N-H. Introduction to nonlinear finite element analysis. Berlin: Springer; 2018.

Largilliere F, Verona V, Coevoet E, Sanz-Lopez M, Dequidt J, Duriez C. Real-time control of soft-robots using asynchronous finite element modeling. In: Proceedings—IEEE international conference on robotics and automation. 2015. pp. 2550–2555.

Marchese AD, Onal CD, Rus D. Autonomous soft robotic fish capable of escape maneuvers using fluidic elastomer actuators. Soft Rob. 2014;1(1):75–87.

Murray RM, Sastry SS, Zexiang L. A mathematical introduction to robotic manipulation. USA: Taylor and Francis; 1994.

Ortega R, Loria A, Nicklasson P, Sira-Ramirez H. Passivity-based control of Euler-Lagrange systems. Berlin: Springer; 1998.

Ortega R, Spong MW, Gómez-Estern F, Blankenstein G. Stabilization of a class of underactuated mechanical systems via interconnection and damping assignment. IEEE Trans Autom Control. 2002;47(8):1218–33.

Renaud C, Cros JM, Feng ZQ, Yang B. The Yeoh model applied to the modeling of large deformation contact/impact problems. Int J Impact Eng. 2009;36(5):659–66.

Renda F, Boyer F, Dias J, Seneviratne L. Discrete Cosserat approach for multisection soft manipulator dynamics. IEEE Trans Rob. 2018;34(6):1518–33.

Renda F, Armanini C, Lebastard V, Candelier F, Boyer F. A geometric variable-strain approach for static modeling of soft manipulators with tendon and fluidic actuation. IEEE Robot Automat Lett. 2020;5(3):4006–13.

Schaft AJ. Port-Hamiltonian systems: network modeling and control of nonlinear physical systems. Adv Dyn Control Struct Mach. 2004;444(1):127–67.

Simo JC, Vu-Quoc L. A three-dimensional finite-strain rod model part II: computational aspects. Comput Methods Appl Mech Eng. 1986;58(1):79–116.

Sonneville V, Cardona A, Brüls O. Geometrically exact beam finite element formulated on the special Euclidean group SE(3). Comput Methods Appl Mech Eng. 2014;268:451–74.

Spong MW, Hutchinson S, Vidyasagar M. Robot modeling and control. New York: Wiley; 2006.

Till J, Aloi V, Rucker C. Real-time dynamics of soft and continuum robots based on Cosserat rod models. Int J Robot Res. 2019;38(6):723–46.

Wu K, Zheng G. FEM-based gain-scheduling control of a soft. Robot Automat Lett. 2021;6(2):3081–8.

Zhang Z, Bieze TM, Dequidt J, Kruszewski, A, Duriez C. Visual servoing control of soft robots based on finite element model. In: 2017 IEEE/RSJ international conference on intelligent robots and systems (IROS). 2017. pp. 2895–2901.

## Acknowledgements

This work is partly supported by NWO, Netherlands Organization for Scientific Research; and is part of the Wearable Robotics perspective program with project number P16-05 (P2). Website: www.wearablerobotics.nl

## Author information

### Authors and Affiliations

### Corresponding author

## Ethics declarations

### Conflict of interest

The authors declare that they have no conflict of interest.

### Code availability

The framework used and simulation examples used in this work are made publicly available at https://github.com/BJCaasenbrood/SorotokiCode.

## Additional information

### Publisher's Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

This article is part of the topical collection “Informatics in Control, Automation and Robotics” guest edited by Kurosh Madani, Oleg Gusikhin and Henk Nijmeijer.

## Appendices

### Appendix 1

### Continuous Kinematics

For completeness, we introduce the adjoint action on the group and its algebra by

respectively. Under Assumption 1, the configuration space of the soft robot \(\varvec{g}\) is everywhere differentiable. Then, using the equality of mixed partials, i.e. \(\frac{\partial }{\partial t}(\frac{\partial \varvec{g}}{\partial \sigma }) = \frac{\partial }{\partial \sigma } (\frac{\partial \varvec{g}}{\partial t})\), we substitute \(\partial \varvec{g}/\partial t = \varvec{}{g}\hat{\varvec{}{\eta }}\) and \(\partial \varvec{g}/\partial \sigma = \varvec{g}\hat{\varvec{}{\xi }}\) to find

Pre-multiplying with \(\varvec{g} ^{{-1}}\in {\text {SE}(3)}\) and rearranging the equality above, we obtain

where we can recognize the Lie bracket or the commuter between the vector fields \(\varvec{}{\xi }\) and \(\varvec{}{\eta }\) [22]. Since the Lie bracket \([\hat{\varvec{}{\xi }},\hat{\varvec{}{\eta }}]\) itself also belongs to \({\text {se}(3)}\), which is isomorphic to \(\mathbb {R}^6\) via \(\hat{\varvec{}{\eta }} \rightarrow \varvec{}{\eta }\), we can rewrite the expressions as follows

where \({\textbf {ad}}_{(\cdot )}: \mathbb {R}^6 \rightarrow \mathbb {R}^{6\times 6}\) defines the adjoint action map on the Lie algebra \({\text {se}(3)}\). The approach provided above is analogous to [4, 27, 32].

### Appendix 2

### Partial Derivative of the Geometric Jacobian Matrix

The mapping from generalized coordinates \(\dot{\varvec{}{q}} \in \mathbb {R}^n\) to the velocity-twist vector \(\hat{\varvec{}{\eta }} = \varvec{}{g}^{{-1}}\dot{\varvec{g}} \in {\text {se}(3)} \cong \mathbb {R}^6\) for a point \(\sigma\) be given by \(\varvec{}{\eta }= \varvec{J}\dot{\varvec{}{q}}\) where \(\varvec{J}\) is the geometric Jacobian. The *k*-th order truncations of the exact geometric Jacobian is given by

Unlike its notation in rigid robotics, note that the geometric Jacobian matrix here is time and space-variant. Following the chain rule of differentiation, the partial time-derivative of the geometric Jacobian matrix yields

Given the differential relations of the adjoint action mapping on the Lie group, that is, \({\mathrm{d}/\mathrm{d}s}\left( {\textbf {Ad}}_{\varvec{g}} \right) = {\textbf {Ad}}_{\varvec{g}} {\textbf {ad}}_{\varvec{\Upsilon }}\) given a twist \(\varvec{}{\Upsilon } = (\varvec{g}^{{-1}}\mathrm{d}\varvec{g}/\mathrm{d}s)^\vee\), we can express the time-derivate of the adjoint action and its inverse as

Substituting the truncated variations of (B7) and (B8) into (B6), we find the complete expression of the time-derivate of the geometric Jacobian matrix

Since \({\textbf {ad}}_{\varvec{}{\eta }} (\varvec{J} \dot{\varvec{}{q}}) = {\textbf {ad}}_{\varvec{}{\eta }} \varvec{}{\eta } = \varvec{}{0}_6\) by definition, the first right-hand term vanishes if (B9) is post-multiplied with the generalized velocities \(\dot{\varvec{}{q}}\), thus leading to the acceleration twist \(\dot{[\varvec{}{\eta }]}_k\) in (12).

## Rights and permissions

**Open Access** This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article's Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article's Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecommons.org/licenses/by/4.0/.

## About this article

### Cite this article

Caasenbrood, B., Pogromsky, A. & Nijmeijer, H. Energy-Shaping Controllers for Soft Robot Manipulators Through Port-Hamiltonian Cosserat Models.
*SN COMPUT. SCI.* **3**, 494 (2022). https://doi.org/10.1007/s42979-022-01373-w

Received:

Accepted:

Published:

DOI: https://doi.org/10.1007/s42979-022-01373-w