Optimal Contact Points for an Octopus Arm

Abstract

We investigate the optimality of the configurations of a tentacle-like soft manipulator ensuring a planar force-closure condition. In particular, the optimization is performed with respect to both to the control strategies and to the set of contact points ensuring a planar, frictionless, first-order force-closure of the target. The case of an elliptic target object is discussed in detail, and numerical simulation are presented.

Introduction

In this paper, we investigate the optimality of the configurations of a tentacle-like soft manipulator ensuring a planar force closure condition on an elliptic target. The novelty consists in considering the set of contact points as an unknown of the optimization problem. Indeed, the contact points on the target object are selected to yield force-closure configurations, while minimizing an integral cost on the curvature of the manipulator. We adopt an optimal control theoretic approach: optimal contact points and optimal configuration satisfy stationarity conditions for a constrained minimization problem. In particular, the objective cost functional accounts for an integral cost on the curvature, an non-interpenetration (penalized) constraint, and the grasping task. The minimization is performed subject to an equilibrium condition for the manipulator, and to force-closure conditions for the target contact points.

In [1], we considered prescribed contact subregions on general objects. Here, we consider planar, stationary, frictionless grasps of elliptic targets—with positive eccentricity. This choice is motivated by the fact that we see the case of an ellipse as a projection of the three-dimensional problem of grasping cylindrical and ellipsoidal objects.

We borrow from [2] the study of a multi-target optimal control problem: the main difference is that in [2], the target points were fixed a priori, whereas in the present work, the target points are provided as part of the optimization problem.

The control model underlying our study was introduced in [3]. It is a control model for a planar string, modeling the symmetry axis of an octopus arm. Its dynamics encompasses an exact inextensibility constraint, bending moment, curvature constraints and distributed curvature controls. In this framework, several optimization problems were addressed. They include optimal reachability [3, 4], obstacle avoidance [1, 5], and grasping problems [2, 6]. In particular, [1] addresses the problem of touching a prescribed portion of the boundary of a target object while avoiding interpenetration and minimizing a quadratic cost on the controls. In [2], the contact sub-region of the manipulator is treated as an unknown of the problem and, at the same time, the target is allowed to be disconnected.

Here, we look for optimal configurations ensuring force-closure grasps. The force-closure property is a classical geometric condition on the contact points ensuring the grasp of an object to be stable with respect external disturbances: in particular, a grasp achieves force-closure if any external wrench can be balanced by wrenches at the contact points [7,8,9,10]. For planar objects, four contact points are necessary to guarantee a first-order force-closure grasp. Of course, the contact points ensuring a force-closure grasp are in general non-unique. Then, one may wonder what are the best contact points to target with the soft-manipulator, to optimize some cost associated to the resulting grasping configuration. Based on this observation, we aim to select the contact points, the related force-closure configurations, and, ultimately, the optimal controls, minimizing a given integral cost.

We proceed as follows. In the next section, we recall our model for a tentacle-like soft manipulator, the associated equilibria and a class of optimal stationary grasping problems. Following section is devoted to multi-target reachability problems. In the next section, we address the geometry of the problem. We provide some explicit force-closure conditions for elliptic targets and we present a class of ellipses as cross-sections of three-dimensional objects. In the next section, we present our optimization problem for the force-closure of an ellipse, we discuss the related optimality system, and we provide a projected gradient method for its numerical solution. Finally in the following section, we report the results of some experiments, confirming the effectiveness of the proposed method. In the last section, we draw our conclusions.

We conclude this introduction with the literature that mostly inspired our work. The papers [11,12,13,14] provide a general introduction on soft robotics and related motion planning problems. From a modeling point of view, we refer to [15,16,17,18,19]. A survey on grasping problems for soft manipulators can be found in [20]. Our work is based on an optimal control theoretic approach, constrained reachability problems in a similar fashion are addressed in [21, 22], where a finite number of degrees of freedom is taken into account. Here, we carry on a formal analysis in an infinite-dimensional setting, encompassing at once continuous manipulators as well as hyper-redundant, discrete manipulators [1]. The power of a rigorous derivation of the optimality system consists in its suitability for extensions to a purely dynamic setting, from which a time-varying, optimal control can be synthetized as in [5], where an optimal dynamic reachability problem is addressed. To the best of our knowledge, the present variational approach to optimal grasping problems for soft manipulators with non-uniform curvature constraints is new. We point out that the optimality system is then numerically solved via a descent gradient method. Some information useful for the parameter tuning can be recovered by the numerical analysis of the optimization process. The drawback of this first-order method consists in a possibly slow convergence to the stationary points that, in case of lack of convexity of the Lagrangian, can result in local minima. In this regard, also in view of the smoothness of the constraints, the search of optimal grasping controls could benefit from metaheuristic methods [23,24,25]. In particular, the application of a method like the particle swarm optimization (PSO) to the residual of the optimality system (rather than to the Lagrangian itself) could produce a speedup in the computation. We are not aware of data available in the literature for the specific problem under exam, and we postpone this investigation in a future work.

An Optimal Control Problem for Multi-touch Configurations

We introduce our model and we set the problem of optimizing the contact of the arm with a general, possibly disconnected target set, while avoiding an obstacle. The obstacle can represent either an external element in the workspace or an object to be grasped without interpenetration, in the latter case, it suffices to assume the target point set to belong to the boundary of the obstacle.

A Control Model for an Octopus Arm

Our model was introduced in [3]. It consists in a control model for an octopus arm, characterized by an axial symmetry, a non-uniform thickness and a fixed endpoint. We assumed an exact inextensibility constraint and a bending moment, representing the natural resistance of the body to deformations. Also, a constraint prevents the body to bend above a fixed threshold. Finally, the bending can be pointwise forced by the controller. By virtue of the axial symmetry, we specialized the problem to the evolution of the symmetry axis on the plane. In particular, the axis is modelled as an inextensible string. The mass and the bending constraints/controls of the whole manipulator are projected on the symmetry axis. They, respectively, result in a non-uniform mass and suitably weighted curvature constraints and controls, see [4] for details on this projection.

More precisely, the model includes: (i) a curvature constraint, forcing the curvature of the axis under a fixed (non-uniform) threshold $\omega$; (ii) a curvature control, forcing the signed curvature to the quantity $\omega u$, where $u\in [-1,1]$ is the control map; (iii) a bending moment, forcing the uncontrolled manipulator to arrange on a straight line. On the other hand, the inextensibility constraint is exact.

