Constrained reachability problems for a planar manipulator
CConstrained reachability problems for a planarmanipulator
Simone Cacace , Anna Chiara Lai − − − , and Paola Loreti Dipartimento di Matematica e Fisica, Universit`a degli studi Roma Tre, Rome, Italy [email protected] Dipartimento di Scienze di Base e Applicate per l’Ingegneria, Sapienza Universit`adi Roma, Rome, Italy { anna.lai,paola.loreti } @sbai.uniroma1.it Abstract.
We address an optimal reachability problem for a planarmanipulator in a constrained environment. After introducing the opt-mization problem in full generality, we practically embed the geometryof the workspace in the problem, by considering some classes of obsta-cles. To this end, we present an analytical approximation of the distancefunction from the ellipse. We then apply our method to particular mod-els of hyper-redundant and soft manipulators, by also presenting somenumerical experiments.
Keywords:
Optimal reachability · obstacle avoidance · octopus-like ma-nipulators · hyper-redundant manipulators. We address an optimal reachability problem for a planar manipulator in a con-strained environment, which is part of an ongoing investigation on snake-like andoctopus-like manipulators in the framework of optimal control theory of partialdifferential equations. The models discussed in the present paper were originallyintroduced in [5]. Subsequent works by the authors refined the investigationsin the cases of uncontrolled regions of the manipulators (modeling mechanicalbreakdowns)[4], and grasping tasks [2]. Part of the results presented here earlierappeared in [3], the main novelty consists in a more general setting of the prob-lem, the investigation of a much wider class of obstacle geometries, and relatednew numerical tests.Our setting is stationary, namely we optimize the shape of a planar manipu-lator at the equilibrium. We begin our investigation by considering the problemin full generality, from an optimal control theoretic perspective. We introduce acost functional encompassing by penalization the obstacle avoidance and reach-ability tasks, and a quadratic running cost on the controls. The problem is thento minimize such functional in the set all of the admissible equilibrium configu-rations of the manipulator.Then we address the issue of practically encompassing the geometry of theworking space in the problem. More precisely, the obstacle avoidance task is a r X i v : . [ m a t h . O C ] J a n S. Cacace et al. enforced by introducing an elastic potential steering the manipulator outsidethe obstacles. In our setting, such potential is deeply related to the distancefunction from the obstacles. Our case study includes obstacles composed bycircles, squares and ellipses. In particular, the study of the distance functionfrom an ellipse involves root finding of a quartic polynomial, and its numericalcomputation can result cumbersome in the case of general or time-varying ellipses[19]. We present an analytical approximation of the distance function from theellipse, based on the linearization of an explicit formula for the roots of quarticpolynomials. Moreover, we describe the approximation of the distance functionfor general closed obstacles with compact boundary.In the second part of the paper, we specialize the optimization problem tothe case of two classes of planar manipulators: a hyper-redundant manipulatorand a soft manipulator. These devices share the same physical features, respec-tively declined in an either discrete or continuous fashion. We assume indeedan inextensibility constraint, a non-uniform angle/curvature constraint, a bend-ing moment (on the joints in the discrete case and pointwise in the continuousone) and angle/curvature controls. A Lagrangian formulation of the dynamics isintroduced for both models, and we provide an explicit characterization of theequilibria. Finally, the optimal reachability problem with obstacle avoidance isnumerically solved in some cases of interest.From the seminal paper [7], where the hyper-redundant manipulators werefirstly introduced, countless papers were devoted to the control of octopus-likemanipulators in constrained environments, see for instance [16,17,14,8] and thereference therein for a general introduction. The papers that mostly inspiredour work include [6], for an early study on the interplay between the continuousand discrete settings, and [1,20] for an optimal control theoretic approach toconstrained reachability problems. We also refer to the papers [9,10,11,12,13] fora modeling overview.
Organization of the paper.
In Section 2, we introduce the optimal control prob-lem, while Section 3 is devoted to the computation of distance functions fromcompact sets. In Section 4 and Section 5, we specialize the optimal control prob-lem, respectively to a class of hyper-redundant and soft manipulators, and wepresent some numerical simulations.
In this section, we consider a general, unidimensional planar manipulator, whosestationary configuration is modeled by a function q ( s ; u ) := [0 , → R depend-ing on its arclength coordinate s and on a control u : A ⊆ [0 , → U , where U isthe control set . The function q is described as a solution of an either controlledcontinuous differential equation or a difference equation, in the form (cid:40) q (cid:48) = f ( q, u ) q (0) = q ∈ R , (1) onstrained reachability problems for a planar manipulator 3 where, with a little abuse of notation, q (cid:48) denotes either a derivative or a finitedifference, and f : R × U → R . However, in the special cases treated in thepresent paper, we also have an explicit input-to-state map u (cid:55)→ q ( · ; u ). The do-main A of the control function depends on the adopted model. For instance, ifwe are dealing with a discrete manipulator, A is a finite (or countable) set ofpoints corresponding to the joints. Otherwise, if we are dealing with a soft robot,we may set A = [0 , A may be a finite union of intervals to model sce-narios in which only a portion of the manipulator is controlled, see for instance[4]. We denote by A the set of admissible configuration-control pairs , that is thecouples ( q, u ) such that u is a control function, q ( s ) = q ( s ; u ) is the correspond-ing configuration, and such that some regularity assumptions are satisfied. Forinstance, if we are in a continuous, differential setting, one can define A as A := { ( q, u ) | u : [0 , → U is measurable and q is a Carath´eodory solution of (1) } . Concerning the working space geometry, we denote by Ω ⊂ R a closedsubset of R with compact boundary representing an obstacle . In our examples Ω is either a circle, a square, an ellipse or a finite union of these objects. Wetake into account also the distance function from Ω c := R \ Ωq (cid:55)→ d ( q, Ω c ) := inf x ∈ Ω c {| x − q |} . The target is a point q ∗ ∈ R . Finally, we consider a running cost (cid:96) ( q, u ) : R × U → [0 , + ∞ ). For instance, a quadratic cost on the controls is independentfrom the position of the manipulator, and it reads (cid:96) ( u ) = u .In this setting, we consider the problem of finding an admissible configuration-control pair ( q, u ) such that1. q avoids the obstacle Ω minimizing the tip-target distance | q (1) − q ∗ | ;2. ( q, u ) minimizes the associated integral cost (cid:90) (cid:96) ( q ( s ) , u ( s )) ds. The problem can be attacked by considering the cost functional: J ( q, u ) := 12 (cid:90) (cid:96) ( q ( s ) , u ( s )) ds + 12 δ | q (1) − q ∗ | + 12 τ (cid:90) d ( q ( s ) , Ω c ) ds, (2)with penalty parameters δ, τ >
0. We recognize in the first two terms of J theintegral cost and the tip-target distance. The third term vanishes if and only ifthere is no interpenetration of q with the obstacle Ω , i.e., this term encompasses S. Cacace et al. the obstacle avoidance task as τ →
0. Then, we recast the optimal reachabilityproblem as the following constrained optimization problemminimize J ( q, u ) subject to ( q, u ) ∈ A . (3)In Section 4 and 5, we specialize this problem to a class of hyper-redundant andsoft manipulators, by providing an explicit description of the underlying controlmodel. In this section, we collect some distance formulas for obstacles with compactboundary. In our tests we take into exam circular, square and elliptic obstacles.We recall here the distance functions of a point q = ( q , q ) from the boundariesof a square of side l and of a circle of radius r , centered in c ∈ R : d square ( q ) := min a ,a ∈{ , − l/ ,l/ } min {| q − c + ( a , a ) | } d circle ( q ) := ( | q − c | − r ) . In what follows, we take into exam an analytical approximation for the distancefunction from an ellipse, and we describe a strategy for the numerical approxi-mation of the distance function from general sets with compact boundaries.
Let 0 < b ≤ a and consider the ellipse E ( a, b ) centered in the origin with semi-axes a and b , implicitly defined by the equation E ( x ) := E ( x , x ) = (cid:16) x a (cid:17) + (cid:16) x b (cid:17) − . (4)We define the square distance from E ( a, b ) by d ( q ) := min {| x − q | | E ( x ) = 0 , x ∈ R } . We fix q ∈ R and we use the Lagrange multiplier method to investigate d ( q ).Consider the Lagrangian function L q ( x, λ ) := | x − q | − λE ( x ) . The minimization of L q leads to the optimality system with unkowns x = ( x , x )and λ : q = x (cid:0) − λa (cid:1) q = x (cid:0) − λb (cid:1) E ( x ) = 0 λ ≤ b . (5) onstrained reachability problems for a planar manipulator 5 Note that the first three equations are stationarity conditions, while the in-equality in the multiplier λ is an actual local minimality condition. By algebraiccomputations one ends up with the equivalent formulation: q = x (cid:0) − λa (cid:1) q = x (cid:0) − λb (cid:1) P ( λ ) := (cid:0)(cid:0) ( λ − a (cid:1) (cid:0) λ − b (cid:1)(cid:1) − a q (cid:0) λ − b (cid:1) − b q (cid:0) λ − a (cid:1) = 0 λ ≤ b . (6)Now, one can prove that the required multiplier λ ∗ ( q ) is the smallest root of P . Indeed, the case a = b corresponds to the circle, and it is trivial to checkthat λ ∗ ( q ) = a − a | q | ≤ a = b . If otherwise b < a and if q (cid:54) = 0, then λ ∗ ( q )is univoquely determined by the above system, since P admits one and onlyone root in the interval ( −∞ , b ). Finally, if b < a and q = 0, then P ( b ) = P ( a − a | q | ) = 0 and a direct computation implies the global minimum of L q tobe attained at points of the form ( x ∗ ( q ) , λ ∗ ( q )) with λ ∗ ( q ) = min { a − a | q | , b } .Hence, the exact formula for the distance is given by d ( q ) = (cid:18) λ ∗ ( q ) a − λ ∗ ( q ) q (cid:19) + (cid:18) λ ∗ ( q ) b − λ ∗ ( q ) q (cid:19) if q (cid:54) = 0or a − a | q | < b b if q = q = 0 b − b a − b q if q = 0 , q (cid:54) = 0and a − a | q | ≥ b . (7)Note that, if b < a , the second case in above expression is a particular case of thethird case. Our idea is to use explict formula for the roots of quartic polynomialsto approximate λ ∗ as ε := a − b → + . Let us rewrite P ( λ ) as P ε ( λ ) := (cid:0)(cid:0) ( λ − b − ε (cid:1) (cid:0) λ − b (cid:1)(cid:1) − ( b + ε ) q (cid:0) λ − b (cid:1) − b q (cid:0) λ − b − ε (cid:1) . Clearly, λ ∗ ( q ) is also the smallest root of P ε , and we denote by λ ε ( q ) its firstorder approximation, so that λ ∗ ( q ) = λ ε ( q ) + o ( ε ) as ε → + for all q ∈ R .Then we replace λ ∗ ( q ) in (7) by λ ε ( q ):¯ d ε ( q ) := (cid:18) λ ε ( q ) b + ε − λ ε ( q ) q (cid:19) + (cid:18) λ ε ( q ) b − λ ε ( q ) q (cid:19) if q (cid:54) = 0 or q > ε b + εb − b ε q otherwise. (8)Incidentally, notice that ¯ d ε ( q ) ≤ b − ε b b + ε when q = 0 and q ≤ ε b + ε . Weperformed a symbolic computation using the Wolfram Mathematica software to S. Cacace et al. get the following first order approximation of ¯ d ε : d ε ( q ) := ( b − | q | ) + ε q | q | ( b − | q | ) if q (cid:54) = 0or q ≥ ε b + εb − b ε q otherwise. (9)By construction, we finally get, for all q ∈ R , the estimate d ( q ) = d ε ( q ) + o ( ε ) as ε → + . When dealing with a general obstacle, analytical expressions for the distancefunction are no longer available. Nevertheless, from a theoretical point of view,the distance function can be characterized as the solution of a first order partialdifferential Hamilton-Jacobi equation, the celebrated Eikonal equation: (cid:26) |∇ d ( x ) | = 1 x ∈ Ω , d ( x ) = 0 x ∈ R \ Ω .
It is well known that the distance function is merely continuous, since its gradientcan exhibit singularities. This is the case even for the examples discussed above,namely the distance function for the circle is not differentiable at its center, forthe square on the diagonals, and for the ellipse on the segment joining its foci(see Figure 1). Hence, the solution to the Eikonal equation should be meant
Fig. 1.
Level sets of the distance function for a circle, a square, an ellipse. in a suitable weak sense, introducing the notion of viscosity solutions. There isa wide literature on this subject, also from a numerical point of view, whichdates back to the seventies and it is still growing nowadays. This is far beyondthe scope of the present paper, and we refer the interested reader to [15] as a onstrained reachability problems for a planar manipulator 7 starting point. Here, we just remark that the Eikonal equation can be solvednumerically employing one of the available state-of-the-art algorithms, such asthe fast marching method (see [18]). To this end, it is enough to provide thesolver a triangulation of Ω , and impose the Dirichlet condition d = 0 on thediscrete boundary. Once the numerical solution is computed, it can be extendedto the whole space via interpolation. We consider the optimal control problem introduced in Section 2 in the case ofa planar hyper-redundant manipulator, whose joints are subject to an angularconstraint, a bending moment and an angular control. This model was earlierintroduced in [5] and later extended to a more general setting in [3]. Here, afterrecalling the main features and properties of the model, we address the associatedoptimal constrained reachability problem for different types of obstacles.
The planar manipulator under exam is composed by N rigid links and N + 1joints. We denote by m k the mass of the k -th joint, for k = 0 , . . . , N , and weconsider negligible the mass of the corresponding links. The positions of the jointsare stored in the array q = ( q , . . . , q N ), where q := (0 ,
0) is the anchor point.To make some of the definitions below consistent, we also consider the ghostjoints q − := q + (0 , (cid:96) ) for some positive (cid:96) , and q N +1 := q N + ( q N − q N − ) atthe free end. The features of this manipulator are the following.First, we have an inextensibility constraint , representing the fact that thelinks of the manipulator are rigid, therefore each couple of consecutive jointssatisfies | q k − q k − | = (cid:96) k , for k = 1 , . . . , N , where (cid:96) k > k -thlink. We introduce this constraint exactly, by considering the functions F k ( q, σ ) := σ k (cid:0) | q k − q k − | − (cid:96) k (cid:1) for k = 1 , . . . , N , (10)where σ k is a Lagrange multiplier.The second matter under exam is the behavior of the joints. The modelprescribes that two consecutive links, say the k -th and the k +1-th, tend to resistto bending and, however, they cannot form an angle larger in modulus than afixed threshold α k . These two constraints are introduced via penalization, i.e.,by considering two angular elastic potentials. We set B k ( q ) := ε k b k ( q ) , (11)with b k ( q ) := ( q k +1 − q k ) × ( q k − q k − ) , and v × v := v · v ⊥ , where v ⊥ denotes the clockwise orthogonal vector to v . Thefunction B k ( q ) represents an elastic potential, with penalty parameter ε k > S. Cacace et al. associated to the bending moment , corresponding to the constraint b k ( q ) = 0.Similarly, we set G k ( q ) := ν k g k ( q ) , (12)with g k ( q ) := (cid:32) cos( α k ) − (cid:96) k +1 (cid:96) k ( q k +1 − q k ) · ( q k − q k − ) (cid:33) + , where ( · ) + denotes the positive part of its argument. The function G k ( q ) isassociated to the angular constraint g k ( q ) = 0, forcing, with penalty parameter ν k >
0, the relative angle between the k -th and k + 1-th links in the interval[ − α k , α k ].Finally, we consider the control term . We choose the control set U := [ − , k -th and k + 1-th links to be equal to α k u k – the control set [ − ,
1] is chosen in order to be consistent with the angleconstraint. This reduces to the following equality constraint: b k ( q ) − (cid:96) k +1 (cid:96) k sin( α k u k ) = 0 . Also in this case, we enforce the constraint via penalization, by considering H k ( q, u ) := µ k ( (cid:96) k +1 (cid:96) k sin( α k u k ) − b k ( q )) , (13)where µ k ≥ µ k = 0 correspondsto deactivate the control of the k -th joint and let it evolve according to theremaining constraints only.We then build the Lagrangian associated to the hyper-redundant manipulatorby introducing a kinetic energy term and the above discussed elastic potentials: L N ( q, ˙ q, σ, u ) := N (cid:88) k =0 m k | ˙ q k | − F k ( q, σ ) − G k ( q ) − B k ( q ) − H k ( q, u ) . (14)For every fixed control array u ∈ [ − , N , the associated equilibria corre-spond to the (unique) solution of the following stationary system: ∇ q L N = 0 | q k − q k − | = (cid:96) k k = 1 , . . . , Nq = (0 , q − = q + (cid:96) (0 , q N +1 = q N + ( q N − q N − ) . (15)We recall from [3] the explicit characterization of the solutions of the abovesystem. Proposition 1
Fix u ∈ [ − , N , assume α k ∈ [0 , π/ for k = 0 , . . . , N − ,define ¯ α k := arcsin µ k ε k + µ k sin u k α k onstrained reachability problems for a planar manipulator 9 and, for k = 1 , . . . , N z k := − i k (cid:88) j =1 (cid:96) j e i (cid:80) j − h =0 ¯ α h . Then the vector q = ( q , q , . . . , q N ) defined by q k = (cid:40) (0 , if k = 0( Re ( z k ) , Im ( z k )) if k = 1 , . . . , N is the solution of (15) . By Proposition 1, if α k ∈ [0 , π/
2] then the input-to-state map u (cid:55)→ ( q [ u ] , . . . , q N [ u ])associated to (15) reads q k [ u ] := k (cid:88) j =1 (cid:96) j (cid:16) sin ( θ j [ u ]) , − cos ( θ j [ u ]) (cid:17) (16)where θ j [ u ] := j − (cid:88) h =0 ¯ α h [ u ]; ¯ α k [ u ] = arcsin µ k ε k + µ k sin u k α k . Finally, we assume for simplicity that the total length of the manipulator isnormalized to 1, i.e., (cid:80) Nk =1 (cid:96) k = 1. Since the manipulator is composed by a seriesof rigid, inextensible links, its equilibria configurations can be parametrized bya linear interpolation q ( s ; u ) of its joints coordinates ( q [ u ] , . . . , q N [ u ]): q ( s ; u ) := (cid:96) k − s + s k (cid:96) k q k [ u ] + s − s k (cid:96) k q k +1 [ u ] , s ∈ ( s k , s k +1 ] , k = 0 , . . . , N − s k := (cid:80) kj =1 (cid:96) j . We now specialize the optimal reachability problem described in Section 2 tothe present model. The control set is U = [ − , q ( s ; u )is given by (17). We choose a control quadratic running cost (cid:96) ( u ) := u . Then,for a given target point q ∗ ∈ R , and a closed subset Ω of R representing theobstacle, problem (3) reads:min J , subject to (15) and to u ∈ [ − , N , (18)with J ( q, u ) := 12 || u || + 12 δ | q (1 , u ) − q ∗ | + 12 τ (cid:90) d ( q ( s ; u ) , Ω c ) ds, (19)where || u || is the l norm of the control vector u and δ and τ are positivepenalty parameters. Note that, due to the particular form of the input-to-statemap (16), the function (19) actually depends on u only. N = 8Number of samples S = 104 ( m = 13)length of the links (cid:96) k = 1 / ε k = 10 − (1 − . s km )curvature control µ k = 1 − . s km penaltyangle constraint α k = 2 π (2 + s km )target point q ∗ = (0 . , − . δ = 10 − obstacle penalty τ = 10 − Table 1.
Global parameter settings for the hyper-redundant manipulator.
Numerical simulations.
We discretize the parametrization interval [0 ,
1] using S + 1 uniformly distributed samples s i = i/S , for i = 0 , ..., S . Here, S = mN isa multiple ( m (cid:29)
1) of the number of links, so that, q ( s km + j ; u ) = (1 − λ j ) q k [ u ] + λ j q k +1 [ u ] , ∀ k = 0 , . . . , N − , j = 0 , . . . , m − λ j = j/m . As in [3], we approximate the integral term in (19) by a rect-angular quadrature rule, obtaining a fully discrete objective function J ( u ) with u ∈ [ − , N . We then use a projected gradient descent method to solve thefinite-dimensional constrained optimization of J ( u ). Moreover, we start with τ (cid:29) δ and run the optimization up to convergence, then we slowly decrease τ and repeat the optimization until τ is suitably small. In this way, we firstobtain an optimal configuration for the tip-target distance without consideringthe obstacle. Then, we iterate the procedure, to progressively penalize all thepossible interpenetrations with the obstacle. In Algorithm 1, we recall from [3]the algorithm summarizing the whole optimization process– note that we denoteby Π [ − , N ( u ) the projection of u on [ − , N . The simulation parameters aresummarized in Table 1. Test ObstacleTest 1 Ω = ∅ Test 2 Ω = B . (0 . , − . Ω = B . (0 . , − . ∪ B . (0 . , − . Ω = Q ◦ . (0 . , − . Ω = E ◦ . , . (0 . , − . Ω = Q ◦ . (0 . , − . ∪ E ◦ . , . (0 . , − . Table 2.
Obstacle settings. B r ( x ) , Q αl ( x ) , E αa,b ⊂ R denote respectively the ball ofradius r , the square of side l , the ellipse of semi axes a , b , centered in x and clockwiserotated by the angle α (in degrees).onstrained reachability problems for a planar manipulator 11 Algorithm 1
1: Fix tol > tol τ = τ , and a step size 0 < γ <
12: Assign an initial guess u (0) ∈ [ − , N
3: Compute J ( u (0) ) and set J tmp = 04: Set τ >> δ repeat n ← τ ← τ / repeat J tmp ← J ( u ( n ) )9: Compute ∇ J ( u ( n ) )10: u ( n ) ← Π [ − , N { u ( n ) − γ ∇ J ( u ( n ) ) } n ← n + 112: Compute J ( u ( n ) )13: until | J ( u ( n ) ) − J tmp | < tol u (0) ← u ( n ) until τ < tol τ -0.5-0.4-0.3-0.2-0.10-0.1 0 0.1 0.2 0.3 0.4 -0.5-0.4-0.3-0.2-0.10-0.1 0 0.1 0.2 0.3 0.4 -0.5-0.4-0.3-0.2-0.10-0.1 0 0.1 0.2 0.3 0.4 -0.5-0.4-0.3-0.2-0.10-0.1 0 0.1 0.2 0.3 0.4 -0.5-0.4-0.3-0.2-0.10-0.1 0 0.1 0.2 0.3 0.4 -0.5-0.4-0.3-0.2-0.10-0.1 0 0.1 0.2 0.3 0.4 Fig. 2.
The solution q of Test 1-6, respectively. We compare the cases reported in Table 2, namely the cases in which Ω isthe empty set (Test 1), Ω is a ball (Test 2), Ω is the disjoint union of two balls(Test 3), Ω is a rotated square (Test 4), Ω is a rotated ellipse (Test 5), and Ω isthe disjoint union of a rotated square and a rotated ellipse (Test 6). Note that inTest 1 and Test 2 the target q ∗ is reached by the end-effector of the manipulator,with clearly different optimal solutions emerging from the differences betweenthe workspaces. On the other hand, in the remaining tests, we observe that thetarget is unreachable, since the parameters are set in order to prioritize obstacleavoidance. Table 3.
Exact constraint equations and associated potentials in both discrete andcontinuous settings.
Constraint
Discrete ContinuousInextensibility Equation | q k − q k − | = (cid:96) | q s | = 1Curvature Equation ( q k +1 − q k ) · ( q k − q k − ) ≥ (cid:96) cos( α k ) | q ss | ≤ ω Penalization ν k (cid:0) cos( α k ) − (cid:96) ( q k +1 − q k ) · ( q k − q k − ) (cid:1) ν ( | q ss | − ω ) Bending Equation ( q k +1 − q k ) × ( q k − q k − ) = 0 | q ss | = 0Penalization ε k (cid:16) ( q k +1 − q k ) × ( q k − q k − ) (cid:17) ε | q ss | Control Equation ( q k +1 − q k ) × ( q k − q k − ) = (cid:96) sin( α k u k ) q s × q ss = ωu Potential µ k (cid:0) sin( α k u k ) − (cid:96) ( q k +1 − q k ) × ( q k − q k − ) (cid:1) µ ( ωu − q s × q ss ) We consider a soft manipulator introduced in [5] and encompassing the con-tinuous counter part of the features of the hyper-redundant manipulators de-scribed in Section 2. In particular, the device is modeled as an inextensibleelastic string subject to curvature constraints, representing a bending momentand preventing the device to bend over a fixed threshold. Moreover the curvatureis forced pointwise by a control term, modeling an angular elastic internal force.
The time-varying configuration of the soft-manipulator is parametrized by thefunction q : [0 , × [0 , + ∞ ) → R . Its evolution is determined by internal reactionforces, emerging from the inextensibility and curvature constraints and fromthe control term. Such constraints and the associated angular elastic potentialsare derived from the formal limit (as the number of joints goes infinity) of theangular constraints of the hyper-redundant manipulator, see [2,5]. In Table 3,we compare the discrete and continuous versions of the constraints under examand the related elastic potentials. We build the continuous counter part of theLagrangian introduced in (14): L ( q, σ ) := (cid:90) (cid:16) ρ | q t | (cid:124) (cid:123)(cid:122) (cid:125) kinetic energy − σ ( | q s | − (cid:124) (cid:123)(cid:122) (cid:125) inextensibility constr. − ν (cid:0) | q ss | − ω (cid:1) (cid:124) (cid:123)(cid:122) (cid:125) curvature constr. − ε | q ss | (cid:124) (cid:123)(cid:122) (cid:125) bending moment − µ ( ωu − q s × q ss ) (cid:124) (cid:123)(cid:122) (cid:125) curvature control (cid:17) ds , (20) onstrained reachability problems for a planar manipulator 13 where q t , q s , q ss denote partial derivatives in time and space respectively, ρ :[0 , → R + is the mass distribution, ν, ε, µ : [0 , → R + are the angular elasticweights associated, respectively, to the curvature constraint, the bending momentand the curvature control, while u : [0 , × [0 , + ∞ ) → [ − ,
1] is the curvaturecontrol.The equilibria of the system associated with the Lagrangian (20) were ex-plicitely characterized in [5]. In particular, assuming the technical condition µ (1) = µ s (1) = 0, the shape of the manipulator at the equilibrium is the solution q of the following second order controlled ODE: q ss = ¯ ω u q ⊥ s in (0 , | q s | = 1 in (0 , q (0) = (0 , q s (0) = (0 , − , (21)where ¯ ω ( s ) := µ ( s ) ω ( s ) µ ( s ) + ε ( s ) . Assuming a sufficient regularity on the control function u and solving (21), weobtain the following continuous version of (16), namely the input-to-state map u (cid:55)→ q ( s ; u ) = (cid:90) s (cid:16) sin( (cid:90) ξ ¯ ω ( z ) u ( z ) dz ) , − cos( (cid:90) ξ ¯ ω ( z ) u ( z ) dz ) (cid:17) dξ . (22) We interpret the general static optimal reachability problem, discussed in Sec-tion 2, in the framework of soft robotics. The control set is U = [ − , q ( s ; u ) is a solution of the control ordinary differential equa-tion (21). As in the discrete case, we choose a control quadratic running cost (cid:96) ( u ) := u . Then, given an obstacle Ω ⊂ R and a target point q ∗ ∈ R \ Ω , thegeneral problem (3) readsmin J , subject to (21) and to | u | ≤ J ( q, u ) := 12 (cid:90) u ( s ) ds + 12 δ | q (1) − q ∗ | + 12 τ (cid:90) d ( q ( s ) , Ω c ) ds, (24)with δ, τ >
0. We recall that the three components of the above cost functionalrespectively represent: a quadratic cost on the controls, a tip-target distance, andan integral term vanishing if and only if no interpenetration with the obstacle Ω occurs. Similarly to the discrete case, the input-to-state map (22) allows toreduce J to a functional depending on the control u only. N = 100Discretization step ∆ s = 1 /N = 0 . ε ( s ) = 10 − (1 − . s )curvature control µ ( s ) = 1 − . s penaltycurvature constraint ω ( s ) = 2 π (2 + s )target point q ∗ = (0 . , − . δ = 10 − obstacle penalty τ = 10 − Table 4.
Global parameter settings for the soft manipulator.
Numerical simulations.
Discretization and optimization are performed asin the case of hyper-redundant manipulators, using quadrature rules to approxi-mate the integrals appearing in the input-to-state map (22) and in the functional(2). For the sake of comparison, we adopt the same obstacle settings of the dis-crete case, reported in Table 2. The other global parameter settings are in Table4. We note that in Test 1 and Test 2 the target is reached and the optimal con-trolled curvature κ is far below the fixed threshold ¯ ω – see Figure 3(a.1-2) andFigure 3(b.1-.2). The remaining tests displayed in Figure 3 show more clearlythe impact of curvature and obstacle avoidance constraints on the optimizationprocess: the optimal configuration fails in reaching the target. References
1. Bobrow, J.E., Dubowsky, S., Gibson, J.: On the optimal control of robotic ma-nipulators with actuator constraints. In: 1983 American Control Conference. pp.782–787. IEEE (1983)2. Cacace, S., Lai, A.C., Loreti, P.: Optimal reachability and grasping for a softmanipulator. arXiv preprint, 2002.05476 (2020)3. Cacace, S., Lai, A.C., Loreti, P.: Optimal reachability with obstacle avoidance forhyper-redundant and soft manipulators. In: ICINCO 2020 - Proceedings of the 17thInternational Conference on Informatics in Control, Automation and Robotics. pp.134–141 (2020)4. Cacace, S., Lai, A.C., Loreti, P.: Control strategies for an octopus-like soft ma-nipulator. In: Proceedings of the 16th International Conference on Informatics inControl, Automation and Robotics - Volume 1: ICINCO,. pp. 82–90. INSTICC,SciTePress (2019)5. Cacace, S., Lai, A.C., Loreti, P.: Modeling and optimal control of an octopustentacle. SIAM Journal on Control and Optimization (1), 59–84 (2020)6. Chirikjian, G.S.: Hyper-redundant manipulator dynamics: A continuum approxi-mation. Advanced Robotics (3), 217–243 (1994)7. Chirikjian, G.S., Burdick, J.W.: An obstacle avoidance algorithm for hyper-redundant manipulators. In: Proceedings., IEEE International Conference onRobotics and Automation. pp. 625–631. IEEE (1990)onstrained reachability problems for a planar manipulator 15 -0.5-0.4-0.3-0.2-0.10-0.1 0 0.1 0.2 0.3 0.4 (a.1) -20-15-10-505101520 0 0.2 0.4 0.6 0.8 1 (b.1) -0.5-0.4-0.3-0.2-0.10-0.1 0 0.1 0.2 0.3 0.4 (a.2) -20-15-10-505101520 0 0.2 0.4 0.6 0.8 1 (b.2) -0.5-0.4-0.3-0.2-0.10-0.1 0 0.1 0.2 0.3 0.4 (a.3) -20-15-10-505101520 0 0.2 0.4 0.6 0.8 1 (b.3) -0.5-0.4-0.3-0.2-0.10-0.1 0 0.1 0.2 0.3 0.4 (a.4) -20-15-10-505101520 0 0.2 0.4 0.6 0.8 1 (b.4) -0.5-0.4-0.3-0.2-0.10-0.1 0 0.1 0.2 0.3 0.4 (a.5) -20-15-10-505101520 0 0.2 0.4 0.6 0.8 1 (b.5) -0.5-0.4-0.3-0.2-0.10-0.1 0 0.1 0.2 0.3 0.4 (a.6) -20-15-10-505101520 0 0.2 0.4 0.6 0.8 1 (b.6) Fig. 3.
Figures labeled with (a. (cid:93) ) represent the solution q of Test (cid:93) , in (b. (cid:93) ) the relatedsigned curvature κ ( s ) (bold line) and curvature constraints ± ¯ ω (thin lines).8. George Thuruthel, T., Ansari, Y., Falotico, E., Laschi, C.: Control strategies forsoft robotic manipulators: A survey. Soft robotics (2), 149–163 (2018)9. Jones, B.A., Walker, I.D.: Kinematics for multisection continuum robots. IEEETransactions on Robotics (1), 43–55 (2006)10. Kang, R., Kazakidi, A., Guglielmino, E., Branson, D.T., Tsakiris, D.P., Ekateri-naris, J.A., Caldwell, D.G.: Dynamic model of a hyper-redundant, octopus-like ma-nipulator for underwater applications. In: Intelligent Robots and Systems (IROS),2011 IEEE/RSJ International Conference on. pp. 4054–4059. IEEE (2011)6 S. Cacace et al.11. Lai, A.C., Loreti, P.: Robot’s hand and expansions in non-integer bases. DiscreteMathematics & Theoretical Computer Science (1) (Jun 2014)12. Lai, A.C., Loreti, P., Vellucci, P.: A fibonacci control system with applicationto hyper-redundant manipulators. Mathematics of Control, Signals, and Systems (2), 15 (2016)13. Laschi, C., Cianchetti, M., Mazzolai, B., Margheri, L., Follador, M., Dario, P.: Softrobot arm inspired by the octopus. Advanced Robotics (7), 709–727 (2012)14. Laschi, C., Cianchetti, M.: Soft robotics: new perspectives for robot bodyware andcontrol. Frontiers in bioengineering and biotechnology , 3 (2014)15. Lions, P.L.: Generalized solutions of Hamilton-Jacobi equations, vol. 69. Pitman(1982)16. Michalak, K., Filipiak, P., Lipinski, P.: Multiobjective dynamic constrained evo-lutionary algorithm for control of a multi-segment articulated manipulator. In:International Conference on Intelligent Data Engineering and Automated Learn-ing. pp. 199–206. Springer (2014)17. Rus, D., Tolley, M.T.: Design, fabrication and control of soft robots. Nature (7553), 467–475 (2015)18. Sethian, J.A.: A fast marching level set method for monotonically advancing fronts.Proceedings of the National Academy of Sciences (4), 1591–1595 (1996)19. Uteshev, A.Y., Goncharova, M.V.: Point-to-ellipse and point-to-ellipsoid distanceequation analysis. Journal of computational and applied mathematics , 232–251 (2018)20. Wang, B., Wang, J., Zhang, L., Zhang, B., Li, X.: Cooperative control of het-erogeneous uncertain dynamical networks: An adaptive explicit synchronizationframework. IEEE transactions on cybernetics47