Model-based dynamic feedback control of a planar soft robot: trajectory tracking and interaction with the environment

Abstract

Leveraging the elastic bodies of soft robots promises to enable the execution of dynamic motions as well as compliant and safe interaction with an unstructured environment. However, the exploitation of these abilities is constrained by the lack of appropriate control strategies. This work tackles for the first time the development of closed-loop dynamic controllers for a continuous soft robot. We present two architectures designed for dynamic trajectory tracking and surface following, respectively. Both controllers are designed to preserve the natural softness of the robot and adapt to interactions with an unstructured environment. The validity of the controllers is proven analytically within the hypotheses of the model. The controllers are evaluated through an extensive series of simulations, and through experiments on a physical soft robot capable of planar motions.

Keywords

Soft robotics motion control flexible arms soft control model-based control

1. Introduction

Animals move very differently from rigid robots: they perform dynamic tasks efficiently, and interact robustly, compliantly, and continuously with the external world through their body’s elasticity (Roberts and Azizi, 2011). Inspired by biology and with the aim of reaching a higher level of agility and compliance, researchers are designing soft robots with elastic bodies (Laschi et al., 2016; Polygerinos et al., 2017; Rus and Tolley, 2015). However, despite the emergence of several soft robotic hardware architectures (Holland et al., 2017; Homberg et al., 2019; Katzschmann et al., 2018; Laschi et al., 2012; Seok et al., 2013), examples that show the execution of dynamic movements and controlled compliant interaction with the environment are still missing. One of the main motivations for building soft robots is to improve dynamic movements and compliant interactions, but robots with rigid structures still outperform their soft counterparts in these tasks (Andersson, 1989; Haddadin et al., 2017; Hong et al., 2017; Kuindersma et al., 2016; Leidner et al., 2014).

This limitation is largely to be attributed to the lack of a soft robotic brain exploiting the embodied intelligence that the elastic body of a soft robot provides (Pfeifer et al., 2012). Both tasks, dynamic movements and compliant interactions, are indeed inherently dynamical, whereas most of the existing control algorithms for soft robots rely only on static modeling (George Thuruthel et al., 2018; Godage et al., 2015a; Lismonde et al., 2017; Mahl et al., 2014; Skorina et al., 2016; Till et al., 2015; Wang et al., 2017; Webster III and Jones, 2010; Zhang et al., 2016) .

An issue that slowed down the development of dynamical control strategies is the diﬃculty of developing reliable yet tractable mathematical models for soft robots. The general formulation of an exact model requires the inﬁnite dimensionality of the robot’s state space to be taken into account (de Payrebrune and O’Reilly, 2016; Rone and Ben-Tzvi, 2014; Rubin, 2013) . However, the theory of inﬁnite state space control is still conﬁned to relatively simple systems (Curtain and Zwart, 2012), and its applications are still preliminary, even if interesting in their own right (Armanini et al., 2017; Luo et al., 2012). This issue drives the development of simpliﬁed models that are capable of describing the robot’s behavior through a ﬁnite set of variables. For some hybrid soft–rigid systems the rigid part is dynamically dominant, which allows the soft dynamics to be neglected in the control design (Deutschmann et al., 2017a,b; Skorina et al., 2015). Moving to a more general scenario, ﬁnite element methods are commonly used in the mechanical design of soft robots (Chenevier et al., 2018; Polygerinos et al., 2015). However, their high dimensionality limits the practical use of these models for feedback control. Simulations provided in Thieﬀry et al. (2017) use a linearized ﬁnite element model (FEM) to regulate postures. Prior work on dynamic models with ﬁnite dimensions also includes discrete Kirchhoﬀ–Love models (Bergou et al., 2008; Greco and Cuomo, 2013), and discrete Cosserat models ( Gazzola et al., 2016; Grazioso et al., 2016; Renda et al., 2017; Sadati et al., 2018). We are not aware of any prior work that applies these dynamic models for the control of soft robots. Dynamic models based on the piecewise constant curvature (PCC) hypothesis were presented in Falkenhahn et al. (2015) and Marchese et al. (2016). The models presented in these works are merely used for generating purely feedforward actuations. To the best of the authors’ knowledge, there has been no previous work on the design and validation of dynamical feedback controllers for soft robots in dynamic tasks, except for our previous conference work (Della Santina et al., 2018), of which this article serves as an extension.

Dynamic control of continuum robots with higher Young’s modulus is a relatively well-studied ﬁeld. Dynamic models based on the PCC assumption were used by Kapadia et al. (2014, 2010) to achieve static posture regulation. The same task was performed under milder assumptions by Gravagne et al. (2003) and Penning and Zinn (2014). Moving to more dynamic tasks, tracking control is addressed by Hisch et al. (2017 ) and Kapadia and Walker (2011), supported by simulation results only. Kapadia et al. (2010) proposed a sliding mode controller speciﬁcally designed for tracking control with an extensible robot made of three segments. They also provided preliminary experimental validations in tracking a single sinusoidal trajectory. Thus, even in this ﬁeld very limited results exists in developing controllers for dynamic task execution.

Leveraging on this literature, and with the goal of making a step towards the proper exploitation of soft robotic potentials, we propose two feedback control architectures speciﬁcally designed for controlling soft robots in the execution of dynamic tasks. The ﬁrst controller aims to achieve dynamic trajectory following of curvatures in free space. The second controller is an impedance controller that allows the position of the end eﬀector to be controlled in free space and can move along a surface, while staying in contact with that surface. Both controllers rely on an augmented formulation linking the soft robot to a classic rigid serial manipulator with a parallel elastic mechanism. Prior tools developed for this classical type of robot model can be exploited under this formulation (Ott, 2008; Sciavicco and Siciliano, 2012). The eﬀectiveness of both controllers is assessed theoretically within the modeling hypotheses, and through extensive simulations and experiments. Note that only the planar case is considered in this article.

This works makes the following contributions to the control of soft robots:

the first closed-loop dynamic controller for a continuum soft robot capable of dynamically tracking desired curvature evolutions;

the first closed-loop controller for a continuum soft robot capable of dynamically moving in Cartesian space and compliantly tracking a surface;

an “augmented formulation” linking a soft robot to a classic rigid-bodied serial manipulator;

validation of the controllers in simulations and experiments using the planar system in Figure 1.

2. Model

In this section, we propose a framework for modeling the dynamics of soft robots, linking it to an equivalent rigid body constrained through a set of nonlinear integrable constraints. The key property of the model is to define a perfect matching under the hypothesis of PCC, enabling the application of control strategies typically used in rigid-bodied robots onto soft robots.

2.1. Kinematics

In the PCC model, the infinite dimensionality of the soft robot configuration is resolved by considering the robot’s shape as composed of a fixed number of segments with constant curvature (CC), merged such that the resulting curve is everywhere differentiable.

Consider a PCC soft robot composed by n CC segments. We introduce n reference frames ${S_{1}}, \dots, {S_{n}}$ attached at the end of each segment, plus one base frame ${S_{0}}$ . Figure 2(a) presents an example of a soft robot composed by four CC segments. Using the CC hypothesis, ${S_{i - 1}}$ and ${S_{i}}$ fully define the configuration of the ith segment. Thus, the robot kinematics can be defined by the n homogeneous transformations $T_{0}^{1}, \dots, T_{n - 1}^{n}$ , which map each reference system to the subsequent one.

In the interest of conciseness, we consider the planar case. Please refer to Webster III and Jones (2010) for more details about the PCC kinematics in the 3D case. Figure 3 shows the kinematics of a single CC segment. Under the hypothesis of non-extensibility, one variable is sufficient to describe the segment’s configuration. We use the relative rotation between the two reference systems as the configuration variable. Let us call this variable $q_{i}$ for the ith segment, and $q \in R^{n}$ the vector collecting $q_{i}$ for all the n segments. Note that this angle is directly related to the radius of curvature $ρ_{i}$ of the segment through the linear relation $q_{i} ρ_{i} = L_{i}$ , where $L_{i}$ is the constant length of a segment. We refer to $q_{i}$ as the bending angle.

The ith homogeneous transformation can be derived using geometrical considerations as

T_{i - 1}^{i} (q_{i}) = [\begin{matrix} \cos (q_{i}) & - \sin (q_{i}) & L_{i} \frac{\sin (q_{i})}{q_{i}} \\ \sin (q_{i}) & \cos (q_{i}) & L_{i} \frac{1 - \cos (q_{i})}{q_{i}} \\ 0 & 0 & 1 \end{matrix}]

(1)

Note that for $q_{i} \to 0$ , the transformation is still well defined by

T_{i - 1}^{i} (0) \dot{=} lim_{q_{i} \to 0} T_{i - 1}^{i} (q_{i}) = [\begin{matrix} 1 & 0 & L_{i} \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{matrix}]

(2)

2.2. Dynamics

Equation (1) can be reformulated using elemental Denavit–Hartenberg (DH) transformations, as described in Hannan and Walker (2003) and Jones and Walker (2006) . Such equivalence implicitly defines a connection between the soft robot and a rigid robot described by the equivalent parametrization. Figure 3(a) and (b) show two examples of rigid robotic structures matching a single CC segment. More complex rigid structures for generic PCC soft robots can be built by interconnecting such basic elements.

From a kinematic point of view, any representation satisfying the condition that the end points of each CC segment coincide with the corresponding reference points of the rigid robot is equivalent. However, as soon as we consider the dynamics of the two robots, another constraint has to be taken into account: the inertial properties of the augmented and the soft robot must be equivalent. In the hypothesis of negligible rotational inertia, this can be obtained by matching the centers of mass of each CC segment by an equivalent point mass attached to the rigid robot structure. Figure 4(c) and (d) present two examples of rigid structures dynamically matching a CC segment. The first example uses the approximation of a point mass placed on the middle of the main chord, whereas the second example can be used to match more accurate hypotheses such as that described by Godage et al. (2015b).

We refer to the state space of the equivalent rigid robot as the augmented state $ξ \in R^{n h}$ , with h being the number of joints per CC segment and n being the number of segments. The two configurations are connected through the continuously differentiable map

m : R^{n} \to R^{n h}

(3)

The map m is such that the nonlinear constraint $ξ = m (q)$ assures that the end points and the point masses of each CC segment coincide in position and orientation with the corresponding points of the rigid robot.

Consider, for example, the middle point of the chord as an approximation of the mass distribution of the CC segment, see Figure 4(c). For that representation we report the dynamically consistent DH parametrization in Table 1. The corresponding map is

m_{i} (q_{i}) = [\begin{matrix} \frac{q_{i}}{2} \\ L_{i} \frac{\sin (\frac{q_{i}}{2})}{q_{i}} \\ L_{i} \frac{\sin (\frac{q_{i}}{2})}{q_{i}} \\ \frac{q_{i}}{2} \end{matrix}]

(4)

Table 1.

Description of the rigid robot equivalent to a single CC segment. The parameters $θ$ , d, a, $α$ refer to the classical DH parametrization, whereas $μ$ refers to the mass.

Link	$θ$	d	$α$	$μ$
1	$\frac{q_{i}}{2}$	0	$\frac{π}{2}$	0
2	$0$	$L_{i} \frac{\sin (\frac{q_{i}}{2})}{q_{i}}$	0	$μ_{i}$
3	$0$	$L_{i} \frac{\sin (\frac{q_{i}}{2})}{q_{i}}$	$- \frac{π}{2}$	0
4	$\frac{q_{i}}{2}$	0	$0$	0

Figure 5 presents the CC segment and the corresponding rigid counterpart for five postures ranging from $q_{i} = 0$ rad to $q_{i} = π$ rad. A continuous soft robot represented by the PCC model can always be matched to a dynamically consistent rigid robot, built as a sequence of these elements. Figure 2(b) presents an example with four segments. The robot configurations are connected by the map

m (q) = {[\begin{matrix} m_{1} {(q_{1})}^{T} \dots m_{n} {(q_{n})}^{T} \end{matrix}]}^{T}

(5)

The dynamics of the augmented rigid robot are

B_{ξ} (ξ) \overset{\cdot\cdot}{ξ} + C_{ξ} (ξ, \overset{\cdot}{ξ}) \overset{\cdot}{ξ} + G_{ξ} (ξ) = J_{ξ}^{T} (ξ) f_{ext}

(6)