We consider the curve $q(s,t):[0,1]\times {{\mathbb {R}}}^+\rightarrow {{\mathbb {R}}}^2$ parametrizing the symmetry axis of the manipulator in arclength coordinates, and the associated inextensibility multiplier $\sigma (s,t)\in {{\mathbb {R}}}$, playing the role of the tension of the string. We denote by $q_s$, $q_{ss}$, $q_{tt}$ partial derivatives in space and time, respectively. The scalar quantity $\Vert q_{ss}\Vert$ is the curvature of q. The product $q_s \times q_{ss}:=q_s\cdot q_{ss}^\bot$ represents the signed curvature, where the symbol $q_{ss}^\bot$ denotes the counter-clockwise orthogonal vector to $q_{ss}$. We describe in detail the constraints and the control.

• 1. Inextensibility constraint: $\Vert q_s\Vert =1.$

• 2. Curvature constraint: $\Vert q_{ss}\Vert \le \omega$. The penalization elastic potential is $\nu (\Vert q_{ss}\Vert ^2-\omega ^2)^2_+$, where the function $\nu$ is a non-uniform elastic constant.

• 3. Curvature control: $q_s \times q_{ss}=\omega u$. The penalization elastic potential is $\mu \left( \omega u-q_s \times q_{ss}\right) ^2$, where the function $\mu$ is a non-uniform elastic constant.

Then, we consider the following Lagrangian:

$$\begin{aligned} {\mathcal {L}}(q,\sigma )&:= {\int _0^1}\Big (\underbrace{\dfrac{1}{2}\rho \Vert q_t\Vert ^2 }_\text {kinetic energy}-\underbrace{\dfrac{1}{2} \sigma (\Vert q_s\Vert ^2 - 1) }_\text {inextensibility constr.}\\&\quad -\underbrace{\dfrac{1}{4}\nu \left( \Vert q_{ss}\Vert ^2-\omega ^2\right) _+^2}_\text {curvature constr.}-\underbrace{\dfrac{1}{2}\varepsilon \Vert q_{ss}\Vert ^2}_\text {bending moment}\\&\quad -\underbrace{\dfrac{1}{2}\mu \left( \omega u-q_s\times q_{ss}\right) ^2 }_\text {curvature control} \Big )\mathrm{d}s. \end{aligned}$$

We refer to [3] for a rigorous justification of $\mathcal L$ and for the complete derivation, via the least action principle, of the equations of motion of q (and $\sigma$) from ${\mathcal {L}}$.

In this paper, we are interested in the optimization, with respect to the control u, of the equilibrium configuration of $\mathcal L$, namely the shape of the manipulator provided by the solution q of the stationary control system (refer to [3] for the proof)

$$\begin{aligned} \left\{ \begin{array}{ll} q_{ss}={\bar{\omega }}\, u\, q_s^\bot &{}\text{ in } (0,1)\\ \Vert q_s\Vert ^2=1 &{}\text{ in } (0,1)\\ q(0)=(0,0)\\ q_{s}(0)=(0,-1)\,, \end{array} \right. \end{aligned}$$

(1)

where ${\bar{\omega }}:=\mu \omega /(\mu +\varepsilon )\le \omega$ represents the effective curvature threshold resulting from the balancing between the bending moment and the curvature control in ${\mathcal {L}}$. Note that the curvature constraint is embedded in the first equation of (1), since $\mid u \mid \le 1$ and the inextensibility constraint $\Vert q_s\Vert =1$ imply $\Vert q_{ss}\Vert ={\bar{\omega }}\mid u\mid \le \omega$.

Optimality Conditions for a Multi-target, Obstacle Avoidance Reachability Problem

We recall from [2], an optimal control problem involving the simultaneous contact with a point set target, an obstacle avoidance task and the minimization of a quadratic cost in the control. Let $\Omega _0$ be an open subset of ${{\mathbb {R}}}^2$ representing the obstacle, and let $\Omega _1:=\{ p_1,\ldots , p_N\}$, be a set of N fixed points, possibly located on $\partial \Omega _0$. Note that the condition $\Omega _1\subset \partial \Omega _0$ is assumed when $\Omega _0$ is a target object that needs to be grasped without interpenetration.

We also consider N unknowns $S=\{ s_1,\ldots , s_N\}\subset I_\gamma :=[\gamma ,1-\gamma ]$. The set S belongs to the interior of the parametrization interval of the manipulator: in other words, $q(s_i)$ is a point of the manipulator, for all $i=1,\ldots ,N$. Since $\gamma >0$, then $q(s_k)$ is not any of the two endpoints of the manipulator. The set $q(S)\subset {{\mathbb {R}}}^2$ represents the set of points of the manipulator touching the N target points. Then S, and consequently q(S), is an unknown, since the contact subregion of the manipulator with the target is not prescribed a priori (as, instead, for [1, 6]). We let the manipulator adapt to choose the best contact points as a result of the following optimization process:

$$\begin{aligned} \begin{aligned}&\min \, \frac{1}{2}\int _0^1 u^2(s) \mathrm{d}s +\frac{1}{2\tau _0}\int _0^1 {\mathcal {O}}(q(s))\mathrm{d}s\\&\quad +\frac{1}{2\tau _1}\sum _{i=1}^N \Vert q(s_i)-p_i\Vert ^2,\\&\text {subject}\,\,\text {to} (1), \mid u(s)\mid \le 1\,\, \text {for}\,\, s\in (0,1), s_i\in I_\gamma , i=1\ldots ,N. \end{aligned}, \end{aligned}$$

(2)

The first integral term represents a quadratic cost on the controls. The second integral term encompasses the obstacle avoidance task: the function ${\mathcal {O}}:{{\mathbb {R}}}^2\rightarrow {{\mathbb {R}}}$ is a non-negative function with support $\Omega _0$, acting as repulsive potential. The effect of this integral term is then precisely to push the points of the manipulator outside $\Omega _0$, according to a penalization parameter $\tau _0>0$. Finally, the third term penalizes, according to $\tau _1>0$, the distance of the (unknown) point of the manipulator $q(s_i)$ from the target point $p_i$, for all $i=1,\ldots ,N$.

To address (2), we recall that (1) implies the identity $u^2(s)=\Vert q_{ss}(s)\Vert ^2/{\bar{\omega }}^2(s)$ and the equivalence $\mid u(s)\mid \le 1$ if and only if $\Vert q_{ss}(s)\Vert \le {\bar{\omega }}(s)$ for a.e. $s\in (0,1)$. Therefore, the optimization problem is equivalent to the following:

$$\begin{aligned} \begin{aligned}&\min \, \frac{1}{2}\int _0^1 \frac{\Vert q_{ss}\Vert ^2}{{\bar{\omega }}^2(s)} ds +\frac{1}{2\tau _0}\int _0^1 \mathcal O(q(s))ds\\&\qquad +\frac{1}{2\tau _1}\sum _{i=1}^N \Vert q(s_i)-p_i\Vert ^2,\,\\&\text {subject to}\\&q(0)=(0,0), q_{s}(0)=(0,-1), \Vert q_s(s)\Vert ^2\\&\quad =1, \Vert q_{ss}(s)\Vert \le \bar{\omega }(s)\,\, \text {for}\,\, s\in (0,1)\\&\text {and}\,\, s_i\in I_\gamma \,\, \text {for}\,\, i=1,\ldots ,N, \end{aligned} \end{aligned}$$

(3)

in which the explicit dependence on the controls is dropped by virtue of the constraint (1).

Force-Closure Conditions for an Ellipse

We consider first-order force-closure conditions for frictionless contact points, in the case of elliptic boundary of the target object—see the end of the section for some motivating examples. Force-closure conditions are geometric conditions on the disposition of contact points on the boundary of the object, to ensure the immobility of the object despite external disturbances. In other words, the contact points are placed so that the associated (normal) contact forces are able generate an arbitrary wrench to counteract a disturbance wrench. We consider frictionless contact forces.

More precisely, we denote by $p=(p_1,p_2)\in \partial \Omega _0$ a generic contact point. We set the reference frame as the center of $\Omega _0$, which is assumed to be an ellipse with semiaxes $0<b\le a$, described by the equation $E(x):=E(x_1,x_2)=\left( \frac{x_1}{a}\right) ^2+\left( \frac{x_2}{b}\right) ^2-1=0$. We represent p in polar coordinates, so that for some $\theta \in [0,2\pi )$:

$$\begin{aligned} p=p(\theta )= \left( a \cos (\theta ),b\sin (\theta )\right) . \end{aligned}$$

Denoting by $\mathbf{n}( p)\in {{\mathbb {R}}}^2$ the inward normal vector to the ellipse at p,

$$\begin{aligned} \mathbf{n}( p):=-\nabla E( p)=-2\left( \frac{\cos (\theta )}{a},\frac{\sin (\theta )}{b}\right) ^T, \end{aligned}$$

(T denotes the transpose) we consider frictionless contact forces of the form $\mathbf{f}( p)=f\, \mathbf{n}( p)$ with $f\ge 0$. Then, the wrench associated to p is the vector $\mathbf{w}( p)\in {{\mathbb {R}}}^3$ defined by

$$\begin{aligned} \mathbf{w}( p):=\begin{pmatrix} \mathbf{f}( p)\\ \mathbf{p}\times \mathbf{f}( p) \end{pmatrix} = -\frac{2f}{ab} \begin{pmatrix}b\cos (\theta )\\ a\sin (\theta )\\ (a^2-b^2)\cos (\theta )\sin (\theta ) \end{pmatrix}. \end{aligned}$$

Following [10], given N contact points $\{ p_1,\ldots , p_N\}\in \partial \Omega _0$, we say that $\Omega _0$ is in (first-order) force-closure if the set of wrenches $\{\mathbf{w}(p_1),\ldots ,\mathbf{w}(p_N)\}$ positively spans ${{\mathbb {R}}}^3$, i.e., for all $\mathbf{x}\in {{\mathbb {R}}}^3$, there exist $\alpha _1,\ldots ,\alpha _N\ge 0$ such that

$$\begin{aligned} \mathbf{x}=\sum _{i=1}^N \alpha _i \mathbf{w}( p_i). \end{aligned}$$

Denoting by ${\mathcal {W}}^{FC}$ the $3\times N$ matrix whose columns are the wrenches, the force-closure condition can be equivalently stated as follows:

$$\begin{aligned} \text {rank } {\mathcal {W}}^{FC}=3, \end{aligned}$$

(4)

$$\begin{aligned} {\mathcal {W}}^{FC} {\mathbf {y}}=0\text { for}\,\, \text {some}\,\, {\mathbf {y}}\in {{\mathbb {R}}}^N, y_i>0, i=1,\ldots ,N. \end{aligned}$$

(5)

Note that the full-rank condition (4) cannot be satisfied if $a=b$, namely when $\Omega _0$ is a circle. Moreover, the number N of contact points must be equal to or greater than 4, which is the minimal number of generators of a three dimensional conic hull.

Ellipses as Cross-sections of Three-Dimensional Objects

In this section, we embed the reference system of the manipulator in ${{\mathbb {R}}}^3$, so that the workspace of the manipulator belongs the plane $z=0$ and the x and y axis are oriented according to (1). We consider two classes of three-dimensional objects: a family of cylinders and a family of ellipsoids. For each member of each class, we derive an elliptic target, as the intersection between the three-dimensional object and the workspace of the manipulator. Such elliptic intersection is the region of the object that can be actually reached by the manipulator.

We begin by modelling a collection of cylindrical objects. When a cylinder is intersected with the workspace, we obtain an elliptic target object. More precisely, we parametrize a collection of cylinders of radius $r>0$ whose symmetry axes have $(\mathbf{c},0)=(c_1,c_2,0)$, as common point. We assume the symmetry axis to be parallel to the yz-plane and we describe its orientation via the parameter $\varphi \in (-\pi /2,\pi /2)$—the case $\varphi =0$ corresponds to a vertical cylinder, see Fig. 1.

Fig. 1

The workspace plane, the cylinders $\mathbf{C}_{1,\varphi }$, and, in light blue, the ellipses $\Omega _{1,\varphi }$ with $\varphi$ varying between 0 and $\pi /4$ with step $\pi /16$

More precisely, let

$$\begin{aligned} \begin{aligned} \mathbf{C}_{r,0}:=&\{(c_1+r \cos (\theta ),c_2+r\sin (\theta ),z\cos (\phi )),\,\\&\theta \in [0,2\pi ],\,z\in {{\mathbb {R}}}\}. \end{aligned} \end{aligned}$$

For $\varphi \in (-\pi /2,\pi /2)$ define

$$\begin{aligned} \begin{aligned} \mathbf{C}_{r,\varphi }&:=(\mathbf{c},0)^T+R_x(\varphi )(\mathbf{C}_{r,\varphi }-(\mathbf{c},0)^T)\\&\quad =\{(c_1+r \cos (\theta ),c_2+r\sin (\theta )-z\sin (\phi ),r\sin (\theta )\sin (\phi )\\&\qquad +z\cos (\phi )),\,\, \theta \in [0,2\pi ],\,z\in {{\mathbb {R}}}\}, \end{aligned} \end{aligned}$$

where $R_x(\varphi )$ is the rotation matrix about the x-axis with angle of rotation $\varphi$.

The associated target ellipse is hence given by the intersection $\Omega _{r,\varphi }:=\mathbf{C}_{r,\varphi }\cap \{(x_1,x_2,0),\,x_1,x_2\in {{\mathbb {R}}}\}$. By a direct computation,

$$\begin{aligned} \Omega _{r,\varphi }:= & {} \left\{ (x_1,x_2)\in {{\mathbb {R}}}^2\mid \left( \frac{x_1-c_1}{r}\right) ^2\right. \\&+\left. \left( \frac{x_2-c_2}{r(\cos \varphi +\tan \varphi )}\right) ^2=1\right\} \end{aligned}$$

that is, $\Omega _{r,\varphi }$ is the ellipse centered in $\mathbf{c}$ with semiaxes $a=a_{r}:=r$ and $b=b_{r,\varphi }:=r(\cos \varphi +\tan \varphi )$. Note that if $\varphi =0$, then $\Omega _{r,\varphi }$ is a circle.

Consider now a family of ellipsoids, with center $(\mathbf{c},0)$ and with semiaxes $a_1,a_2,a_3>0$. For brevity, let us collect $a_1,a_2$ and $a_3$ in the three-dimensional vector $\mathbf{a}:=(a_1,a_2,a_3)$. Given the ellipsoid $\mathbf{E}_{\mathbf{a},0}$ implicitly defined by

$$\begin{aligned} \left( \frac{x_1-c_1}{a_1}\right) ^2+ \left( \frac{x_2-c_2}{a_2}\right) ^2+ \left( \frac{x_3}{a_3}\right) ^2= 1 \end{aligned}$$

we consider the family of rotations of $\mathbf{E}_{a,0}$ about the symmetry axes that is parallel to the x-axis, see Fig. 2. We have

$$\begin{aligned} \mathbf{E}_{a,\varphi }{:}{=} (\mathbf{c},0^T)+R_x(\varphi )(\mathbf{E}_{\mathbf{a},\varphi }-(\mathbf{c},0)^T). \end{aligned}$$

We recall that we defined $R_x(\varphi )$ as the rotation matrix about the x-axis. More precisely, for $\varphi \in [-\pi ,\pi ]$, we have the parametrization in polar coordinates

Fig. 2

The workspace plane, the ellipsoids $\mathbf{E}_{\mathbf{a},\varphi }$ with $\mathbf{a}=(1,2,3)$, and, in light blue, the ellipses $\Omega _{\mathbf{a},\varphi }$ with $\varphi$ varying between 0 and $\pi /2$ with step $\pi /8$

$$\begin{aligned} \begin{aligned} \mathbf{E}_{\mathbf{a},\varphi }&:=\{ (c_1+a_1\sin \psi \cos \theta ,c_2\\&\quad +a_2\sin \psi \sin \theta \cos \varphi +a_3\cos \psi \sin \varphi ,\\&\quad -a_2\sin \psi \sin \theta \sin \varphi +a_3\cos \psi \cos \varphi )\\&\quad \mid \theta \in [0,2\pi ),\,\psi \in [0,\pi ) \}. \end{aligned} \end{aligned}$$

The intersection with the $z=0$ plane yields the ellipse

$$\begin{aligned} \Omega _{\mathbf{a},\varphi }:= & {} \left\{ (x_1,x_2)\in {{\mathbb {R}}}^2\mid \left( \frac{x_1-c_1}{a_1}\right) ^2\right. \\&\quad +\left. \left( \frac{x_2-c_2}{ a_2a_3\frac{1}{\sqrt{a_2^2\sin ^2\varphi +a_3^2\cos ^2\varphi }}}\right) ^2=1\right\} \end{aligned}$$

that is, $\Omega _{\mathbf{a},\varphi }$ is the ellipse centered in $\mathbf{c}$ with semiaxes $a=a_{1}$ and $b=b_{a_2,a_3,\varphi }:=a_2a_3\frac{1}{\sqrt{a_2^2\sin ^2\varphi +a_3^2\cos ^2\varphi }}$.

Optimal Grasping Strategies

In this section, we show how to suitably embed the force-closure condition (4)–(5) in the multi-target optimization problem with obstacle avoidance (3) presented in “Optimality conditions for a multi-target, obstacle avoidance reachability problem”. Moreover, we discuss the related optimality system and we provide a projected gradient method for its numerical solution.

We consider again the case of an elliptic obstacle $\Omega _0$ with semi-axes $0<b<a$, but possibly rotated by a given angle $\theta _0\in [0,2\pi )$ and translated to the point $c_0\in {{\mathbb {R}}}^2$. Then, we settle on the minimal requirement of $N=4$ target points, looking for a point set $\Omega _1:=\{ p_1,\ldots , p_4\}\subset \partial \Omega _0$ of the form

$$\begin{aligned} p_i= & {} p(\theta _i)=c_0+R_0(a\cos (\theta _i),b\sin (\theta _i))^T,\nonumber \\&\theta _i\in [0,2\pi ),\quad i=1,\ldots ,4, \end{aligned}$$

with $R_0=\left( \begin{array}{cc} \cos (\theta _0)&{} \sin (\theta _0) \\ -\sin (\theta _0) &{} \cos (\theta _0) \end{array}\right)$, satisfying the force-closure condition.

To this end, we consider the wrenches of $\Omega _1$, expressed in local coordinates as in “Force-closure conditions for an ellipse” and scaled by the constant factor $\frac{2f}{ab}$, i.e.,

$$\begin{aligned} \mathbf{w}_i:= & {} \mathbf{w}(p_i)=-(b\cos (\theta _i),a\sin (\theta _i),(a^2-b^2)\cos (\theta _i)\sin (\theta _i)),\\ i= & {} 1,\ldots ,4, \end{aligned}$$

and we denote by ${\mathcal {W}}$ the $3\times 3$ matrix whose columns are the components of the first three wrenches $\mathbf{w}_1, \mathbf{w}_2, \mathbf{w}_3$. Moreover, we consider its formal inverse ${\mathcal {W}}^{-1}$, obtaining, by direct computation

$$\begin{aligned} {\mathcal {W}}^{-1}=\frac{1}{\det {\mathcal {W}}}\left( \begin{array}{c} \bar{{\mathbf {w}}}_1^T \\ \bar{{\mathbf {w}}}_2^T \\ \bar{{\mathbf {w}}}_3^T \end{array}\right) \qquad \text{ with } \det {\mathcal {W}}=\mathbf{w}_1\cdot \mathbf{w}_2\times \mathbf{w}_3\,, \end{aligned}$$

whose rows are given by the components of the vectors

$$\begin{aligned}&\bar{{\mathbf {w}}}_i:=\left( \begin{array}{c} -a(a^2-b^2)\sin (\theta _{i+1})\sin (\theta _{i+2})(\cos (\theta _{i+1}) -\cos (\theta _{i+2})) \\ b(a^2-b^2)\cos (\theta _{i+1})\cos (\theta _{i+2})(\sin (\theta _{i+1}) -\sin (\theta _{i+2})) \\ -ab\sin (\theta _{i+1}-\theta _{i+2}) \end{array}\right) ,\\&\quad i=1,2,3, \end{aligned}$$

where the indices are meant to cycle when out of bounds (e.g., for $i=3$ we set $i+1=1$ and $i+2=2$). Note that, for $i=1,2,3$, the wrench $\mathbf{w}_i$ depends on $\theta _i$ only, whereas $\bar{{\mathbf {w}}}_i$ depends on the whole triplet $(\theta _1,\theta _2,\theta _3)$.

Then, it is easy to see that the force-closure condition (4)–(5) is equivalent to the following:

$$\begin{aligned} \det {\mathcal {W}}\not = 0,\quad \text{ sign }(\det {\mathcal {W}})\, \bar{{\mathbf {w}}}_i\cdot \mathbf{w}_4 \le 0\,,\quad i=1,2,3. \end{aligned}$$

(6)

We now collect the unknown angles of the target points in the set $\Theta =\{\theta _1,\theta _2,\theta _3,\theta _4\}\subset [0,2\pi )$, and we define the function

$$\begin{aligned} F(\Theta ):=\frac{1}{2}\sum _{i=0}^3 F_i(\Theta ), \end{aligned}$$

where

$$\begin{aligned} F_0(\Theta )={\max }^2\left\{ 0,\varepsilon _0-\mid \det {\mathcal {W}}\mid \right\} , \\ F_i(\Theta )=\left\{ \begin{array}{ll} {\max }^2\left\{ 0, \bar{{\mathbf {w}}}_i\cdot \mathbf{w}_4\right\} &{} \text{ if } \det {\mathcal {W}}>0,\\ {\min }^2\left\{ 0,\bar{{\mathbf {w}}}_i\cdot \mathbf{w}_4\right\} &{} \text{ otherwise }, \end{array}\right. \quad i=1,2,3, \end{aligned}$$

and

$$\begin{aligned} \varepsilon _0= & {} \varepsilon _0(a,b)\nonumber \\:= & {} \varepsilon \max \{\mid \det {\mathcal {W}}(\theta _1,\theta _2,\theta _3)\mid \text{ s.t. } (\theta _1,\theta _2,\theta _3)\in [0,2\pi ]^3\} \end{aligned}$$

(7)

is a fixed parameter, for some $\varepsilon \in (0,1)$.

We remark that F is a non-negative function and, by construction, it follows that $\Theta$ is an absolute minimizer (achieving $F(\Theta )=0$) if and only if the corresponding target points satisfy the force-closure condition (6) with $\mid \det {\mathcal {W}}\mid \ge \varepsilon _0$.

We also remark that $\varepsilon _0$ only depends on (a, b), and it is well defined by virtue of compactness arguments, in particular it is strictly positive if and only if $a\not =b$, i.e., if $\Omega _0$ is not a circle. In Fig. 3, we report the level sets of $\varepsilon _0$ as a function of (a, b), computed numerically for $(a,b)\in (0,2)^2$ and $\varepsilon =1$.

Fig. 3

Level sets of $\varepsilon _0$ as a function of $(a,b)\in (0,2)^2$ for $\varepsilon =1$. The grey-scale color-map ranges from 0 (light grey) to $\max \varepsilon _0$ (black)

Recalling (3), we then define the functional

$$\begin{aligned} \begin{aligned} {\mathcal {G}}(q,S,\Theta ):&=\frac{1}{2}\int _0^1 \frac{\Vert q_{ss}\Vert ^2}{{{\bar{\omega }}}^2(s)} \mathrm{d}s+\frac{1}{2\tau _0}\int _0^1 {\mathcal {O}}(q(s))\mathrm{d}s\\&\quad +\frac{1}{2\tau _1}\sum _{i=1}^4 \Vert q(s_i)-p(\theta _i)\Vert ^2+\frac{1}{\tau _2}F(\Theta ), \end{aligned} \end{aligned}$$

(8)

where the potential for the elliptic obstacle takes the special form

$$\begin{aligned} {\mathcal {O}}(q)=\left( 1-\sqrt{(q-c_0)^T R_0 \left( \begin{array}{cc} \frac{1}{a^2}&{} 0 \\ 0 &{} \frac{1}{b^2} \end{array}\right) R_0^T (q-c_0)}\right) _+^2 \end{aligned}$$

and $\tau _2>0$ is an additional parameter penalizing the violation of the force-closure condition.

Finally, we consider the following optimization problem for the grasping with optimal contact:

$$\begin{aligned} \begin{aligned}&\min \, {\mathcal {G}}(q,S,\Theta )\\&\text {subject to}\\&q(0)=(0,0), q_{s}(0)=(0,-1), \Vert q_s(s)\Vert ^2\\&\quad =1, \Vert q_{ss}(s)\Vert \le {\bar{\omega }}(s) \text {for}\,\, s\in (0,1)\\&\text {and}\,\, s_i\in I_\gamma , \theta _i\in [0,2\pi ) \text {for}\,\, i=1,\ldots ,4. \end{aligned} \end{aligned}$$

(9)

We observe that the third term in the functional (8) provides a quite involved cross coupling of the unknowns $(q,S,\Theta )$, in particular its minimization forces the tentacle q to grasp the obstacle, and also the simultaneous attraction between the contact set S and the target set $\Theta$.

The next step is to obtain necessary optimality conditions for problem (9) in a form suitable for numerical computations, i.e. avoiding for simplicity the KKT conditions associated to the inequality constraint $\Vert q_{ss}\Vert \le {\bar{\omega }}$. This is done via relaxation, by introducing an auxiliary variable z satisfying the equivalent equality constraint

$$\begin{aligned} \Vert q_{ss}\Vert ^2-{\bar{\omega }}^2+z=0 \quad \text {with} z\ge 0, \end{aligned}$$

(10)

and by defining the following augmented Lagrangian

$$\begin{aligned} \begin{aligned} {\mathcal {L}}(q,\sigma ,S,\Theta ,z,\lambda )&:={\mathcal {G}}(q,S,\Theta )+\frac{1}{2}\int _0^1 \sigma (\Vert q_s\Vert ^2-1)\mathrm{d}s\\&\quad +\frac{1}{2}\int _0^1\lambda (\Vert q_{ss}\Vert ^2-{\bar{\omega }}^2+z)\mathrm{d}s\\&\quad +\frac{1}{4\rho _\lambda }\,{-2pt}\int _0^1\,{-5pt}(\Vert q_{ss}\Vert ^2-{\bar{\omega }}^2+z)^2\mathrm{d}s, \end{aligned} \end{aligned}$$

(11)

where $\sigma :[0,1]\rightarrow {{\mathbb {R}}}$ is the Lagrange multiplier for the exact inextensibility constraint $\Vert q_s\Vert =1$. The function $\lambda :[0,1]\rightarrow {{\mathbb {R}}}$ and the constant $\rho _\lambda >0$ are respectively the multiplier and penalty parameter related to the relaxed constraint on the curvature (10). Then, the optimization problem (9) is equivalent to the minimization of ${\mathcal {L}}$, subject to $S\subset I_\gamma$ and $z\ge 0$ (note that, due to the $2\pi$-periodicity of $\{\mathbf{w}_i\}_{i=1,\ldots ,4}$, $\bar{{\mathbf {w}}}_i\_{i=1,\ldots ,3}$ and $\det {\mathcal {W}}$ in the definition of $F(\Theta )$, no constraint is actually required on $\Theta$). Such optimization process can be performed employing the method of multipliers ( [26, 27], see also [28] and the references therein for the infinite-dimensional case), starting from $\lambda ^{(0)}\equiv 0$ and iterating on $k\ge 0$ up to convergence

$$\begin{aligned} \left\{ \begin{array}{l}({\tilde{q}}^{(k+1)},{\tilde{\sigma }}^{(k+1)},{\tilde{S}}^{(k+1)},{\tilde{\Theta }}^{(k+1)},{\tilde{z}}^{(k+1)})=\\ \arg \min _{\begin{array}{l} q,\,\sigma ,\,\Theta \\ S\subset I_\gamma ,\,z\ge 0 \end{array}}\,\,\, {\mathcal {L}}(q,\sigma ,S,\Theta ,z,\lambda ^{(k)})\\ \\ \lambda ^{(k+1)}=\lambda ^{(k)}+\frac{1}{\rho _\lambda }(\Vert {\tilde{q}}_{ss}^{(k+1)}\Vert ^2-{\bar{\omega }}^2+{\tilde{z}}^{(k+1)}). \end{array} \right. \end{aligned}$$

(12)

The optimization in (12) with respect to $z\ge 0$ yields the following variational inequality

$$\begin{aligned} \int _0^1(\lambda ^{(k)} \rho _\lambda +\Vert q_{ss}\Vert ^2-{\bar{\omega }}^2+z)(v-z) ds\ge 0\,,\quad \forall v\ge 0\,, \end{aligned}$$

whose solution is given almost everywhere by

$$\begin{aligned} z(q,\lambda ^{(k)}):=\max \left\{ -\lambda ^{(k)}\rho _\lambda -\Vert q_{ss}\Vert ^2+{{\bar{\omega }}}^2,0\right\} \,. \end{aligned}$$

Plugging the above expression in (12), we get

$$\begin{aligned} \left\{ \begin{array}{l}({\tilde{q}}^{(k+1)},{\tilde{\sigma }}^{(k+1)},{\tilde{S}}^{(k+1)},{\tilde{\Theta }}^{(k+1)})=\\ \arg \min _{\begin{array}{l} q,\,\sigma \,,\Theta \\ S\subset I_\gamma \end{array}}\,\,\,{\mathcal {L}}(q,\sigma ,S,\Theta ,z(q,\lambda ^{(k)}),\lambda ^{(k)})\\ \\ \lambda ^{(k+1)}=\max \left\{ \lambda ^{(k)}+\frac{1}{\rho _\lambda }(\Vert {\tilde{q}}^{(k+1)}_{ss}\Vert ^2-{\bar{\omega }}^2),0\right\} . \end{array} \right. \end{aligned}$$

(13)

On the other hand, the solution $({\tilde{q}}^{(k+1)},{\tilde{\sigma }}^{(k+1)},{\tilde{S}}^{(k+1)},{\tilde{\Theta }}^{(k+1)})$ of the optimization sub-problem for the reduced Lagrangian

$$\begin{aligned} {\mathcal {L}}^{(k)}(q,\sigma ,S,\Theta ):={\mathcal {L}}(q,\sigma ,S,\Theta ,z(q,\lambda ^{(k)}),\lambda ^{(k)}) \end{aligned}$$

(14)

satisfies the following optimality system

$$\begin{aligned} {\left\{ \begin{array}{ll} \left( \Lambda ^{(k)}(q_{ss})q_{ss}\right) _{ss} -\left( \sigma q_s\right) _s\\ +\frac{1}{\tau _0}\nabla {\mathcal {O}}(q(s)) +\frac{1}{\tau _1}\sum _{i=1}^4 (q(s)-p(\theta _i))\delta _{s_i}(s)=0 &{}\text{ in } (0,1)\\ \\ q(0)=0,\quad q_{s}(0)=(0,-1),\quad q_{ss}(1)=0,\quad q_{sss}(1)=0\,,\\ \sigma (1)=0\,,\\ \\ \Vert q_s\Vert ^2=1 &{} \text{ in } (0,1)\\ \\ \frac{1}{\tau _1} (q(s_i)-p(\theta _i))\cdot q_s(s_i)(w_i-s_i)\ge 0\,,&{} \forall w_i\in I_\gamma \,\\ &{}i=1,\ldots ,4\\ \\ \frac{1}{\tau _1}(q(s_i)-p(\theta _i))\cdot \frac{\partial }{\partial \theta _i}p(\theta _i) +\frac{1}{\tau _2}\frac{\partial }{\partial \theta _i}F(\Theta )=0 &{}i=1,\ldots ,4 \end{array}\right. } \end{aligned}$$

(15)

with

$$\begin{aligned} \Lambda ^{(k)}(q_{ss}){:}{=} \frac{1}{{\bar{\omega }}^2}+\max \left\{ \lambda ^{(k)}+\frac{1}{\rho _\lambda }(\Vert q_{ss}\Vert ^2-{\bar{\omega }}^2),0\right\} . \end{aligned}$$

As remarked in [2] (see also [3]), the first equation and the boundary conditions in (15) emerge from the optimization of ${\mathcal {L}}^{(k)}$ with respect to q. Similarly, optimizing ${\mathcal {L}}^{(k)}$ in $\sigma$, S and $\Theta$, respectively, we recover (in order) the inextensibility constraint, four variational inequalities for the contact set (due to the constraint $S\subset I_\gamma )$ and four equations for the target set.

The solution of (15) is approximated as in [2], using a projected gradient descent method. More precisely, we first compute the partial Fréchet derivatives of ${\mathcal {L}}^{(k)}$:

$$\begin{aligned} {\small {\mathcal {L}}^{\prime \,(k)}(q,\sigma ,S,\Theta )=\left( \begin{array}{c} \left( \Lambda ^{(k)}(q_{ss})q_{ss}\right) _{ss} -\left( \sigma q_s\right) _s\\ +\frac{1}{\tau _0}\nabla {\mathcal {O}}(q(s))+\frac{1}{\tau _1}\sum _{i=1}^4 (q(s)-p(\theta _i))\delta _{s_i}(s) \\ \\ \frac{1}{2}(\Vert q_s\Vert ^2-1)\\ \\ \frac{1}{\tau _1}(q(s_1)-p(\theta _1))\cdot q_s(s_1)\\ \vdots \\ \frac{1}{\tau _1}(q(s_4)-p_4)\cdot q_s(s_4)\\ \\ \frac{1}{\tau _1}(q(s_1)-p(\theta _1))\cdot \frac{\partial }{\partial \theta _1}p(\theta _1) +\frac{1}{\tau _2}\frac{\partial }{\partial \theta _1}F(\Theta )\\ \vdots \\ \frac{1}{\tau _1}(q(s_4)-p(\theta _4))\cdot \frac{\partial }{\partial \theta _4}p(\theta _4) +\frac{1}{\tau _2}\frac{\partial }{\partial \theta _4}F(\Theta ) \end{array}\right) }\,. \end{aligned}$$

(16)

Then, given an initial guess $(q^{(0)},\sigma ^{(0)},S^{(0)},\Theta ^{(0)})$ satisfying the boundary conditions and the constraints, we iterate on $n\ge 0$ up to convergence

$$\begin{aligned} {\small {\left\{ \begin{array}{ll} \begin{pmatrix} q^{(n+1)}\\ \sigma ^{(n+1)}\\ {\bar{S}}\\ \Theta ^{(n+1)} \end{pmatrix}=\begin{pmatrix} q^{(n)}\\ \sigma ^{(n)}\\ S^{(n)}\\ \Theta ^{(n)} \end{pmatrix}-\alpha {\mathcal {L}}^{\prime \,(k)}(q^{(n)},\sigma ^{(n)},S^{(n)},\Theta ^{(n)})\,,\\ \\ S^{(n+1)}=\Pi _{I_\gamma }\left( {\bar{S}}\right) \,, \end{array}\right. }} \end{aligned}$$

(17)

where $\alpha >0$ is the step size and

$$\begin{aligned} \Pi _{I_\gamma }(\cdot )=\min \{ \max \{\cdot ,\gamma \}, 1-\gamma \} \end{aligned}$$

is the component-wise projection on $I_\gamma$ ensuring a feasible contact set at each iteration. We refer to [2] for further details on the discretization of the optimality system (15), and on the actual implementation of the full algorithm, including both (17) and the method of multipliers (13).

Numerical Experiments

In this section, we present several numerical experiments, showing the ability of the proposed method to obtain optimal grasping configurations for the manipulator, satisfying the force-closure condition.

Let us first define the setting for the tests. We always choose the curvature threshold function ${\bar{\omega }}(s)=\frac{1-0.9 s}{ (1-0.9 s)+(0.1-0.09 s)}2\pi (3+s^2)$, corresponding to (1) with $\omega (s)=2\pi (3+s^2)$, $\mu (s)=1-0.9 s$ and $\varepsilon (s)=0.1-0.09 s$. We also assume that the manipulator has unit length and it is discretized with 201 nodes. Then, we set $\gamma =\frac{1}{200}$, namely equal to the mesh size, so that the interval $I_\gamma$ for the contact set S contains all the grid nodes except the end points. The elliptic obstacle $\Omega _0$ is centered at the point $c_0=(0.1,-0.3)$, while its semi-axes and rotation are chosen as summarized in Table 1. Moreover, for each pair (a, b), we numerically compute $\varepsilon _0(a,b)$ in (7) choosing $\varepsilon =\frac{1}{2}$.

Table 1 Elliptic obstacle settings

As initial guess for (17), we always choose a profile $q^{(0)}$ surrounding the obstacle, whereas $\sigma ^{(0)}\equiv 0$ and $S^{(0)}$ corresponds to a small random perturbation of equally spaced points along the manipulator. Accordingly, the target set $\Theta ^{(0)}$ is obtained by a small random perturbation of the projection of $q^{(0)}(S^{(0)})$ on the ellipse, with the exception of Test 1 where $\Theta ^{(0)}$ is chosen randomly.

Finally, the step size for the gradient descent method is set to $\alpha =5\cdot 10^{-3}$, while the penalty parameters $\tau _0$, $\tau _1$, $\tau _2$ and $\rho _\lambda$ are slowly decreased using the continuation method discussed in [2]. In particular, at each iteration of the optimization process, we keep the relationship $\tau _0< \tau _1 < \tau _2$ to prioritize, in order, the obstacle avoidance, the contact, the force-closure, and finally the curvature minimization. This results in a greater mobility of S and $\Theta$ in the grasping task.

In Test 1, we consider, for comparison purposes, the case of frozen target points as in [2], removing from (9) the optimization with respect to $\Theta$. In particular, the chosen target set does not satisfy the force-closure condition.

In Fig. 4, we report the results of the optimization. More precisely, in Fig. 4a, we show the obstacle (grey), the target points (black circles), the optimal contact points (yellow circles) and the optimal configuration q of the manipulator, while Fig. 4b represents the corresponding signed curvature (thicker line) as a function of $s\in [0,1]$, and the thresholds $\pm {\bar{\omega }}(s)$ (thin lines).

Fig. 4

Test 1, optimal grasping configuration (a), optimal curvature (b) and convergence history of the optimal contact set (c)

Moreover, in Fig. 4c, we show the evolution of the contact set S versus the total number of iterations to reach convergence.

We clearly observe a temporary saturation of S, which corresponds to the higher priority given to the minimization of the distance between the contact set and the target set. In the remaining part of the evolution, once the manipulator has grasped the obstacle, the optimization proceeds with a sliding of $s_2$, $s_3$, $s_4$ toward the free end. This reflects an attempt of the manipulator to unroll itself to reduce also the curvature contribution in (8), which has already reached the threshold ${\bar{\omega }}$ is some regions (see again Fig. 4b), due to the large eccentricity of the obstacle. Nevertheless, the resulting configuration of the manipulator looks quite “unnatural” (see again Fig. 4a), we can somehow perceive a great effort of the controls in the grasping task, and this is ultimately related to the lack of mobility of the target set.

In Test 2, we consider the case of an obstacle with a lower eccentricity, and we perform the optimization of the complete functional (8) with respect to $(q,S,\Theta )$. The results are reported in Fig. 5, where we also show the convergence history of the target set (Fig. 5d) and of the four terms related to the force-closure condition (6) (Fig. 5e).

Here, it is interesting to observe that the force-closure condition is satisfied and preserved during the first part of the optimization, which is mostly devoted to the adjustment of $\Theta$ (note indeed in Fig. 5c that the contact set S remains almost unchanged).

From now on, the optimization proceeds focusing on the contact and curvature terms in (8), as confirmed by the sliding of S in Fig. 5c. This slightly violates the condition $\text{ sign }(\det {\mathcal {W}})\, \bar{{\mathbf {w}}}_3\cdot \mathbf{w}_4 \le 0$ and also produces a decrease in the term $\mid \det {\mathcal {W}}\mid$. In the remaining iterations, the missing condition is recovered, while the determinant of ${\mathcal {W}}$ is locked above the bound $\varepsilon _0$, both with small adjustments of $\Theta$.

Finally, comparing Fig. 5a, b, we readily observe the correspondence between the contact points and the faster transitions in the curvature of the manipulator. Since the grasping is imposed on q(S) only, the manipulator need not to retrace the profile (hence the curvature) of the obstacle. This is more apparent in the part of q between the last two contact points.

Fig. 5

Test 2, optimal grasping configuration (a), optimal curvature (b) and convergence history of (c) the optimal contact set, d optimal target set, e force-closure condition

Test 3 and Test 4 are more challenging than the previous one. The obstacle has now a larger eccentricity and it is differently rotated, which forces higher values in the curvature of the manipulator, as observed in Test 1. Moreover, in these two tests, the initial guess for the target set is such that the force-closure condition is more and more violated at the beginning of the optimization. The results are similar, as reported in Figs. 6 and 7 respectively.

In both cases, we observe that one contact occurs at the point of the obstacle furthest from the anchor point of the manipulator, precisely on the major axis of the ellipse, where the curvature achieves its largest value.

Moreover, in both tests, we recognize a relevant sliding of the contact set, especially for the contact point near the free end of the manipulator, aiming at avoiding adherence to the obstacle, so to reduce the total curvature as much as possible. This behavior is more apparent in Test 4, due to the different orientation of the obstacle. In particular, we also observe some adjustment in the first contact point, which prevents the curvature from changing sign in the first part of the manipulator, as in Test 3.

Fig. 6

Test 3, optimal grasping configuration (a), optimal curvature (b) and convergence history of c the optimal contact set, d optimal target set, e force-closure condition

Fig. 7

Test 4, optimal grasping configuration (a), optimal curvature (b) and convergence history of c the optimal contact set, d optimal target set, e force-closure condition

Conclusions

In this paper, we addressed optimal force-closure configurations for a soft manipulator. The underlying model corresponds to the equilibria of a dynamical system, which in turn models the evolution of a tentacle-like manipulator subject to inextensibility and curvature constraint, a bending moment and a distributed curvature control, introduced in [5]. The stationary setting taken into exam here constraints the configurations of the manipulator to a system of controlled ordinary differential equations (1). The goal is to select an admissible force-closure configuration (namely a solution of (1)) which avoids interpenetration with an elliptic target while minimizing a quadratic cost on the controls. Our approach is based on optimal control theory and it consists in the constrained minimization of an appropriate cost functional, see the problem (9). Then, a discretization algorithm is proposed for the numerical solution. Finally, some test cases are taken into exam. The observation of the resulting optimal solutions and the analysis of the optimization process confirmed the consistency of the proposed approach. Also they shed some light on how the parameters impact the selection of the optimal configuration.

The present paper is part of an ongoing investigation on optimal control strategies for soft-manipulators. This is a relatively new line of research, exploiting a variational approach to embed (internal and external) reaction forces in the system and, at the same time, to minimize of the costs associated to the execution of some reachability and grasping tasks by a tentacle-like soft manipulator. The main novelty, compared with previous works by the authors, is the optimization of configurations that not only optimally wrap the target while avoiding interpenetration, but also satisfy a force-closure condition. A far from trivial aspect of the problem is the definition an appropriate cost functional, the functional ${\mathcal {G}}$ introduced in (8), which also required a geometrical analysis of the force-closure condition. Further directions of research include: the study of more general shapes of the target, higher order force-closure conditions, the introduction of friction in the contact forces, and an extension of the above techniques to a fully dynamical setting.