where $ξ$ , $\overset{\cdot}{ξ}$ , $\overset{\cdot\cdot}{ξ} \in R^{nh}$ is the robot configuration with its derivatives, $B_{ξ} (ξ) \in R^{nh \times nh}$ is the robot’s inertia matrix, $C_{ξ} (ξ, \overset{\cdot}{ξ}) \overset{\cdot}{ξ} \in R^{nh}$ collects Coriolis and centrifugal terms, $G_{ξ} (ξ) \in R^{nh}$ takes into account the effect of gravity on the robot. The robot is subject to a set of external wrenches $f_{ext}$ , mapped through the Jacobian $J_{ξ}$ .

The dynamics of the soft robot is, thus, described by Equation (6) expressed on the sub-manifold implicitly defined by the map $ξ = m (q)$ . This map describes nonlinear constraints that are scleronomic and holonomic. Therefore, it is possible to integrate them directly into the dynamics of the system. To this end, we evaluate the augmented configuration derivatives $ξ$ , $\overset{\cdot}{ξ}$ , $\overset{\cdot\cdot}{ξ}$ , with respect to q, $\overset{\cdot}{q}$ , $\overset{\cdot\cdot}{q}$

{\begin{matrix} ξ = m (q) \\ \overset{\cdot}{ξ} = J_{m} (q) \overset{\cdot}{q} \\ \overset{\cdot\cdot}{ξ} = {\overset{\cdot}{J}}_{m} (q, \overset{\cdot}{q}) \overset{\cdot}{q} + J_{m} (q) \overset{\cdot\cdot}{q} \end{matrix}

(7)

where $J_{m} (q)$ : $R^{n} \to R^{n h \times n}$ is the Jacobian of $m (\cdot)$ , i.e., $\frac{\partial m}{\partial q}$ . When $m_{i} (q)$ is defined as in (4), it is

J_{m, i} = {[\begin{matrix} \frac{1}{2} L_{c, i} L_{c, i} \frac{1}{2} \end{matrix}]}^{T}

(8)

where $L_{c, i} = L_{i} \frac{q_{i} \cos (\frac{q_{i}}{2}) - 2 \sin (\frac{q_{i}}{2})}{2 q_{i}^{2}}$ . By substituting (7) into (6), it follows

\begin{matrix} B_{ξ} (m (q)) ({\overset{\cdot}{J}}_{m} (q, \overset{\cdot}{q}) \overset{\cdot}{q} + J_{m} (q) \overset{\cdot\cdot}{q}) + C_{ξ} (m (q), J_{m} (q) \overset{\cdot}{q}) J_{m} (q) \overset{\cdot}{q} \\ + G_{ξ} (m (q)) = J_{ξ}^{T} (m (q)) f_{ext} \end{matrix}

(9)

This generalized balance of forces can be projected by pre-multiplication with $J_{m}^{T} (q)$ onto the constraints described by the map $ξ = m (q)$ . This yields the compact dynamics

B (q) \overset{\cdot\cdot}{q} + C (q, \overset{\cdot}{q}) \overset{\cdot}{q} + G (q) = J^{T} (q) f_{ext}

(10)

where

{\begin{cases} B (q) = J_{m}^{T} (q) B_{ξ} (m (q)) J_{m} (q) \\ C (q, \overset{\cdot}{q}) = J_{m}^{T} (q) B_{ξ} (m (q)) {\overset{\cdot}{J}}_{m} (q, \overset{\cdot}{q}) + J_{m}^{T} (q) \\ C_{ξ} (m (q), J_{m} (q) \overset{\cdot}{q}) J_{m} (q) \\ G (q) = J_{m}^{T} (q) G_{ξ} (m (q)) \\ J^{T} (q) = J_{m}^{T} (q) J_{ξ}^{T} (m (q)) \end{cases}

(11)

Note that the terms in (6) can be efficiently formulated in an iterative form, as discussed in Featherstone (2014). The dynamic model for the soft robot derived in (10) inherits this computational advantage through (11).

Remark 1. While not considered here, it is worth pointing out that a lumped rotational inertia could be easily included by adding it to the link where the mass $μ_{i}$ is attached.

We complete (10) by introducing elastic and dissipative terms. It is convenient to evaluate the impedance directly for the PCC soft robot using the configuration variable q, and its derivative $\overset{\cdot}{q}$ . We consider here the planar case described in Figure 6. We model the elasticity of a link through a continuous distribution of infinitesimal springs along the cross-sectional area of a segment. The net effect of this distribution is well studied in the state of the art, and it is described by the simple linear relationship

τ_{E, i} (q_{i}) = \frac{E_{i} I_{i}}{L_{i}} q_{i}

(12)

where $τ_{E, i}$ is the elastic generalized force at the ith segment defined by the Young’s modulus $E_{i}$ , the second moment of inertia $I_{i}$ , and the segment’s length $L_{i}$ . The product $E_{i} I_{i}$ is typically referred to as bending rigidity.

Similarly, we introduce an infinitesimal linear damper in parallel to each spring. It generates a force equal to $β {\overset{\cdot}{L}}_{δ, i}$ , where $L_{δ, i}$ is the length of a damper at distance $δ$ from the central axis of the segment

L_{δ, i} (q_{i}) = (ρ_{i} - δ) q_{i} = (\frac{L_{i}}{q_{i}} - δ) q_{i}

(13)

As $β_{i}$ is a distribution describing infinitesimal dampers rather than a vector its neat effect is a continuum distribution of forces. The dimension of the first is $Ns / m^{2}$ , and of the latter is $N / m$ . The force distribution is then to be integrated along the cross-section to obtain a torque. If $β_{i}$ is constant, this can be seen as being an average damping along the cross-section normalized by the size of the cross-section diameter ${\bar{β}}_{i} / (2 Δ_{i})$ . Therefore, we obtain the total dissipative force produced at the ith segment, by exploiting kineto-static duality and integrating over the cross-section

\int_{- Δ_{i}}^{Δ_{i}} β_{i} {(\frac{\partial L_{δ, i}}{\partial q_{i}})}^{2} {\overset{\cdot}{q}}_{i} d δ = \frac{2}{3} β_{i} Δ_{i}^{3} {\overset{\cdot}{q}}_{i}

(14)

Thus, in the PCC hypothesis, elastic and damping actions can be described by two linear terms: $K q$ and $D \overset{\cdot}{q}$ , respectively. Here K and D are two diagonal matrices, with $\frac{E_{i} I_{i}}{L_{i}}$ and $\frac{2}{3} β_{i} Δ_{i}^{3}$ as ith diagonal elements. Note that $K ≻ 0$ and $D ≻ 0$ , because $E_{i} > 0$ , $I_{i} > 0$ , $L_{i} > 0$ , $β_{i} > 0$ , and $Δ_{i} > 0$ , for all i. To conclude, the properties of the impedance model that are essential in the following are: (i) it is linear; (ii) $K ≻ 0$ , $D ≻ 0$ ; and (iii) K and D are diagonal.

Furthermore, we assume the soft robot is actuated through a pair of internal torques for each segment applied as shown in Figure 6. The mapping between actuations and generalized torques can be evaluated using the Jacobians as defined in (11). In the planar case, the mapping is the identity. For the sake of space we do not report the calculation of these Jacobians. Note that directly controlling $τ$ assumes that the actuator dynamics are negligible. While this is a fair assumption for tendon-driven soft robots (Laschi et al., 2012), the hypothesis does not necessarily hold for other types of actuation technologies such as pneumatic actuation (Marchese and Rus, 2016). Future work will be devoted to further investigate also the influence of the actuator dynamics and possibly incorporate those into the proposed model. Thus, the complete model is

B (q) \overset{\cdot\cdot}{q} + (C (q, \overset{\cdot}{q}) + D) \overset{\cdot}{q} + G (q) + K q = τ + J^{T} (q) f_{ext}

(15)

where $D ≻ 0$ is the damping and $K ≻ 0$ is the stiffness.

2.3. Model properties

The proposed model verifies a set of basic properties of classical rigid robots, of which we report a selection in the following. Note that they could be derived directly by relying on fundamental characteristics of Lagrangian system (Bullo and Lewis, 2004). Nonetheless, we prefer to derive them from scratch, so as to make the work self-contained. These properties are of particular interest for the aim of the present article, because they are used for proving the stability of the two proposed controllers in Section 3.

Lemma 1. If $C_{ξ} (ξ, \overset{\cdot}{ξ})$ is defined with Christoffel symbols (Murray, 1994: Ch. 4), then the matrix $\overset{\cdot}{B} (q) - 2 C (q, \overset{\cdot}{q})$ is skew symmetric.

Proof. We start by evaluating the time derivative of the inertia matrix, using its definition in (11)

\overset{\cdot}{B} = {\overset{\cdot}{J}}_{m}^{T} B_{ξ} J_{m} + J_{m}^{T} {\overset{\cdot}{B}}_{ξ} J_{m} + J_{m}^{T} B_{ξ} {\overset{\cdot}{J}}_{m}

(16)

Note that in this proof we omit arguments for the ease of reading. Combining (16) with the expression of C in (11) yields

\begin{matrix} \overset{\cdot}{B} - 2 C = {\overset{\cdot}{J}}_{m}^{T} B_{ξ} J_{m} + J_{m}^{T} {\overset{\cdot}{B}}_{ξ} J_{m} + J_{m}^{T} B_{ξ} {\overset{\cdot}{J}}_{m} - 2 J_{m}^{T} B_{ξ} {\overset{\cdot}{J}}_{m} \\ - 2 J_{m}^{T} C_{ξ} J_{m} \\ = J_{m}^{T} ({\overset{\cdot}{B}}_{ξ} - 2 C_{ξ}) J_{m} + {\overset{\cdot}{J}}_{m}^{T} B_{ξ} J_{m} - J_{m}^{T} B_{ξ} {\overset{\cdot}{J}}_{m} \end{matrix}

(17)

The skew symmetry of the first term of the sum follows by direct application of the quadratic form definition. Indeed, for every $ν \in R^{n}$ the following holds

ν^{T} J_{m}^{T} ({\overset{\cdot}{B}}_{ξ} - 2 C_{ξ}) J_{m} ν = (ν J_{m})^{T} ({\overset{\cdot}{B}}_{ξ} - 2 C_{ξ}) (J_{m} ν) = 0

(18)

where for the last step we exploit the skew symmetry of ${\overset{\cdot}{B}}_{ξ} - 2 C_{ξ}$ (see Slotine and Li, 1987). The skew symmetry of the matrix $\overset{\cdot}{B} - 2 C$ is then derived by considering that

\begin{matrix} ({\overset{\cdot}{J}}_{m}^{T} B_{ξ} J_{m} - J_{m}^{T} B_{ξ} {\overset{\cdot}{J}}_{m})^{T} = J_{m}^{T} B_{ξ} {\overset{\cdot}{J}}_{m} - {\overset{\cdot}{J}}_{m}^{T} B_{ξ} J_{m} \\ = - ({\overset{\cdot}{J}}_{m}^{T} B_{ξ} J_{m} - J_{m}^{T} B_{ξ} {\overset{\cdot}{J}}_{m}) \end{matrix}

and that the sum of skew-symmetric matrices is also skew symmetric.□

Remark 2. If $C_{ξ}$ is not built through Christoffel symbols, the thesis of Lemma 1 becomes ${\overset{\cdot}{q}}^{T} (\overset{\cdot}{B} (q) - 2 (C (q, \overset{\cdot}{q}))) \overset{\cdot}{q} = 0$ . This can be proven by following the same steps of the proof above.

Lemma 2. The inertia matrix $B (q)$ is positive semidefinite and limited, i.e.,

B (q) ⪰ 0, ∥ B (q) ∥ < \infty \forall q \in R^{n}

if $- \infty < m_{i} (q) < + \infty$ for all $q \in R^{n}$ and for all i such that the ith joint is prismatic, and $∥ J_{m} ∥ < \infty$ .

Proof. For the first property, we start by considering that $B_{ξ} (m) ⪰ 0$ for every $ξ$ by construction. Note that in this proof we omit the dependencies $q, \overset{\cdot}{q}$ for the ease of reading. Thus, recalling that $B = J_{m}^{T} B_{ξ} (m) J_{m}$ , the first property is derived from the very definition of positiveness. Indeed, for every $ν \in R^{n}$ the following holds

ν^{T} J_{m}^{T} B_{ξ} (m) J_{m} ν = (ν J_{m})^{T} B_{ξ} (m) (J_{m} ν) > 0

(19)

For the second property, the submultiplicative property of the matrix norm of a square matrix tells us that $∥ B ∥ < ∥ J_{m}^{T} ∥ ∥ B_{ξ} (m) ∥ ∥ J_{m} ∥$ . The limitedness of $∥ J_{m} ∥$ is assured by the hypothesis on $J_{m}$ . Here $B_{ξ} (m)$ is the inertia matrix of a rigid robot. Its norm can tend to infinity only if the length of at least one of its prismatic joints tends to infinity. This possibility is excluded by the hypothesis on m.

Corollary 1. Let $R (q) \in R^{nh \times d_{r}}$ be such that $Rank (B_{ξ} (m (q))) = d_{r}$ and $Span (R (q))$ is the range of $B_{ξ} (m (q))$ , and let the hypotheses of Lemma 2 be verified. Then $Rank (R^{T} (q) J_{m} (q)) = n$ implies that $B (q) ≻ 0$ .

Proof. The inertia matrix $B_{ξ}$ of a rigid robot is symmetric, which assures that it is diagonalizable. Note that in this proof we omit arguments for the ease of reading. As the augmented rigid robot can include massless links, the kernel of $B_{ξ}$ is in general not empty. Thus, by spectral decomposition the following holds

B_{ξ} = M^{T} [\begin{matrix} U & 0 \\ 0 & 0 \end{matrix}] M

(20)

where $U \in R^{nh \times nh}$ is the diagonal matrix collecting all the non-null eigenvalues of $B_{ξ}$ , and

M = [\begin{matrix} R^{T} \\ N^{T} \end{matrix}]

(21)

where $R \in R^{nh \times d_{r}}$ and $N \in R^{nh \times nh - d_{r}}$ have as columns a base of the range and the kernel of $B_{ξ}$ , respectively.

Combining (11), (20), and (21), the following holds

\begin{matrix} B = J_{m}^{T} B_{ξ} (m) J_{m} \\ = J_{m}^{T} R U R^{T} J_{m} \end{matrix}

(22)

The application of the Sylvester inequality (Petersen and Pedersen, 2008) yields

\begin{matrix} Rank (B) \geq Rank (J_{m}^{T} R U) + Rank (R^{T} J_{m}) - d_{r} \\ \geq Rank (U) + 2 Rank (R^{T} J_{m}) - 2 d_{r} \end{matrix}

(23)

Note that $Rank (U) = d_{r}$ is given by construction, and $Rank (R^{T} J_{m}) = n$ by hypothesis. The latter also implies that $n \geq d_{r}$ . Therefore,

Rank (B) \geq n

(24)

As B lives in $R^{n \times n}$ , then (24) implies that B is full rank. This excludes the semi-definiteness of B, and together with Lemma 2 proves the thesis.□

Lemma 3. Suppose that $γ_{m}, γ_{J}, γ_{\overset{\cdot}{J}} > 0$ exist such that

\begin{matrix} ∥ m (q) ∥ < γ_{m} ∥ q ∥, ∥ J_{m} (q) ∥ < γ_{J}, \\ ∥ {\overset{\cdot}{J}}_{m} (q) ∥ < γ_{\overset{\cdot}{J}} ∥ \overset{\cdot}{q} ∥ \end{matrix}

(25)

then $γ_{C 1}, γ_{C 2}, γ_{C 3} > 0$ exist such that

∥ C (q, \overset{\cdot}{q}) ∥ < (γ_{C 1} + γ_{C 2} ∥ q ∥ + γ_{C 3} ∥ q ∥^{2}) ∥ \overset{\cdot}{q} ∥

(26)

Proof. Extracting the norm from the definition of C in (11) yields

\begin{matrix} ∥ C (q, \overset{\cdot}{q}) ∥ \leq ∥ J_{m}^{T} (q) B_{ξ} (m (q)) {\overset{\cdot}{J}}_{m} (q, \overset{\cdot}{q}) ∥ \\ + ∥ J_{m}^{T} (q) C_{ξ} (m (q), J_{m} (q) \overset{\cdot}{q}) J_{m} (q) ∥ \\ \leq γ_{m} γ_{\overset{\cdot}{J}} ∥ B_{ξ} (m (q)) ∥ ∥ \overset{\cdot}{q} ∥ \\ + γ_{J}^{2} ∥ C_{ξ} (m (q), J_{m} (q) \overset{\cdot}{q}) ∥ \end{matrix}

(27)

Recalling that $B_{ξ}$ and $C_{ξ}$ are inertia components of a standard rigid robot yields (Sciavicco and Siciliano, 2012)

\begin{matrix} ∥ B_{ξ} (ξ) ∥ \leq κ_{B, 1} + κ_{B, 2} ∥ ξ ∥ + κ_{B, 3} ∥ ξ ∥^{2} \\ ∥ C_{ξ} (ξ, \overset{\cdot}{ξ}) ∥ \leq (κ_{C, 1} + κ_{C, 2} ∥ ξ ∥) ∥ \overset{\cdot}{ξ} ∥ \end{matrix}

(28)

with $κ_{B, 1}, κ_{B, 2}, κ_{B, 3}, κ_{C, 1}, κ_{C, 2}$ positive constants. Therefore, we introduce the two following upper bounds

\begin{array}{l} ∥ B_{ξ} (m (q)) ∥ \leq κ_{B, 1} + κ_{B, 2} ∥ m (q) ∥ \\ + κ_{B, 3} ∥ m (q) ∥^{2} \\ \leq κ_{B, 1} + κ_{B, 2} γ_{m} ∥ q ∥ + κ_{B, 3} γ_{m}^{2} ∥ q ∥^{2} \\ ∥ C_{ξ} (m (q), J_{m} (q) \overset{\cdot}{q}) ∥ \leq (κ_{C, 1} + κ_{C, 2} ∥ m (q) ∥) ∥ J_{m} (q) ∥ ∥ \overset{\cdot}{q} ∥ \\ \leq (κ_{C, 1} + κ_{C, 2} γ_{m} ∥ q ∥) γ_{J} ∥ \overset{\cdot}{q} ∥ . \end{array}

(29)

We can now plug these inequalities back into (27), obtaining

\begin{matrix} ∥ C (q, \overset{\cdot}{q}) ∥ \leq γ_{m} γ_{\overset{\cdot}{J}} (κ_{B, 1} + κ_{B, 2} γ_{m} ∥ q ∥ + κ_{B, 3} γ_{m}^{2} ∥ q ∥^{2}) \\ ∥ \overset{\cdot}{q} ∥ + γ_{J}^{2} (κ_{C, 1} + κ_{C, 2} γ_{m} ∥ q ∥) γ_{J} ∥ \overset{\cdot}{q} ∥ \end{matrix}

(30)

The thesis follows by defining

\begin{matrix} γ_{C 1} = γ_{m} γ_{\overset{\cdot}{J}} κ_{B, 1} + γ_{J}^{3} κ_{C, 1} \\ γ_{C 2} = γ_{m} γ_{\overset{\cdot}{J}} κ_{B, 2} γ_{m} + γ_{J}^{3} κ_{C, 2} γ_{m} \\ γ_{C 3} = γ_{m} γ_{\overset{\cdot}{J}} κ_{B, 3} . \end{matrix}

(31)

□

Remark 3. The hypotheses on $m (q)$ and on $J_{m} (q)$ in Lemmas 2 and 3, and Corollary 1 are verified for the representation described by (4), (8) and Table 1.

3. Control design

While it is largely accepted for describing the kinematics, the use of a PCC model in representing the infinite dimensionality of a soft robot can always introduce some mismatch with respect to the real system. The same holds for the introduction of approximations in the inertia distribution. Thus, it is critical to design controllers that are able to exploit the information given by the proposed model, while being robust to uncertainties. We thus avoid the use of complete feedback cancellations of robot dynamics (De Luca and Lucibello, 1998), as well as other kinds of control actions that present robustness issues in classic robots, such as pre-multiplications of feedback actions by the inverse of the inertia matrix (Nakanishi et al., 2008).

In the following, we present two novel feedback controllers following the described design principles. The first aims at implementing trajectory tracking in curvature space, whereas the second targets Cartesian impedance control with surface following. Note that the following results hold for any choice of maps $m (q)$ satisfying the hypotheses introduced in Section 2.3.

3.1. Curvature dynamic control

We propose the following controller for implementing trajectory tracking in the soft robot’s state space q

τ = K \bar{q} + D \overset{\cdot}{\bar{q}} + G (q) + C (q, \overset{\cdot}{q}) \overset{\cdot}{\bar{q}} + B (q) \overset{\cdot\cdot}{\bar{q}}

(32)

where $q, \overset{\cdot}{q} \in R^{n}$ are the bending angle vector and its derivative, $\bar{q} (t), \overset{\cdot}{\bar{q}} (t), \overset{\cdot\cdot}{\bar{q}} (t) \in R^{n}$ are the desired evolution and its derivatives expressed in the bending angle space, $B \in R^{n \times n}$ is the robot’s inertia, $C \in R^{n \times n}$ is the Coriolis and Centrifugal matrix obtained by computing $C_{ξ}$ through Christoffel symbols (Murray, 1994: Ch. 4), and $K \in R^{n \times n}$ and $D \in R^{n \times n}$ are the robot’s stiffness and damping matrices, respectively. Figure 7 presents a block representation of (32). It is worth noting that $K \bar{q} + D \overset{\cdot}{\bar{q}}$ are purely feedforward actions, whereas $G (q)$ , $C (q, \overset{\cdot}{q})$ , and $B (q)$ close the feedback loop by depending on q and $\overset{\cdot}{q}$ .

The resulting form of the closed-loop system is

B (q) (\overset{\cdot\cdot}{q} - \overset{\cdot\cdot}{\bar{q}}) + C (q, \overset{\cdot}{q}) (\overset{\cdot}{q} - \overset{\cdot}{\bar{q}}) = K (\bar{q} - q) + D (\overset{\cdot}{\bar{q}} - \overset{\cdot}{q})

(33)

The feedforward action $K \bar{q} + D \overset{\cdot}{\bar{q}}$ is combined with the physical impedance of the system, generating a natural proportional-derivative (PD) action $K (\bar{q} - q) + D (\overset{\cdot}{\bar{q}} - \overset{\cdot}{q})$ . In this way, the softness of the robot is preserved in sight of possible interactions with the environment (Angelini et al., 2018; Della Santina et al., 2017).

Let us consider for a moment the case for which $\overset{\cdot}{\bar{q}} \equiv 0$ . For simplicity, we also consider no gravitational field. For this scenario of a constant reference $\bar{q}$ , the control action (32) is purely feedforward, and (33) becomes a classical rigid robot controlled through a PD, which is well known to be globally asymptotically stable in the reference $\bar{q}$ (Sciavicco and Siciliano, 2012). This can be regarded as a manifestation of the self-stabilizing properties of soft robotic systems, which have been already recognized in previous works such as those of Bosi et al. (2016) and Thuruthel et al. (2018). The following theorem will show that the introduction of the dynamic feedback terms $G (q)$ , $C (q, \overset{\cdot}{q}) \overset{\cdot}{\bar{q}}$ , and $B (q) \overset{\cdot\cdot}{\bar{q}}$ enhances this property by also including a non-constant $\bar{q}$ into the range of the self-stabilized references.

Theorem 1. The continuously differentiable trajectory $\bar{q} (t)$ is a globally asymptotically stable equilibrium for the closed-loop system (33) under the hypotheses of Lemma 3, Corollary 1, and as soon as two positive constants $γ_{\bar{q}}$ and $γ_{\overset{\cdot}{\bar{q}}}$ exist such that

\bar{q} (t) < γ_{\bar{q}}, \overset{\cdot}{\bar{q}} (t) < γ_{\overset{\cdot}{\bar{q}}} \forall t

(34)

Proof. Let us define the error variable $e \dot{=} q - \bar{q}$ . The system (33) becomes

B (e + \bar{q}) \overset{\cdot\cdot}{e} + C (e + \bar{q}, \overset{\cdot}{e} + \overset{\cdot}{\bar{q}}) \overset{\cdot}{e} + Ke + D \overset{\cdot}{e} = 0

(35)

which is a nonlinear time-variant system owing to the explicit dependency from $\bar{q}$ .

The thesis can now be proven by considering as radially unbounded Lyapunov candidate the following natural extension of the energy function

V (e, \overset{\cdot}{e}, t) = \frac{1}{2} {\overset{\cdot}{e}}^{T} B (e + \bar{q}) \overset{\cdot}{e} + \frac{1}{2} e^{T} Ke

(36)

Note that in order to become a candidate, B has to be positive definite. This is assured by Corollary 1.

The time derivative of V is

\overset{\cdot}{V} (e, \overset{\cdot}{e}, t) = \frac{1}{2} {\overset{\cdot}{e}}^{T} \overset{\cdot}{B} \overset{\cdot}{e} + {\overset{\cdot}{e}}^{T} B \overset{\cdot\cdot}{e} + {\overset{\cdot}{e}}^{T} Ke

(37)

By substituting (35), we obtain

\overset{\cdot}{V} (e, \overset{\cdot}{e}, t) = \frac{1}{2} {\overset{\cdot}{e}}^{T} (\overset{\cdot}{B} - 2 C) \overset{\cdot}{e} - {\overset{\cdot}{e}}^{T} D \overset{\cdot}{e}

(38)

Using Lemma 1, the first term falls away and we obtain $\overset{\cdot}{V} (e, \overset{\cdot}{e}) = - {\overset{\cdot}{e}}^{T} D \overset{\cdot}{e} \leq 0$ . This assures that V decreases in time, and thus that $γ_{e}$ and $γ_{d}$ exist such that

∥ e ∥ < γ_{e}, ∥ \overset{\cdot}{e} ∥ < γ_{d}

(39)

As (35) is time-variant, LaSalle’s invariance principle cannot be applied directly for proving the stability of the closed-loop system (Khalil, 1996). We must instead invoke Barbalat’s lemma (Slotine and Li, 1991), which requires $\overset{\cdot}{V}$ to be uniformly continuous to assure stability. This can be proven by showing that its time derivative is bounded. Evaluating the second time derivative of the Lyapunov candidate yields

\overset{\cdot\cdot}{V} (e, \overset{\cdot}{e}, t) = 2 {\overset{\cdot}{e}}^{T} D B^{- 1} (C \overset{\cdot}{e} + Ke + D \overset{\cdot}{e})

(40)

which can be bounded as follows

∥ \overset{\cdot\cdot}{V} ∥ \leq 2 γ_{d} ∥ D ∥ γ_{B} (∥ C ∥ γ_{d} + ∥ K ∥ γ_{e} + ∥ D ∥ γ_{d})

(41)

where $γ_{B}$ is the inverse of the lower bound of the minimum singular value of B. Note indeed that the latter is always lower bounded by a positive constant because $B ≻ 0$ , as assured by Corollary 1. Note also that $∥ D ∥$ and $∥ K ∥$ are constant scalars. For the sake of conciseness of notation, we introduce the two positive constants $a = 2 γ_{d} ∥ D ∥ γ_{B} γ_{d}$ and $b = 2 γ_{d} ∥ D ∥ γ_{B} (∥ K ∥ γ_{e} + ∥ D ∥ γ_{d})$ . Equation (41) can thus be rewritten and then bounded as follows

\begin{matrix} ∥ \overset{\cdot\cdot}{V} ∥ \leq a ∥ C (e + \bar{q}, \overset{\cdot}{e} + \overset{\cdot}{\bar{q}}) ∥ + b \\ \leq a (γ_{C 1} + γ_{C 2} ∥ e + \bar{q} ∥ + γ_{C 3} ∥ e + \bar{q} ∥^{2}) ∥ \overset{\cdot}{e} + \overset{\cdot}{\bar{q}} ∥ + b \\ \leq a (γ_{C 1} + γ_{C 2} (γ_{e} + γ_{\bar{q}}) + γ_{C 3} (γ_{e} + γ_{\bar{q}})^{2}) (γ_{d} + γ_{\overset{\cdot}{\bar{q}}}) + b \end{matrix}

(42)

where we invoked Lemma 3, and we exploited the limitedness of the desired trajectory, which is imposed by hypothesis. The thesis follows by direct application of Barbalat’s lemma (Slotine and Li, 1991).

3.2. Preliminary robustness analysis

Here, we consider uncertainty in the knowledge of the stiffness and damping matrices, represented as

\tilde{K} = K + Δ K, \tilde{D} = D + Δ D

(43)

where $\tilde{K}, \tilde{D}$ are the estimations, $K, D$ the real values, and $Δ K, Δ D$ the perturbations. The control action (32) becomes $τ = \tilde{K} \bar{q} + \tilde{D} \overset{\cdot}{\bar{q}} + G (q) + C (q, \overset{\cdot}{q}) \overset{\cdot}{\bar{q}} + B (q) \overset{\cdot\cdot}{\bar{q}}$ . This results in the following closed-loop dynamics

\begin{matrix} B (q) (\overset{\cdot\cdot}{q} - \overset{\cdot\cdot}{\bar{q}}) + C (q, \overset{\cdot}{q}) (\overset{\cdot}{q} - \overset{\cdot}{\bar{q}}) = K (\bar{q} - q) + D (\overset{\cdot}{\bar{q}} - \overset{\cdot}{q}) \\ + Δ K \bar{q} + Δ D \overset{\cdot}{\bar{q}} \end{matrix}

(44)

Uncertainties only appear as an additional feedforward excitation of the system’s dynamics. Note that the unperturbed closed-loop system (33) is globally asymptotically stable as proven by Theorem 1. This implies that (44) is contractive, as discussed in Lohmiller and Slotine (1998). Contractiveness assures convergence of all the trajectories of the system to a single one independently from the initial condition. To prove practical stability is then sufficient to find at least one trajectory of the system that does not diverge. Consider, for example, a quasi static reference, that is $\overset{\cdot}{\bar{q}} \approx 0$ . The following equilibrium configuration $q_{eq}$ results

K (\bar{q} - q_{eq}) + Δ K \bar{q} = 0 \Rightarrow q_{eq} = (I + K^{- 1} Δ K) \bar{q}

(45)

which, in turn, is globally asymptotically stable thanks to the contractiveness of the system. Therefore, having uncertainty in the knowledge of stiffness and damping matrices just moves the equilibrium by $K^{- 1} Δ K \bar{q}$ .

In case $q_{eq}$ is not close enough to the desired reference $\bar{q}$ , the control (32) can be generalized to include an explicit position and damping control loop.

Corollary 2. Under the hypotheses of Lemma 3 and Corollary 1 the control action

\begin{matrix} τ = K \bar{q} + D \overset{\cdot}{\bar{q}} + G (q) + C (q, \overset{\cdot}{q}) \overset{\cdot}{\bar{q}} + B (q) \overset{\cdot\cdot}{\bar{q}} + K_{P} (\bar{q} - q) \\ + K_{D} (\overset{\cdot}{\bar{q}} - \overset{\cdot}{q}) \end{matrix}

(46)

with $K_{P}, K_{D} \in R^{n \times n}$ makes $\bar{q}$ a globally asymptotically stable trajectory of the closed loop, if

K_{P} ≻ - K, K_{D} ≻ - D

Proof. The closed loop of the system is obtained from the application of (46) to (15) and results in

B (q) (\overset{\cdot\cdot}{q} - \overset{\cdot\cdot}{\bar{q}}) + C (q, \overset{\cdot}{q}) (\overset{\cdot}{q} - \overset{\cdot}{\bar{q}}) = \tilde{K} (\bar{q} - q) + \tilde{D} (\overset{\cdot}{\bar{q}} - \overset{\cdot}{q})

with

\tilde{K} = K_{P} + K, \tilde{D} = K_{D} + D

(47)

This closed loop generalizes (33). The thesis follows by applying Theorem 1 under consideration of hypotheses $\tilde{K} ≻ 0$ and $\tilde{D} ≻ 0$ .

The control approach actually enforces robustness. The system’s closed-loop equilibrium becomes

q_{eq}^{cl} = (I + (K_{P} + K)^{- 1} Δ K) \bar{q}

(48)

Here $q_{eq}^{cl}$ is always closer to $\bar{q}$ than the equilibrium defined by (45), as soon as $K_{P} ≻ 0$ . However, this behaviour is obtained at the cost of stiffening the robot. This is discussed in the general case by Della Santina et al. (2017), and it is made evident here by Equation (47).

3.3. Cartesian stiffness control

We consider a set of contact points with coordinates $x \in R^{k}$ . Through them, the environment can exert on the soft robot a set of wrenches $f_{ext}$ , mapped through the Jacobian $J (q)$ , as described in (15).

A correct regulation of the impedance at the contact points is essential to implement robust and reliable interactions with the environment (Ott, 2008). We propose to implement the desired compliant behavior through the following dynamic feedback loop

\begin{matrix} τ = J^{T} (q) J_{B}^{+ T} (q) (K q + D \overset{\cdot}{q}) + G (q) + J^{T} (q) η (q, \overset{\cdot}{q}) \\ (I - J_{B}^{+} (q) J (q)) \overset{\cdot}{q} + J^{T} (q) (K_{c} (x_{d} - x) - D_{c} J (q) \overset{\cdot}{q}) \end{matrix}

(49)

where $q, \overset{\cdot}{q} \in R^{n}$ are the soft robot’s bending angle vector and its derivative, $J (q) \in R^{k \times n}$ is the Jacobian, which maps $\overset{\cdot}{q}$ into the end-effector velocity $\overset{\cdot}{x}$ , $x_{d} \in R^{k}$ is a reference position for the contact point, x is the configuration of the contact point, and $J_{B}^{+} \in R^{k \times n}$ is the dynamically consistent pseudo-inverse (Chang and Khatib, 1995) described by¹

J_{B}^{+} (q) = B^{- 1} (q) J^{T} (q) {(J (q) B^{- 1} (q) J^{T} (q))}^{- 1}

(50)

that can be used to map forces in configuration space towards their counterpart in operational space. Here $η (q, \overset{\cdot}{q}) \in R^{k \times n}$ is the Cartesian Coriolis and centrifugal matrix, defined as

η (q, \overset{\cdot}{q}) = (J B^{- 1} J^{T})^{- 1} (J B^{- 1} C - \overset{\cdot}{J}) = J_{B}^{T} C - Λ \overset{\cdot}{J}

(51)

with the dependencies on $q, \overset{\cdot}{q}$ not reported. Here $Λ (q)$ is the Cartesian inertia matrix (Khatib, 1987), defined as

Λ (q) = (J (q) B^{- 1} (q) J (q)^{T})^{- 1} \in R^{k \times k}

(52)

The term $J^{T} (q) (K_{c} (x_{d} - x) - D_{c} J (q) \overset{\cdot}{q})$ simulates the presence of a spring and a damper connected between x and $x_{d}$ . This imposes the desired Cartesian stiffness $K_{c}$ , and Cartesian damping $D_{c}$ . We will choose these matrices to be diagonal in order to implement a full decoupling within the Cartesian directions. The remaining terms act as a compensation of the non-diagonal Cartesian stiffness $J^{T} (q) J_{B}^{+ T} (q) K q + G (q)$ , and as a decoupling of the Cartesian dynamics from the residual dynamics of the redundant degrees of freedom $J^{T} (q) (J_{B}^{+ T} (q) D + η (q, \overset{\cdot}{q}) (I - J_{B}^{+} (q) J (q))) \overset{\cdot}{q}$ . Figure 8 presents a block representation of (49).

The following theorem assures that the closed-loop system implements the desired compliant behavior at the contact point.

Theorem 2. The closed loop defined by controller (49) and system (15) is such that

lim_{t \to \infty} K_{c} (x (t) - x_{d}) = f_{ext}

(53)

for all $K_{c} ≻ 0$ and $D_{c} ≻ 0$ , ${\overset{\cdot}{x}}_{d} = 0$ , ${\overset{\cdot}{f}}_{ext} = 0$ , and for every $x (0) \in R^{k}$ , under the hypotheses of Corollary 1.

Proof. We augment the operational space of the soft robot at the velocity level through a set of complementary velocities $v_{n} \in R^{n - k}$

[\begin{matrix} \overset{\cdot}{x} \\ v_{n} \end{matrix}] = [\begin{matrix} J \\ N \end{matrix}] \overset{\cdot}{q}

(54)

where $N \in R^{(n - k) \times n}$ is chosen such that the resulting transformation is full rank. Note that the dependencies on $q, \overset{\cdot}{q}$ are not reported in this proof whenever not necessary. The inverse of $[J^{T}, N^{T}]^{T}$ can be written as $[J_{B}^{+}, Z^{T}]$ , where Z is an opportunely defined matrix. We do not report the proof and the form of Z here, which can be found in Ott (2008). Note that B is invertible since Corollary 1 holds by hypothesis, a property needed for (50).

Thus, Equation (54) yields

\overset{\cdot}{q} = J_{B}^{+} \overset{\cdot}{x} + Z^{T} v_{n}

(55)

Using the Jacobian from (54) we can apply a transformation into operational space (Ott, 2008: Ch. 4) on the system (15) to obtain the open-loop operational space dynamics

\begin{matrix} Λ \overset{\cdot\cdot}{x} + η_{x} \overset{\cdot}{x} + η_{N} v_{n} + J_{B}^{+ T} D \overset{\cdot}{q} + J_{B}^{+ T} (K q + G) \\ = J_{B}^{+ T} τ + f_{ext} \end{matrix}

(56)

The matrix $J_{B}^{+ T}$ is the dynamically consistent pseudo-inverse (50). The Coriolis and centrifugal terms are collected by $η_{x} = η J_{B}^{+} \in R^{k \times k}$ and $η_{N} = η Z^{T} \in R^{k \times (n - k)}$ , with $η$ defined in (51). Here $Λ$ is defined in (52).

Substituting the controller (49) into (56) yields

\begin{matrix} Λ \overset{\cdot\cdot}{x} + η_{x} \overset{\cdot}{x} + η_{N} v_{n} + J_{B}^{+ T} D \overset{\cdot}{q} + J_{B}^{+ T} (Kq + G) \\ = J_{B}^{+ T} (K q + D \overset{\cdot}{q}) + J_{B}^{+ T} G + η Z^{T} v_{n} \\ + K_{c} (x_{d} - x) - D_{c} J \overset{\cdot}{q} + f_{ext} \end{matrix}

(57)

where we exploited that $J_{B}^{+ T} J^{T} = I$ , and that (55) yields

Z^{T} v_{n} = \overset{\cdot}{q} - J_{B}^{+} \overset{\cdot}{x} = (I - J_{B}^{+} J) \overset{\cdot}{q}

(58)

Through simple algebraic manipulations, Equation (57) yields

Λ (q) \overset{\cdot\cdot}{x} + η_{x} (q, \overset{\cdot}{q}) \overset{\cdot}{x} + D_{C} \overset{\cdot}{x} + K_{C} (x - x_{d}) = f_{ext}

(59)

Note that the controller left the dynamic terms unchanged and only removed the dynamic coupling with the residual dynamics, as discussed earlier in this section.

We prove the thesis for a generic evolution $q (t)$ in joint space. Under this assumption, Equation (59) is rewritten as

Λ (t) \overset{\cdot\cdot}{e} + η (t) \overset{\cdot}{e} + D_{C} \overset{\cdot}{e} + K_{C} e = 0

(60)

where $e = x - x_{d} - K_{C}^{- 1} f_{ext}$ and $\overset{\cdot}{e} = \overset{\cdot}{x}$ . Thus, proving (53) is equivalent to proving the asymptotic stability around the origin of (60). We consider the following Lyapunov candidate

V (e, \overset{\cdot}{e}, t) = \frac{1}{2} {\overset{\cdot}{e}}^{T} Λ (t) \overset{\cdot}{e} + \frac{1}{2} e^{T} K_{C} e

(61)

which is radially unbounded and uniformly positive for each $e, \overset{\cdot}{e}$ . Note that this function is a straightforward generalization of the commonly employed total energy of the system (Sciavicco and Siciliano, 2012). The time derivative of (61) is

\begin{matrix} \overset{\cdot}{V} (e, \overset{\cdot}{e}, t) = \frac{1}{2} {\overset{\cdot}{e}}^{T} \overset{\cdot}{Λ} (t) \overset{\cdot}{e} + {\overset{\cdot}{e}}^{T} Λ (t) \overset{\cdot\cdot}{e} + {\overset{\cdot}{e}}^{T} K_{C} e \\ = \frac{1}{2} {\overset{\cdot}{e}}^{T} (\overset{\cdot}{Λ} (t) - 2 η (t)) \overset{\cdot}{e} - {\overset{\cdot}{e}}^{T} D_{C} \overset{\cdot}{e} \\ = - {\overset{\cdot}{e}}^{T} D_{C} \overset{\cdot}{e} \end{matrix}

(62)

where we used (60) for the first step, and the skew symmetry of $\overset{\cdot}{Λ} (t) - 2 η (t)$ for the second step. Note that the latter derives from Lemma 1, combined with standard results in the operational space formulation (Ott, 2008: Ch. 4). Thus, $\overset{\cdot}{V} \leq 0$ , because $D_{c} ≻ 0$ by hypothesis.

Invoking the LaSalle–Yoshizawa theorem (Khalil, 1996) yields $\overset{\cdot}{e} \to 0$ . This, in turn, implies that $e \to 0$ through (60), from which the thesis is proven.

Remark 4. Equation (49) performs a cancellation of only the parts of the elastic field and the dissipative forces that act on the operational space dynamics. In this way, the redundant degrees of freedom can reach a natural equilibrium without the need to explicitly impose such. This is in contrast to the rigid case where it is necessary to impose the equilibrium (Ott, 2008).

3.4. Contact planning

For the sake of clarity, we consider in the following as a single point of contact the soft robot’s end effector, that is $x \in R^{2}$ . The proposed controller (49) imposes on the end effector of the soft robot the dynamics of a simple spring–damper–mass system (59), albeit with a configuration-dependent inertia. Furthermore, Theorem 2 assures that the equilibrium resulting from an interaction with the environment is as expected from such a system.

This allows higher-level policies to be written in an intuitive way. We define a local frame $(n_{∥}, n_{⊥})$ connected to the end effector, as depicted in Figure 9. The unit vector $n_{∥}$ is chosen to be always tangent to the environment. The unit vector $n_{⊥}$ is such that $n_{∥}^{T} n_{⊥} = 0$ , and always points from the inside to the outside of the environment. We assume the knowledge of the following information:

the coordinate $x_{0}$ of a point included within the environment;

the occurrence of a contact between the end effector and the environment, acquired by isInContact();

parallel $n_{∥}$ and perpendicular $n_{⊥}$ unit vectors at the contact point, which are extracted by the methods readParallelDirection() and readPerpendicularDirection(), respectively;

the final target $x_{t}$ on the surface of the environment.

We specify the value of $x_{d}$ on-line through a simple planning procedure, described by Algorithm 1. It consists of two phases: approaching and exploring. In the first phase (lines 1–3), a generic point inside the environment $x_{0}$ is selected as reference for the impedance controller. When the soft robot makes contact with the environment, the second phase begins (lines 4–8). Here, the desired end-effector position is chosen as the final target $x_{t}$ . A constant displacement $- n_{⊥} δ_{ct}$ is introduced to ensure that the contact with the environment is maintained during the exploration. Algorithm 1 terminates when the seminorm of the error weighted on $n_{∥} n_{∥}^{T}$ is under a manually defined threshold $ϵ$ . In this way, only the error along the surface is considered. Figure 9 presents a sketch of the two phases of Algorithm 1.

Algorithm 1. High-level control.

1. while isInContact() == False do

x_{d} \leftarrow x_{0}

3. end while

4/ while

∥ x - x_{t} ∥_{n_{∥} n_{∥}^{T}} > ϵ

n_{∥} \leftarrow readParallelDirection ()

n_{⊥} \leftarrow readPerpendicularDirection ()

x_{d} \leftarrow x_{t} - n_{⊥} δ_{ct}

8. end while

The environment provides guidance for the end effector that helps to keep the error between the desired and the actual position low in the perpendicular direction $n_{⊥}$ . To mitigate the error tangentially, we introduce an integral action acting on the tangential direction $I_{c} J^{T} (q) n_{∥} \int n_{∥}^{T} (x_{d} - x)$ when the soft robot is in contact with the environment. Theorem (2) can be extended to consider this term by using a Lyapunov candidate analogous to that proposed in Kelly (1995).

4. Simulation results

We first introduce the FEM used for the simulation, followed by a description of the considered identification procedure. We then introduce the benchmark controller, which we then use for comparisons with our proposed curvature and Cartesian impedance controllers. For the simulation results presented in this section, we assume the map given in (4) for the implementation of our control strategies described in Section 3. This map implements the configuration of Figure 4(c) and presents a balanced trade-off between good modeling accuracy and low complexity in the number of states.

4.1. Considered model

In this section, we validate the proposed control strategies outside the design hypothesis of PCC. The control algorithms are applied to a planar soft manipulator that is simulated through a FEM. The model is the discretization of a continuum rod as a series of infinitesimal links (see, for example, Penning and Zinn, 2014).

The total length of the arm is 1 m, and the arm is divided into six actuated segments of length 0.175 m. Each segment is discretized into 10 rigid links, connected through revolute joints with parallel axes. We refer to the joint angles as $q_{h} \in R^{60}$ . Each link weighs 13 g, and it is connected to the subsequent element through a linear torsional spring–damper system. The stiffness is 5 N m rad⁻¹ and the damping is 20 N m s rad⁻¹. The base frame is rotated such that in the rest position, that is $q_{h} = 0$ , the tip of the soft arm is pointing downwards and the soft arm aligned with the direction of the gravitational field.

The state $q_{h}, {\overset{\cdot}{q}}_{h} \in R^{60}$ of the soft robot is mapped in real-time to an equivalent vector of CCs $q \in R^{6}$ and their derivatives $\overset{\cdot}{q} \in R^{6}$ . The equivalent vector is then used by the controller. The mapping is done by matching the orientation of the end of each actuated segment with the corresponding element in the PCC model. In this particular example, this can be accomplished by summing up for all finite elements in one segment their angles and their angular velocities, respectively.

4.2. Identification

The model (15) has several free parameters, those are masses $μ_{i}$ , lengths $L_{i}$ , stiffnesses $k_{i}$ , and damping $d_{i}$ . We measure lengths $L_{i}$ and masses $μ_{i}$ directly. The data used for the identification are collected through a simulated experiment. A constant input torque of magnitude 1 N m is applied to each segment. The initial condition is $q_{h} = 0$ rad, ${\overset{\cdot}{q}}_{h} = 0$ rad s⁻¹.

The remaining parameters are identified by minimizing the 2-norm of the error between the estimated and measured evolutions. This can be done by rewriting the dynamics as a balance between the known dynamical forces, and the product of unknown parameters and their regressor (Ljung, 1998). We use the More–Penrose pseudo–inversion to extract the solution

[\begin{matrix} \hat{k} \\ \hat{d} \end{matrix}] = {[\begin{matrix} diag {q_{I} (t_{0})} & \dots & diag {q_{I} (t_{f})} \\ diag {{\overset{\cdot}{q}}_{I} (t_{0})} & \dots & diag {{\overset{\cdot}{q}}_{I} (t_{f})} \end{matrix}]}^{+ T} f_{d} (q_{I}, {\overset{\cdot}{q}}_{I})

(63)

where $q_{I} (t)$ and ${\overset{\cdot}{q}}_{I} (t)$ are the measured evolutions of q and $\overset{\cdot}{q}$ at time t. With $f_{d} (q_{I}, {\overset{\cdot}{q}}_{I})$ we refer to the set of known forces $- B \overset{\cdot\cdot}{q} - C \overset{\cdot}{q} - G + τ$ evaluated for each element of $q_{I}$ and ${\overset{\cdot}{q}}_{I}$ . The identified stiffness and damping are

\begin{matrix} \hat{k} & = [0.499; 0.499; 0.499; 0.500; 0.498; 0.510] N m {rad}^{- 1} \\ \hat{d} & = [2.00; 2.01; 2.02; 2.01; 2.03; 1.84] N m s {rad}^{- 1} \end{matrix}

4.3. Benchmark controller

We compare the simulated results against state-of-the-art benchmarks in continuum soft robot manipulators. More specifically, for regulating the robot’s configuration q we consider a proportional–integral–derivative (PID) controller as discussed by Bajo et al. (2011), Marchese and Rus (2016), and George Thuruthel et al. (2018) and described by

τ = k_{P} (\bar{q} - q) + k_{I} \int (\bar{q} - q) + k_{D} (\overset{\cdot}{\bar{q}} - \overset{\cdot}{q})

(64)

where $\bar{q}$ and $\overset{\cdot}{\bar{q}}$ are a reference curvature and its derivative, for example defined by (66). Here $k_{P}$ , $k_{I}$ , and $k_{D}$ are the PID gains to be tuned.

We could not find in the state of the art any insight about tuning the PID gains for soft robots. In order to have a standard comparison, we consider the well-known Skogestad internal model control (SIMC) tuning rule (Skogestad, 2003), which can be regarded as the state of the art in PID tuning. Using the SIMC-PID tuning, the resulting gains are

\begin{matrix} k_{P} = diag {7.2, 3.6, 9.3, 11.2, 9.0, 7.2} N m {rad}^{- 1} \\ k_{I} = diag {2.8, 3.2, 3.4, 3.6, 3.7, 4.0} N m {rad}^{- 1} s^{- 1} \\ k_{D} = diag {0.9, 1.3, 1.4, 1.8, 1.8, 1.8} N m {(rad s)}^{- 1} \end{matrix}

(65)

To regulate the end effector for this comparison, we use the kinematic inversion algorithm

\overset{\cdot}{\bar{q}} = J^{+} (q) (x_{d} - x)

(66)

where q is the robot’s configuration, $x_{d}$ is the desired end-effector position, and x is the actual end-effector position. Then $\overset{\cdot}{\bar{q}}$ and $\bar{q}$ , obtained by direct integration, are commanded to the PID controller (64)–(65) to regulate towards $x_{d}$ . The use of the kinematic inversion algorithm in continuous soft robots is discussed, for example, in Webster III and Jones (2010), Zhang et al. (2016), and George Thuruthel et al. (2018).

4.4. Trajectory tracking in curvature space

We test the ability of the curvature controller (32) to produce an accurate trajectory tracking in curvature space q with the following trajectory

\bar{q} (t) = {[\begin{matrix} \frac{π}{12} & \dots & \frac{π}{12} \end{matrix}]}^{T} \sin (ω t)

(67)

We consider $20$ logarithmically distributed frequencies $ω$ , from 0.1 to 10 rad s⁻¹, with both our proposed controller (32) and the benchmark controller (64)–(65). Note that increasing $ω$ reflects linearly into the amplitude of $\overset{\cdot}{\bar{q}}$ and quadratically into the amplitude of $\overset{\cdot\cdot}{\bar{q}}$ . Thus, the considered range of frequencies allows to test both quasi-static and strongly dynamic trajectories. The robot starts from the initial condition $q_{h} = 0$ rad, ${\overset{\cdot}{q}}_{h} = 0$ rad s⁻¹. For each trial, we simulate 120 s to properly capture the steady-state behavior. The performance is evaluated through the $L_{2}$ norm defined as

p_{L 2} = \sqrt{\int_{0}^{t_{f}} \frac{| \bar{q} - q |_{2}^{2}}{t_{f}}}

(68)

where $t_{f} = 120$ s, and $| \cdot |_{2}$ is the Euclidean norm. Figure 10 presents the results of these simulations. The proposed controller (32) outperforms the benchmark controller (64)–(65) for almost all the considered frequencies. For quasi-static conditions, however, the two controllers have comparable performances, and the PID performance monotonically decreases with an increase in frequency. In contrast, the performance of the proposed controller is even better under dynamic conditions than under static conditions. Figure 11 shows the evolution of the curvature q in time, for $ω = 1$ rad s⁻¹. The PID controller is able to track with sufficient accuracy only the reference curvatures of those segments that are close to the tip of the robot. The dynamic effects are less relevant for these elements close to the tip of the robot. In contrast, the proposed controller is able to correctly regulate with good precision all degrees of curvature q on the desired trajectory.

4.5. Cartesian impedance control and surface following

We consider the task of reaching a point on a planar surface. The surface is placed vertically at a $0.4$ m distance from the robot’s rest position, as shown in Figure 12. The surface is simulated as a repelling force field with sigmoidal characteristics described as

f_{contact} = \frac{f_{mag}}{1 + \exp (- a δ_{pen})}

(69)

where $a = 10^{4}$ is the gain, $f_{mag} = 10$ N is the force magnitude, and $δ_{pen}$ is the penetration depth relative to the surface. As previously, two control strategies are considered: the benchmark (64)–(66), and the proposed Cartesian impedance controller (49). The desired Cartesian stiffness is $K_{C} = diag {3, 3}$ N m⁻¹, and the desired Cartesian damping is $D_{C} = diag {3, 3}$ N s m⁻¹. For both controllers the reference $x_{d}$ is planned as described in Section 3.4. For phase 1, we consider as the point inside the environment $x_{0} = [1 - 0.5]^{T}$ m. The control algorithm tries to regulate the end effector to that point, that is $x_{d} = x_{0}$ , until a contact with the environment is established. The magnitude of the displacement $δ_{ct}$ is chosen here to be equal to $0.5$ m. Each closed-loop system is then simulated for a duration of 30 s.

Figure 12 shows the resulting behavior of the soft robot for each of the two controllers. The benchmark (Figure 12(a)) behaves well until the contact with the environment is established. After that, it starts moving the end effector in the wrong direction, away from the target. This is due to the non-diagonal form of the Cartesian stiffness matrix. A horizontal force generates a vertical displacement owing to the non-diagonal coupling terms. The robot ends up stuck in an undesired equilibrium while pushing towards the wall. In contrast, the proposed controller produces a desired impedance behavior at the end effector and does not manifest this problem. Figure 13 presents the evolution over time for the position of the end effector.

We present in Figure 14 the evolution of full configuration $q_{h}$ and equivalent bending angle q for the six segments composing the soft robot. It is interesting to note that the algorithm works properly despite the fact that the actual local curvatures, represented as dotted lines, are widely spread around the equivalent PCC curvature, shown as a solid line.

Repeating the two simulations for 30 targets equally distributed between $[0.4, - 0.8]$ m and $[0.4, - 0.2]$ m, the benchmark (64)–(66) always presents the same poor behavior. The proposed controller likewise is always able to regulate the tip of the soft robot onto the desired position, with an average error over all repetitions of $2 \times 10^{- 3}$ m (minimum $2.0 \times 10^{- 5}$ m, maximum $9.5 \times 10^{- 3}$ m).

5. Experimental results

In this section, we first describe the experimental setup, followed by the identification procedure. Using these identified parameters, we then validate the proposed curvature controller and the Cartesian impedance controller with surface following. Just as we did it for the simulation, we also assume the map given in (4) for the experimental implementation of our control strategies described in Section 3. Again, the map presents a balanced trade-off between good modeling accuracy and low complexity in number of states.

5.1. Experimental setup

In Figure 1 we show the experimental setup on which we tested the proposed control strategies. It is a modified version of a soft planar robotic arm previously used for kinematic motions within confined environments (Marchese et al., 2014), and for autonomous object manipulation (Katzschmann et al., 2015). The robot is highly deformable, as shown heuristically in Figure 15.

The soft robot used here is composed of six segments with inflatable cavities that allow for bidirectional actuation of each segment. Each segment of the soft arm is 6.3 cm long. The connecting element between each segment is supported vertically by two ball transfers that allow the arm to move with low friction on a level plane. The independent pneumatic actuation of the bidirectional arm segments is achieved through an array of 12 pneumatic cylinders driven by linear actuators.² The available inputs to our soft robot are the desired placements of the pistons within the cylinders. They are expressed in encoder tics, ranging from $0$ to 1,000 tics. This value is regulated by a local PID controller on the motor controller board³ of each linear actuator. The relationship between desired placement and exerted force by each chamber can be approximated as linear through the ideal gas law and by the hypothesis of homogeneous pressure distribution over chambers and pistons sections. When a positive torque within a segment is required by the controller, the right chamber of a segment is inflated and the left chamber is deflated by the same amount. Vice versa, an inflation of the left chamber is required for creating a negative torque. In order to avoid a dead zone effect that otherwise would be present for low actuation pressures, we pre-load each chamber. The amount of pre-loading is defined heuristically as the lowest value for which a sinusoidal oscillation with an amplitude of 100 tics is visually perceivable. The resulting linear characteristics between the desired torque $τ_{i}$ and the corresponding placement of the ith piston $l_{i}$ is

τ_{i} = g_{i} l_{i}

(70)

where $g_{i}$ collects all the previously described effects, and it is identified through data fitting in Section 5.2. Figure 16 supports the hypothesis of linearity in actuation and stiffness, by showing that the steady state of a single segment follows a linear trend with respect to the actuation $l_{i}$ . This serves to validate both the linear model of elasticity and the linear model of actuation. Instead, this is not a validation of the proposed model based controllers, which are tested in next section.

We use a motion capture system⁴ to acquire the robot’s posture. The system provides real-time measurements of groups of reflective markers along the back of the soft arm. Groups of four markers are placed at the root and the end of each segment in order to identify the reference frames ${S_{0}}, \dots, {S_{6}}$ . The bending angle q is evaluated by measuring the relative rotation between the frames, and the derivative $\overset{\cdot}{q}$ is obtained through numerical derivation of q. A rigid frame holds all five cameras of the motion capture system in a fixed orientation providing reliable hardware experiments without the need for frequent camera recalibration.

The software architecture is executed on two PC workstations. The first workstation acquires in real-time data from the motion capture system, evaluates the control action, and communicates it to the second workstation via the User Datagram Protocol (UDP). The code is implemented in MATLAB R2017b. We use Peter Corke’s robotics toolbox (Corke, 1996) to evaluate $B_{ξ}$ , $C_{ξ}$ , $G_{ξ}$ , and $J_{ξ}$ in (11). In the experiments that we present in the next sections, the code runs with a control loop period of $0.0169 \pm 0.0023$ s for the curvature controller (32), and with a control loop period of $0.0199 \pm 0.0089$ s for the Cartesian Impedance controller (49). Both controllers appeared to be robust to a non-constant execution rate, whereas other model-based controllers typically have issues with instabilities when executed with non-constant execution rates (Hespanha et al., 2007). The second workstation receives the messages and interfaces with the motor control boards of the linear actuators.

5.2. Identification

The identification procedure is analogous to that introduced in Section 4.2. In addition to stiffness and damping, we account here for the presence of the actuators by introducing a set of gains $g_{i}$ statically mapping the cylinder lengths $l_{i}$ into corresponding segment torques $τ_{i}$ , as in (70). To prevent over-fitting, we hypothesize the same damping value ${\hat{d}}_{s}$ for each segment of the robot. The resulting estimation is

\begin{matrix} [\begin{matrix} \hat{g} \\ \hat{k} \\ {\hat{d}}_{s} \end{matrix}] = {[\begin{matrix} - diag {l_{I} (t_{0})} & \dots & - diag {l_{I} (t_{f})} \\ diag {q_{I} (t_{0})} & \dots & diag {q_{I} (t_{f})} \\ {\overset{\cdot}{q}}_{I} {(t_{0})}^{T} & \dots & {\overset{\cdot}{q}}_{I} {(t_{f})}^{T} \end{matrix}]}^{+ T} \\ f_{d} (q_{I}, {\overset{\cdot}{q}}_{I}) \end{matrix}

(71)

where $q_{I} (t)$ and ${\overset{\cdot}{q}}_{I} (t)$ are the measured evolutions of q and $\overset{\cdot}{q}$ at time t, and $l_{I}$ is the generated input at time t. With $f_{d}$ we refer to the set of known forces $- B \overset{\cdot\cdot}{q} - C \overset{\cdot}{q}$ evaluated for each element of $q_{I}$ and ${\overset{\cdot}{q}}_{I}$ .

The identification data are collected in three experiments. In each one a saturated ramp is injected into each pneumatic cylinder. The amplitudes are 500 tics, 700 tics, and 900 tics, respectively. We choose a ramp with a slope equal to 166 tics s⁻¹ for all the experiments, manually fixed so to be under the velocity saturation threshold of the motors. We run the whole identification procedure at the beginning of each session of experiments. The identified parameters across 10 runs of the algorithm are

\begin{matrix} \hat{k} = [6.0, 6.5, 6.5, 6.6, 5.7, 3.7]^{T} \\ \times 10^{- 2} \pm [0.24, 0.32, 0.43, 0.55, 0.84, 0.52]^{T} \\ \times 10^{- 2} N m {rad}^{- 1} \\ {\hat{d}}_{s} = 0.0119 \pm 0.0025 N m s {rad}^{- 1} \\ \hat{g} = [2.4, 3.4, 2.7, 2.8, 3.4, 2.3]^{T} \\ \times 10^{- 5} \pm [5.0, 7.2, 6.0, 9.0, 7.6, 6.4]^{T} \\ \times 10^{- 7} N m tic s^{- 1} \end{matrix}

(72)

The lengths $L = [0.06 \dots 0.06]$ m and the masses $μ_{i} = [0.334 \dots 0.334]$ kg are measured directly.

5.3. Curvature control

To test the curvature controller (32), we start by considering the tracking of

\begin{matrix} {\bar{q}}_{i} (t) = α (1 + \cos (ω t)) \forall i \in {1, \dots, 6}, t \in [0, 20) s \\ \forall α \in \frac{π}{180} {7, 10, 13} rad \\ \forall ω \in \frac{2 π}{20} {7, 8, 9, 10} rad s^{- 1} \end{matrix}

(73)

thus testing the effectiveness of the proposed controller for an exhaustive range of frequencies and amplitudes. The maximum bending amplitude in Cartesian coordinates is reached at the tip, and it is equal to $12 α$ , which is close to $π$ for the largest $α$ considered. Larger amplitudes would move the robot outside the experimental workspace. Note that the control action $τ_{i}$ is mapped to the desired piston length $l_{i}$ by dividing for the gains $g_{i}$ , that is by inverting Equation (70). Figure 17 presents tracking performances expressed in $L_{2}$ norm of the tracking error (68), with $t_{f} = 20$ s. The algorithm is able to produce a stable oscillation close to the commanded one for all considered trajectories. Note that the best performances are obtained for intermediate values of the frequency $ω$ . This can be explained by considering that for low frequencies the quality is diminished by the presence of static friction between the robot and the plane, whereas for higher frequencies the required torque is often over the saturation limits.

Figure 18 shows the evolution of bending angle q and commanded torques $τ$ for a case with average performances, that is $α = 13^{°}$ and $ω = 2.5133$ rad s⁻¹. In Figure 19(a)–(e), we report the photo sequence of one of the resulting oscillations for the same trajectory.

In Figure 20 we present the tracking of

{\bar{q}}_{i} (t) = {\begin{matrix} + 13 \frac{π}{180} (1 + \cos (8 \frac{2 π}{20} t)) & if i \in {1, 2, 3} \\ - 13 \frac{π}{180} (1 + \cos (8 \frac{2 π}{20} t)) & if i \in {4, 5, 6} \end{matrix}

(74)

with $t \in [0, 20)$ s, and Figure 19(f)–(j) shows the photo sequence of one of the resulting oscillations.

Finally, in Figure 21 we present the tracking of

{\bar{q}}_{i} (t) = {\begin{matrix} 0 & if i \in {1, 2} \\ 13 \frac{π}{180} (1 + \cos (10 \frac{2 π}{20} t)) & if i \in {3, 4, 5, 6} \end{matrix}

(75)

with $t \in [0, 20)$ s, and Figure 19(k)–(o), shows the photo sequence of one of the resulting oscillations. Relying on the knowledge of the model, the algorithm is able to anticipate the necessary control action to maintain the first two segments still, despite the large oscillation of the remaining four segments. This is evident from the evolution of $τ_{1}$ and $τ_{2}$ in Figure 21(b), which have a larger amplitude and a phase shift of about half a period relative to $τ_{3}$ , $τ_{4}$ , $τ_{5}$ , and $τ_{6}$ .

5.4. Cartesian impedance control and surface following

Figure 22 presents the evolutions resulting from the application of (49) in regulating the soft robot’s end effector position. The input to the pistons is produced by dividing $τ$ (Figure 22(c)) by the identified static gains g. We approximate here $D \approx 0$ . The desired impedance at the end effector is

\begin{matrix} K_{c} & = [\begin{matrix} 0.825 & 0 \\ 0 & 0.825 \end{matrix}] N m^{- 1} \\ D_{c} & = [\begin{matrix} 0.25 & 0 \\ 0 & 0.25 \end{matrix}] N m^{- 1} s^{- 1} \end{matrix}

(76)

The desired end effector position is $x_{d} = [0.2637, 0.2196]^{T}$ m, and it was manually defined to be approximately in the center of the robot’s reachable space. Figure 22(a) shows the evolution of end effector position x expressed in Cartesian coordinates. The system reaches the correct steady state after an oscillatory transient, coherent with the imposed behavior. Such oscillations reflect into more complex non-linear oscillations of the bending angle q, presented in Figure 22(b).

However, the main feature of the Cartesian Impedance regulator is to generate reliable interactions with an unstructured environment. We thus test (49) in combination with Algorithm 1, in implementing the desired surface following behavior.

At the beginning of each experiment, a surface is placed in front of the robot, as shown in Figure 24(a), (f), (k), (p), and (u). Remember that the robot is not aware of the exact shape of the surface, or its position. The only information that Algorithm 1 uses are the presence of a contact, a measure of the local tangent direction $n_{∥}$ , and the coordinates of a point included in the surface; for further details, see Section 3.4. This set of information is produced through geometric considerations and the measurement of the environment’s location through the motion capture system. The robot’s goal is to first reach the surface and then slide along it until the desired position is reached. Note that we are not interested in a precise regulation of the contact forces. Instead, the constraint imposed by the environment is purposefully exploited in combination with the decoupled compliance imposed by the control, to naturally generate the interaction forces and guide the end effector towards the desired position.

As described in Algorithm 1, the experiment is divided into two phases. In the first phase, the end effector of the soft robot is attracted toward a point within the environment, which is defined manually. After contact is established, it triggers the execution of the second phase. The end effector is now pulled towards a new target while staying in contact with the environment.

We repeat the experiment for five different locations of the environment. For all the experiments $δ_{ct} = 0.05$ m, and $K_{c}$ and $D_{c}$ are as given in (76). We collect in Figure 24 the photo sequences of the robot successfully performing the task of approaching and tracing along the environment. Figure 23 presents the evolutions over time of the end-effector position x, bending angle q, and commanded torques $τ$ , for the sequence shown in Figure 24(a)–(e). Here, the initial target is $x_{0} = [0.3637, 0.1406]^{T}$ m and the final target is $x_{d} = [0.2307, 0.2576]^{T}$ m. The first contact with the environment is detected around $1.1$ s after the beginning of the experiment, happening just after Figure 24(b). This is evident in the change of reference position in Figure 23(a). The tracing happens between $t = 1.1$ s and $t = 3.8$ s.

6. Conclusion

In this article, we have presented two new algorithms that achieve dynamic control of a soft robotic arm and enable interactions between the soft robot and an environment. Both algorithms leverage on the idea of connecting the soft robot to an equivalent augmented rigid robot in such a way that the matching is exact under the common hypothesis of CC, and under the introduced hypothesis on the distribution of mass. Although we consider our modeling approach advantageous for the use in model-based controllers, this model is not the first using lumped parameters to simplify the dynamics of a robot. In addition to the works already discussed in the introduction, we would like to point the interested reader to Giri and Walker (2011), Zheng et al. (2013), Kang et al. (2012), and Giri (2011). This work extends the conference paper (Della Santina et al., 2018) by providing an extended analysis of the augmented formulation mapping a rigid robot to a PCC robot, revised control algorithms together with an in-depth theoretical analysis, simulations that extensively test the performance of the controllers outside the modeling hypotheses, and new experiments on a longer soft robotic arm with larger actuation space. Future work will be devoted to comparing the experimental performance of the proposed algorithms with more standard techniques, as for example PID with kinematic inversion. The control algorithms presented in this article have been evaluated in the context of exploring a two-dimensional surface using a soft planar robotic arm. However, the potential for this work is much broader. The control algorithm is general and has the potential to enable a wide range of dynamic tasks, ranging from exploring three-dimensional spaces through contact, learning the geometry of the world, picking up delicate objects, moving heavy objects, and enabling dynamic interactions with the world. The main limitation preventing a straightforward translation of our algorithms to the three-dimensional case is the well-known kinematic singularity afflicting PCC kinematics in the straight configuration (Jones and Walker, 2007; Webster III and Jones, 2010). We are currently carrying out work expanding on the PCC parametrization for the three-dimensional case by considering alternative kinematic solutions including Godage et al. (2016), Rone and Ben-Tzvi (2014), and Godage et al. (2015a). We preliminary present these results in Katzschmann et al. (2019) and Della Santina et al. (2019).

Our work should be interpreted as a first step towards equipping soft robots with higher-level performances, rather than the definitive solution. This work aims at following the methodological path that emerged as a best practice in control theory, including rigid-bodied robot control. This is to isolate the fundamental properties of the system from ancillary characteristics of the specific systems, tackle the first, and then start considering specific ways of taking into account the latter, one by one. The properties falling in the first category are the inner nonlinearities produced by the mathematical structure of the problem, i.e., by the multi-body dynamics, and its interaction with the robot’s impedance. The practice of control design proved that attacking the problem in this way allows solid theories to be built, which at the same time work well in practice. We will devote a large part of our future work to tackling these robot-specific characteristics, proposing ways of integrating them in the general controllers that we propose in this article We will also investigate the possibility of further increasing the control performances by including actuator dynamics in the model of the robot.

Footnotes

Acknowledgements

Cosimo Della Santina and Robert K Katzschmann contributed equally to this work. Cosimo Della Santina would like to thank Prof. Alessandro De Luca for the very insightful suggestions on how to improve the theoretical soundness of this work.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the National Science Foundation (grant numbers NSF 1830901 and NSF 1226883).

Notes

ORCID iDs

Cosimo Della Santina

Robert K Katzschmann

References

Andersson

(1989) Aggressive trajectory generator for a robot ping-pong player. IEEE Control Systems Magazine 9(2): 15–21.

Angelini

Della Santina

Garabini

, et al. (2018) Decentralized trajectory tracking control for soft robots interacting with the environment. IEEE Transactions on Robotics 34(4): 924–935.

Armanini

Dal Corso

Misseroni

Bigoni

(2017) From the elastica compass to the elastica catapult: An essay on the mechanics of soft robot arm. Proceedings of the Royal Society A 473: 20160870.

Bajo

Goldman

Simaan

(2011) Configuration and joint feedback for enhanced performance of multi-segment continuum robots. In: 2011 IEEE International Conference on Robotics and Automation (ICRA). IEEE, pp. 2905–2912.

Bergou

Wardetzky

Robinson

Audoly

Grinspun

(2008) Discrete elastic rods. ACM Transactions on Graphics 27(3): 63.

Bosi

Misseroni

Dal Corso

Neukirch

Bigoni

(2016) Asymptotic self-restabilization of a continuous elastic structure. Physical Review E 94(6): 063005.

Bullo

Lewis

(2004) Geometric Control of Mechanical Systems: Modeling, Analysis, and Design for Simple Mechanical Control Systems (Texts in Applied Mathematics, Vol. 49). Springer Science & Business Media.

Chang

Khatib

(1995) Manipulator control at kinematic singularities: A dynamically consistent strategy. In: Proceedings 1995 IEEE/RSJ International Conference on Intelligent Robots and Systems: “Human Robot Interaction and Cooperative Robots”, Vol. 3. IEEE, pp. 84–88.

Chenevier

González

Aguado

Chinesta

Cueto

(2018) Reduced-order modeling of soft robots. PLoS ONE 13(2): e0192052.

10.

Corke

(1996) A robotics toolbox for matlab. IEEE Robotics and Automation Magazine 3(1): 24–32.

11.

Curtain

Zwart

(2012) An Introduction to Infinite-dimensional Linear Systems Theory ( Texts in Applied Mathematics, Vol. 21). Springer Science & Business Media.

12.

De Luca

Lucibello

(1998) A general algorithm for dynamic feedback linearization of robots with elastic joints. In: Proceedings 1998 IEEE International Conference on Robotics and Automation, Vol. 1. IEEE, pp. 504–510.

13.

de Payrebrune

O’Reilly

(2016) On constitutive relations for a rod-based model of a pneu-net bending actuator. Extreme Mechanics Letters 8: 38–46.

14.

Della Santina

Bianchi

Grioli

, et al. (2017) Controlling soft robots: Balancing feedback and feedforward elements. IEEE Robotics and Automation Magazine 24(3): 75–83.

15.

Della Santina

Bicchi

Rus

(2019) Dynamic control of soft robots with internal constraints in the presence of obstacles. In: 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE.

16.

Della Santina

Katzschmann

Bicchi

Rus

(2018) Dynamic control of soft robots interacting with the environment. In: Proceedings of the 1st International Conference on Soft Robotics.

17.

Deutschmann

Dietrich

Ott

(2017a) Position control of an underactuated continuum mechanism using a reduced nonlinear model. In: 2017 IEEE 56th Annual Conference on Decision and Control (CDC). IEEE, pp. 5223–5230.

18.

Deutschmann

Ott

Monje

Balaguer

(2017b) Robust motion control of a soft robotic system using fractional order control. In: International Conference on Robotics in Alpe-Adria Danube Region. Berlin: Springer, pp. 147–155.

19.

Falkenhahn

Hildebrandt

Neumann

Sawodny

(2015) Model-based feedforward position control of constant curvature continuum robots using feedback linearization. In: 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, pp. 762–767.

20.

Featherstone

(2014) Rigid body dynamics algorithms. Berlin: Springer.

21.

Gazzola

Dudte

McCormick

Mahadevan

(2016) Dynamics of soft filaments that can stretch, shear, bend and twist. arXiv preprint arXiv:1607.00430.

22.

George Thuruthel

Ansari

Falotico

Laschi

(2018) Control strategies for soft robotic manipulators: A survey. Soft Robotics 5(2): 0007.

23.

Giri

(2011) A new approach to dynamic modeling of continuum robots. Thesis 1227, Clemson University. Available at: https://tigerprints.clemson.edu/all_theses/1227

24.

Giri

Walker

(2011) Three module lumped element model of a continuum arm section. In: 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, pp. 4060–4065.

25.

Godage

Medrano-Cerda

Branson

Guglielmino

Caldwell

(2015a) Modal kinematics for multisection continuum arms. Bioinspiration and Biomimetics 10(3): 035002.

26.

Godage

Medrano-Cerda

Branson

Guglielmino

Caldwell

(2016) Dynamics for variable length multisection continuum arms. The International Journal of Robotics Research 35(6): 695–722.

27.

Godage

Wirz

Walker

Webster

III (2015b) Accurate and efficient dynamics for variable-length continuum arms: A center of gravity approach. Soft Robotics 2(3): 96–106.

28.

Gravagne

Rahn

Walker

(2003) Large deflection dynamics and control for planar continuum robots. IEEE/ASME transactions on mechatronics 8(2): 299–307.

29.

Grazioso

Sonneville

Di Gironimo

Bauchau

Siciliano

(2016) A nonlinear finite element formalism for modelling flexible and soft manipulators. In: IEEE International Conference on Simulation, Modeling, and Programming for Autonomous Robots (SIMPAR). IEEE, pp. 185–190.

30.

Greco

Cuomo

(2013) B-spline interpolation of Kirchhoff–Love space rods. Computer Methods in Applied Mechanics and Engineering 256: 251–269.

31.

Haddadin

De Luca

Albu-Schaffer

(2017) Robot collisions: A survey on detection, isolation, and identification. IEEE Transactions on Robotics 33(6): 1292–1312.

32.

Hannan

Walker

(2003) Kinematics and the implementation of an elephant’s trunk manipulator and other continuum style robots. Journal of Field Robotics 20(2): 45–63.

33.

Hespanha

Naghshtabrizi

(2007) A survey of recent results in networked control systems. Proceedings of the IEEE 95(1): 138–162.

34.

Hisch

Giusti

Althoff

(2017) Robust control of continuum robots using interval arithmetic. IFAC-PapersOnLine 50(1): 5660–5665.

35.

Holland

Abah

Velasco-Enriquez

, et al. (2017) The soft robotics toolkit: Strategies for overcoming obstacles to the wide dissemination of soft-robotic hardware. IEEE Robotics and Automation Magazine 24(1): 57–64.

36.

Homberg

Katzschmann

Dogar

Rus

(2019) Robust proprioceptive grasping with a soft robot hand. Autonomous Robots 43: 681–696.

37.

Hong

Kim

Park

(2017) Comparative analysis of energy-based criteria for dynamics-based robot motion optimization. In: 2017 IEEE Conference on Control Technology and Applications (CCTA). IEEE, pp. 175–180.

38.

Jones

Walker

(2006) Kinematics for multisection continuum robots. IEEE Transactions on Robotics 22(1): 43–55.

39.

Jones

Walker

(2007) Limiting-case analysis of continuum trunk kinematics. In: 2007 IEEE International Conference on Robotics and Automation. IEEE, pp. 1363–1368.

40.

Kang

Branson

Guglielmino

Caldwell

(2012) Dynamic modeling and control of an octopus inspired multiple continuum arm robot. Computers and Mathematics with Applications 64(5): 1004–1016.

41.

Kapadia

Walker

(2011) Task-space control of extensible continuum manipulators. In: 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, pp. 1087–1092.

42.

Kapadia

Fry

Walker

(2014) Empirical investigation of closed-loop control of extensible continuum manipulators. In: 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2014). IEEE, pp. 329–335.

43.

Kapadia

Walker

Dawson

Tatlicioglu

(2010) A model-based sliding mode controller for extensible continuum robots. In: Proceedings of the 9th WSEAS International Conference on Signal Processing, Robotics and Automation. World Scientific and Engineering Academy and Society (WSEAS), pp. 113–120.

44.

Katzschmann

Della Santina

Toshimitsu

Bicchi

Rus

(2019) Dynamic motion control of multi-segment soft robots using piecewise constant curvature matched with an augmented rigid body model. In: 2019 2nd IEEE International Conference on Soft Robotics (RoboSoft). IEEE, pp. 454–461.

45.

Katzschmann

DelPreto

MacCurdy

Rus

(2018) Exploration of underwater life with an acoustically controlled soft robotic fish. Science Robotics 3(16): eaar3449

46.

Katzschmann

Marchese

Rus

(2015) Autonomous object manipulation using a soft planar grasping manipulator. Soft Robotics 2(4): 155–164.

47.

Kelly

(1995) A tuning procedure for stable pid control of robot manipulators. Robotica 13(2): 141–148.

48.

Khalil

(1996) Nonlinear Systems. Englewood Cliffs, NJ: Prentice-Hall

49.

Khatib

(1987) A unified approach for motion and force control of robot manipulators: The operational space formulation. IEEE Journal on Robotics and Automation 3(1): 43–53.

50.

Kuindersma

Deits

Fallon

, et al. (2016) Optimization-based locomotion planning, estimation, and control design for the Atlas humanoid robot. Autonomous Robots 40(3): 429–455.

51.

Laschi

Cianchetti

Mazzolai

Margheri

Follador

Dario

(2012) Soft robot arm inspired by the octopus. Advanced Robotics 26(7): 709–727.

52.

Laschi

Mazzolai

Cianchetti

(2016) Soft robotics: Technologies and systems pushing the boundaries of robot abilities. Science Robotics 1(1): eaah3690.

53.

Leidner

Dietrich

Schmidt

Borst

Albu-Schäffer

(2014) Object-centered hybrid reasoning for whole-body mobile manipulation. In: 2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, pp. 1828–1835.

54.

Lismonde

Sonneville

Brüls

(2017) Trajectory planning of soft link robots with improved intrinsic safety. IFAC-PapersOnLine 50(1): 6016–6021.

55.

Ljung

(1998) System identification. In: Signal Analysis and Prediction. Berlin: Springer, pp. 163–173.

56.

Lohmiller

Slotine

JJE

(1998) On contraction analysis for non-linear systems. Automatica 34(6): 683–696.

57.

Luo

Guo

Morgül

(2012) Stability and Stabilization of Infinite Dimensional Systems With Applications. Springer Science & Business Media.

58.

Mahl

Hildebrandt

Sawodny

(2014) A variable curvature continuum kinematics for kinematic control of the bionic handling assistant. IEEE Transactions on Robotics 30(4): 935–949.

59.

Marchese

Katzschmann

Rus

(2014) Whole arm planning for a soft and highly compliant 2D robotic manipulator. In: IEEE International Conference on Intelligent Robots and Systems, pp. 554–560.

60.

Marchese

Rus

(2016) Design, kinematics, and control of a soft spatial fluidic elastomer manipulator. The International Journal of Robotics Research 35(7): 840–869.

61.

Marchese

Tedrake

Rus

(2016) Dynamics and trajectory optimization for a soft spatial fluidic elastomer manipulator. The International Journal of Robotics Research 35(8): 1000–1019.

62.

Murray

(1994) A Mathematical Introduction to Robotic Manipulation. Boca Raton, FL: CRC Press.

63.

Nakanishi

Cory

Mistry

Peters

Schaal

(2008) Operational space control: A theoretical and empirical comparison. The International Journal of Robotics Research 27(6): 737–757.

64.

Ott

(2008) Cartesian Impedance Control of Redundant and Flexible-joint Robots. Berlin: Springer.

65.

Penning

Zinn

(2014) A combined modal-joint space control approach for continuum manipulators. Advanced Robotics 28(16): 1091–1108.

66.

Petersen

Pedersen

(2008) The Matrix Cookbook. Technical University of Denmark

67.

Pfeifer

Lungarella

Iida

(2012) The challenges ahead for bio-inspired ‘soft’ robotics. Communications of the ACM 55(11): 76–87.

68.

Polygerinos

Correll

Morin

, et al. (2017) Soft robotics: Review of fluid-driven intrinsically soft devices; manufacturing, sensing, control, and applications in human–robot interaction. Advanced Engineering Materials 19(12): 1700016.

69.

Polygerinos

Wang

Overvelde

, et al. (2015) Modeling of soft fiber-reinforced bending actuators. IEEE Transactions on Robotics 31(3): 778–789.

70.

Renda

Boyer

Dias

Seneviratne

(2017) Discrete Cosserat approach for multi-section soft robots dynamics. arXiv preprint arXiv:1702.03660.

71.

Roberts

Azizi

(2011) Flexible mechanisms: The diverse roles of biological springs in vertebrate movement. Journal of Experimental Biology 214(3): 353–361.

72.

Rone

Ben-Tzvi

(2014) Continuum robot dynamics utilizing the principle of virtual power. IEEE Transactions on Robotics 30(1): 275–287.

73.

Rubin

(2013) Cosserat Theories: Shells, Rods and Points ( Solid Mechanics and Its Applications, Vol. 79). Springer Science & Business Media.

74.

Rus

Tolley

(2015) Design, fabrication and control of soft robots. Nature 521(7553): 467–475.

75.

Sadati

Naghibi

Walker

Althoefer

Nanayakkara

(2018) Control space reduction and real-time accurate modeling of continuum manipulators using Ritz and Ritz–Galerkin methods. IEEE Robotics and Automation Letters 3(1): 328–335.

76.

Sciavicco

Siciliano

(2012) Modelling and Control of Robot Manipulators. Springer Science & Business Media.

77.

Seok

Onal

Cho

Wood

Rus

Kim

(2013) Meshworm: A peristaltic soft robot with antagonistic nickel titanium coil actuators. IEEE/ASME Transactions on Mechatronics 18(5): 1485–1497.

78.

Skogestad

(2003) Simple analytic rules for model reduction and PID controller tuning. Journal of Process Control 13(4): 291–309.

79.

Skorina

Luo

Ozel

Chen

Tao

Onal

(2015) Feedforward augmented sliding mode motion control of antagonistic soft pneumatic actuators. In: 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, pp. 2544–2549.

80.

Skorina

Tao

Chen

Luo

Onal

(2016) Motion control of a soft-actuated modular manipulator. In: 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, pp. 4997–5002.

81.

Slotine

JJE

(1987) On the adaptive control of robot manipulators. The International Journal of Robotics Research 6(3): 49–59.

82.

Slotine

JJE

(1991) Applied Nonlinear Control. Englewood Cliffs, NJ: Prentice-Hall.

83.

Thieffry

Kruszewski

Goury

Guerra

Duriez

(2017) Dynamic control of soft robots. In: IFAC World Congress.

84.

Thuruthel

Falotico

Manti

Laschi

(2018) Stable open loop control of soft robotic manipulators. IEEE Robotics and Automation Letters 3(2): 1292–1298.

85.

Till

Bryson

Chung

Orekhov

Rucker

(2015) Efficient computation of multiple coupled Cosserat rod models for real-time simulation and control of parallel continuum manipulators. In: 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, pp. 5067–5074.

86.

Wang

Yang

Liu

Chen

Liang

Pfeifer

(2017) Visual servoing of soft robot manipulator in constrained environments with an adaptive controller. IEEE/ASME Transactions on Mechatronics 22(1): 41–50.

87.

Webster

III Jones

(2010) Design and kinematic modeling of constant curvature continuum robots: A review. The International Journal of Robotics Research 29(13): 1661–1683.

88.

Zhang

Dequidt

Kruszewski

Largilliere

Duriez

(2016) Kinematic modeling and observer based control of soft robot using real-time finite element method. In: 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, pp. 5509–5514.

89.

Zheng

Branson

Guglielmino

, et al. (2013) Model validation of an octopus inspired continuum robotic arm for use in underwater environments. Journal of Mechanisms and Robotics 5(2): 021004.