Frederic Boyer
email: frederic.boyer@imt-atlantique.fr

Member IEEE Vincent Lebastard
email: vincent.lebastard@imt-atlantique.fr.

Fabien Candelier
email: fabien.candelier@univ-amu.fr.

Member IEEE Federico Renda
email: federico.renda@ku.ac.ae.

M Alami

Statics and dynamics of continuum robots based on Cosserat rods and optimal control theories

This paper explores the relationship between optimal control and Cosserat beam theory from the perspective of solving the forward and inverse dynamics (and statics as a subcase) of continuous manipulators and snake-like bioinspired locomotors. By invoking the principle of minimum potential energy, and the Gauss principle of least constraint, it is shown that the quasi-static and dynamic evolution of these robots, are solutions of optimal control problems (OCPs) in the space variable, which can be solved at each step (of loading or time) of a simulation with the shooting method. In addition to offering an alternative viewpoint on several simulation approaches proposed in the recent past, the optimal control viewpoint allows us to improve some of them while providing a better understanding of their numerical properties. The approach and its properties are illustrated through a set of numerical examples validated against a reference simulator.

I. INTRODUCTION

The relationships between optimal control theory (OCT) and statics of Cosserat rods are well known from the geometric control community [START_REF] Jurdjevic | Non-euclidean elastica[END_REF]. In particular, it has been shown that the kineto-static model of a Cosserat rod consists of the Euler-Lagrange equations of an optimal control problem where the cost function is defined by its potential energy, the time by its arc length, and the optimal trajectories by the equilibrium configurations of the rod. Since the configuration space of a Cosserat rod is defined as a space of curves on SE(3), the optimal control theory of left-invariant systems on Lie groups must be used to relate the two theories intrinsically [START_REF]Geometric Control Theory[END_REF]. As an illustration of this relationship, the search for deformed configurations of a Cosserat rod manipulated quasi-statically at both ends [START_REF] Bretl | Quasi-static manipulation of a kirchhoff elastic rod based on a geometric analysis of equilibrium configurations[END_REF], is equivalent to that of optimal trajectories of a space vehicle on SE(3) between two time-varying pauses [START_REF] Walsh | Optimal path planning on matrix lie groups[END_REF]. In OCT, once the necessary first-order optimality conditions are deduced from the maximum principle, they define a Boundary Value Problem (BVP), whose solutions are the optimal trajectories that can be computed by various numerical technics, such as the shooting method [START_REF] Anil | A survey of numerical methods for optimal control[END_REF]. Thus, once formulated as an OCP, the kineto-statics of a Cosserat rod can be solved at each step of a quasi-static simulation by applying the shooting method to its BVP. Beyond static modelling and quasi-static simulation of rods, further stability issues related to buckling [START_REF] O'reilly | On stability analyses of three classical buckling problems for the elastic strut[END_REF], can be addressed with the second-order (sufficient) conditions around the optimal configurations, solutions of the first-order (necessary) ones [START_REF] Bliss | Lectures on the calculus of variations[END_REF]. With the emergence of continuous robotics, Cosserat rod theory has progressively imposed itself as one of the standards for modelling robots composed of rods actuated at their boundaries, or along them in a distributed way [START_REF] Boyer | Macro-continuous computed torque algorithm for a three-dimensional eel-like robot[END_REF], [START_REF] Trivedi | Geometrically exact models for soft robotic manipulators[END_REF], [START_REF] Dupont | Design and control of concentric-tube robots[END_REF], [START_REF] Rucker | A geometrically exact model for externally loaded concentric-tube continuum robots[END_REF], [START_REF] Rucker | Statics and dynamics of continuum robots with general tendon routing and external loading[END_REF], [START_REF] Renda | Dynamic model of a multibending soft robot arm driven by cables[END_REF]. In the first category, one finds the concentric tube robots (CTR) and continuum parallel robots (CPR), while the second includes tendon actuated continuum robots (TACR) [START_REF] Rucker | Statics and dynamics of continuum robots with general tendon routing and external loading[END_REF], soft robots equipped with pressure chambers [START_REF] Marchese | Design, kinematics, and control of a soft spatial fluidic elastomer manipulator[END_REF], or hyper-redundant swimming robots inspired of slender animals such as snakes and fish [START_REF] Boyer | Macro-continuous computed torque algorithm for a three-dimensional eel-like robot[END_REF]. As an alternative to Lagrangian (variational) approaches such as the geometrically exact FEM [START_REF] Simo | A finite strain beam formulation. the three-dimensional dynamic problem. part i[END_REF], the discrete rod formulation [START_REF] Bergou | Discrete elastic rods[END_REF], or the strain-based parametrization [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF], the vast majority of the continuous robotics community simulates these systems by formulating their model as BVPs in the spatial variable (arc length), which are then solved at each step of the simulation by the shooting method.

OCT has been successfully applied to statics of CTRs and CPRs to study the difficult problem of their stability with second order conditions [START_REF] Ha | Elastic stability of concentric tube robots subject to extemal loads[END_REF], [START_REF] Till | Elastic stability of Cosserat rods and parallel continuum robots[END_REF]. However, to the best of the authors' knowledge, it has so far never been applied to the dynamics of continuous robots, while it does not seem to be mentioned in the statics of distributed actuated robots. In particular, the BVPs addressed so far in these contexts with the shooting method by the robotics community, seem not to derive from optimal control problems (OCPs), but rather to be deduced by an adhoc combination of equations based on beam kinematics and Newton's laws [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF].

It is one of the main contributions of this paper, to show how, and to what extent, it is possible to model and simulate the statics and dynamics of continuous (possibly distributed actuated) robots, by deriving their BVPs systematically with the OCT, and solving them numerically with the shooting method. To illustrate our point and initiate future work, we will focus on the case of a one-piece robot which can be a manipulator (fixed at one end and free at the other) actuated by tendons, or a slender locomotor (free at both ends) actuated internally by an idealized model of the muscular activity of elongated vertebrates [START_REF] Tytell | Interactions between internal forces, body stiffness, and fluid environment in a neuromechanical model of lamprey swimming[END_REF], a case never approached before with this numerical method. In statics, the expected extension of OCT can be achieved by modeling the distributed actuation as a stress field governed by an actuated constitutive law, as proposed in [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]. In dynamics, we will see that the forward dynamic problem of a continuous robot (manipulator or locomotor), can be reformulated as an OCP by invoking one of the founding principles of dynamics originally introduced by Gauss [START_REF] Gauss | Uber ein neues allgemeines grundgesetz der mechanik[END_REF], and integrated into the framework of rational mechanics by Gibbs [START_REF] Gibbs | On the fundamental formulae of dynamics[END_REF] and Appell [START_REF] Appell | Sur une forme générale des équations de la dynamique[END_REF]. Nowadays known as Gauss' Principle of least constraint, the role of this principle in the dynamics of rigid manipulators has been revealed in [START_REF] Damaren | The relationship between recursive multibody and discrete-time optimal control[END_REF]. Remarkably, for these discrete systems, the principle leads to a linearquadratic OCP (LQ-OCP), that once solved with the socalled sweep method [START_REF] Bryson | Applied Optimal Control[END_REF], provides Featherstone's forward dynamic algorithm of rigid multibody systems [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF]. More generally, it is possible to show that all Newton-Euler's (NE) dynamics of rigid discrete systems (from models to algorithms), are, in fact, based on Gauss principle and OCT. From this view point, a further conceptual contribution of the article consists in relating OCT and Newton-Euler dynamics in the context of continuum robotics. In particular, we will show that, as in the case of discrete rigid systems, the dynamic BVPs derived with OCT naturally contain a continuous model of the accelerations as one of the Euler-Lagrange equations, a model that is generally omitted in previous work on the shooting-based approach, and yet plays a non-negligible role in simulation.

In addition to these modeling contributions, the OC perspective will provide new insight into the numerical properties of the shooting-based approaches applied to continuous robots. In particular, we will see that the LQ-OCP underpinned by the Gauss principle applied to a continuous robot is singular, and that it can be regularized using an implicit time integration scheme. Note that a similar numerical strategy has been introduced in the ocean engineering community for towed submarine cables [START_REF] Sun | Modelling and simulation of low-tension oceanic cable/body deployment[END_REF] and more recently, to solve the dynamics of continuous manipulators [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF]. However, the OC point of view will show that such a regularization process is intrinsically limited by a certain critical value of the time step below which, the singularity re-expresses itself and the approach aborts, and that this critical value increases as the robot becomes increasingly soft. These numerical considerations and others are illustrated on a set of numerical benches related to fixed and floating base rods, for which an implicit geometric integration scheme on the Lie group SO(3) × R 3 is proposed. These benches are compared to a reference simulator based on the Lagrangian approach of [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]. Finally, the paper ends with the case of the forward dynamics of a bio-inspired swimmer, and thus completes the past results of the authors devoted to the inverse dynamics of such systems [START_REF] Boyer | Macro-continuous computed torque algorithm for a three-dimensional eel-like robot[END_REF], while opening new perspectives for bio-inspired robotics and bio-mechanics of swimming.

The article is structured as follows. We first remind in section 2 some of the key concepts of Cosserat rods theory used in the article. In section 3, the model of distributed actuation (tendons, muscles) is also reminded and defined as a field of internal stress of a generalized active constitutive law. Based on this model, in section 4, the statics and dynamics of a single piece continuum robot are formulated in the framework of OCT, with the principle of minimum potential energy, and the Gauss least constraint principle, respectively. The BVPs provided by these different OCPs are then solved with the shooting method in section 5. While all these developments concentrate on the forward static and dynamic problems, the inverse problem is addressed in section 6 as a byproduct of the approach. In section 7, the approach is illustrated through several numerical examples. II. REMINDER OF COSSERAT ROD THEORY 1) Configuration and twists: We here consider an elliptic cross-sectional1 elastic rod subject to finite displacements and small strains2 (see Figure 1). In the Cosserat approach, such a medium is modeled by a continuous set of rigid cross sections stacked along a material line and labelled by a coordinate X ∈ [0, 1]. To each X-cross section, a mobile cross-sectional frame F(X) is attached. These frames are located on the center of the cross sections with their first vector normal to them and the two others aligned with the elliptic axes. In this context, the beam configuration space is naturally defined as:

C = {g : X ∈ [0, 1] → g(X) ∈ SE(3)}, (1)
which stands for a functional space of curves in SE(3), where g(X) = (R, r)(X) is represented by a 4 × 4 homogeneous transformation matrix (with R(X) ∈ SO(3), and r(X) ∈ R 3 ), parameterizing the pause of F(X) in F s . Throughout the article, we will consider continuum manipulators or locomotors. In the first case, the proximal cross section (X = 0) is clamped and g(0) = 1 4×4 , while in both cases, the tip cross-section (X = 1) is free to move. Noting ∂./∂X and ∂./∂t by a "prime" and a "dot", the field g depending on both X and t, its space-time variations are described by the two vector fields η and ξ from [0, 1] to se(3) ∼ = R 6 (see [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF] for standard Lie group notations):

η = (g -1 ġ) ∨ , ξ = (g -1 g ′ ) ∨ , (2)
where

η = (Ω 1 , Ω 2 , Ω 3 , V 1 , V 2 , V 3 ) T = (Ω T , V T ) T
stands for the field of the velocity twists of the cross-sections in their mobile frames, while ξ

= (K 1 , K 2 , K 3 , Γ 1 , Γ 2 , Γ 3 ) T = (K T , Γ T )
T is the exact geometrical counterpart of η when replacing t by X (see Figure 1). Since X is a material label, it is independent of t, and we have by symmetry of derivatives of transformations:

(g ′ ) = ( ġ) ′ ⇒ ξ = η ′ + ad ξ η. (3)
Replacing t in the definition of the velocity twist η, by any real parameter ε, independent of t and X, allows defining what we name a variation δg = gδ ζ of the rod configuration. Such a δg generates on a functional f of g, a variation δf defined for all X by:

δf (g(X)) = d dε ε=0 f g(X) exp(εδ ζ(X)) . (4)
The parameter ε of a δ-variation being independent of X, a similar commutation relation to (3) holds where the timederivation ∂/∂t, is simply replaced by the variation δ:

δ(g ′ ) = (δg) ′ ⇒ δξ = δζ ′ + ad ξ δζ. (5)
Relations ( 3) and ( 5) are due to Poincaré [START_REF] Poincaré | Sur une forme nouvelle des equations de la mecanique[END_REF] and play a key role in the modern Euler-Poincaré Lagrangian reduction theory [START_REF] Marsden | Introduction to Mechanics and Symmetry[END_REF]. In this context, they allow applying Hamilton's variational principle to Cosserat rods directly on the configuration space [START_REF] Jurdjevic | Non-euclidean elastica[END_REF], where δζ = (g -1 δg) ∨ are then, some virtual displacements of the rod configuration [START_REF] Boyer | Poincare's equations for cosserat media: Application to shells[END_REF]. In section IV.A, devoted to statics, we adopt the view point of optimal control theory. In this context, we will also use configuration variations δζ, for which (5) holds. However, in contrast to Lagrangian mechanics, these variations are not directly applied to the configuration of the rod as this is the case of virtual displacements, but are the indirect consequences of some variations of its strain field.

2) Strains and stress: The Cosserat rod model can capture several rod sub-models (e.g. Kirchhoff) by allowing only a subset of the components of ξ to be free. Referring to [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF], these allowed components are gathered in a n a × 1 (n a ≤ 6) vector field noted ξ a , while the other components (that are constrained to some constant values), defines a complementary n c × 1 field, noted ξ c , with n c = 6 -n a . With these definitions in hands, we have the partition of ξ:

ξ = Bξ a + Bξ c , (6)
where B and B are two selection matrices of dimension 6 × n a and 6 × n c , with entries equal to zero or one, while ξ c is a constant vector field whose components (zero or constant), depend on the adopted rod kinematics (e.g. for a Kirchhoff rod, B = (1 3×3 , 0 3×3 ) T , B = (0 3×3 , 1 3×3 ) T , and if X is the arc length along the rod ξ c = (1, 0, 0) T ). If g o defines a reference stress-less configuration of the rod with space-twist field ξ o = (g -1 o g ′ o ) ∨ , a linear strain measure is defined by the field:

ϵ = ξ a -ξ a,o = B T (ξ -ξ o ). ( 7
)
Combining ( 6) and ( 7), one can express the field ξ and its time derivatives, in terms of strains as:

ξ = B(ϵ + ξ a,o ) + Bξ c = Bϵ + ξ o , ξ = B ξa = B ε , ξ = B ξa = Bε. ( 8
)
The stress state is described by a field of wrench Λ :

X ∈ [0, 1] → Λ(X) = (C T , N T ) T (X)
, where C = (C 1 , C 2 , C 3 ) T , and N = (N 1 , N 2 , N 3 ) T , are the moment and the resultant of the internal tension forces transmitted from right (Y > X), to left (Y < X), across the X-cross section, and expressed in its mobile frame (see Figure 1). According to [START_REF] O'reilly | On stability analyses of three classical buckling problems for the elastic strut[END_REF], one can partition Λ as:

Λ = BΛ a + BΛ c , (9)
where Λ a = B T Λ, gathers the components of Λ acting along the allowed internal d.o.f of ξ a , while Λ c defines a set of Lagrange multipliers (internal forces and/or reaction torques) in charge of forcing the internal stresses BT ξ = ξ c .

3) Forces and energies: In statics, we only consider the case of a manipulator subject to a density of wrench F and a tip wrench F + applied along its length, and at its distal end respectively. In all the subsequent developments, we assume F and F + to be prescribed by some explicit functions of time and/or defined by a state-dependent model, where the state is g in statics, and (g, η) in dynamics. In statics, a force F (g) exerted on a single cross section of pause g, is said to be conservative if it derives from a potential, i.e. if there exists a function U ext of SE(3) in R, such that:

δζ T F (g) = -δU ext , (10)
where δU ext is defined by instantiating f by U ext in (4). In the following developments on statics, F + and F are assumed to be conservative, i.e. it exists U ext for the entire rod, such that:

δU ext = 1 0 δ Ū dX + δU + , (11)
with:

δ Ū = -δζ T F , δU + = -δζ(1) T F + . (12)
Assuming the rod to be elastic, and subject to small strains, its internal (potential) energy is defined as the integral over the rod of a quadratic form of the strains:

U int = 1 0 U int dX = 1 2 1 0 ϵ T H r ϵ dX, (13)
where H r = B T diag(GJ 1 , EJ 2 , EJ 3 , EA, GA, GA)B is the reduced n a × n a Hooke matrix of the rod. Moreover, in this case, the field of stress Λ a is also conservative, and so derives from the potential (13), i.e. we have:

Λ a = B T Λ = ∂U int ∂ϵ = H r ϵ. (14)
Similarly, the kinetic energy of the rod is:

T = 1 2 1 0 η T Mη dX, (15)
where M = diag(ρJ 1 , ρJ 2 , ρJ 3 , ρA, ρA, ρA) = diag(ρJ, ρA1 3×3 ), is the field of 6 × 6 inertia matrix of its cross-sections.

III. CONTINUUM ROBOTS AS INTERNALLY ACTUATED COSSERAT RODS

We consider a continuum robot as an elastic slender body equipped with some active devices able to exert some forces on it. The body is modelled as a Cosserat rod, actuated with "external" or "internal" actuators, depending whether the forces they exert on the robot, are supported by the external environment, or by the constitutive material of the rod itself. As emblematic examples of each category, rods equipped with magnetic dipoles in interaction with a controlled external magnetic field are externally actuated. On the other hand, a continuum manipulator consisting of a rod made with a piezoelectric material controlled in voltage is internally actuated. Beyond robotics, the muscles contractions that bend the slender body of a snake or an eel, define an internal actuation. To derive the model of a continuum robot, it suffices to modify the model of (passive) Cosserat rods in two different ways, depending on the nature of actuation. If it is external, we add to the external wrenches F (gravity, contact..), a field of external actuation wrench Fd . If it is internal, we add to the field of elastic stress B T Λ = Λ a = H r ϵ, a further exogenous active component Λ ad modelling the actuation. In this case, the passive constitutive law ( 14) is changed into the active/passive one [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]:

B T Λ = Λ ad + H r ϵ. (16)
Physically, when the rod kinematics are not constrained (i.e. B = 1 6×6 ), [START_REF] Bergou | Discrete elastic rods[END_REF] simply means that if we (virtually) cut a slender continuous robot into two parts at any point X along its dominant length, the part of the robot Y > X exerts on the part Y < X a wrench defined by (16) evaluated in X, i.e., with two components, one (H r ϵ) modeling the elastic restoring forces and torques of the structure, the other (Λ ad ) modeling the effects of any internal actuating device transmitting forces along it. Remarkably, some continuous manipulators such as TACRs can be classified in both categories, i.e. the actuation forces can be modeled either by modifying F or by using a constitutive law of the form [START_REF] Bergou | Discrete elastic rods[END_REF]. To illustrate this context, let us consider a TACR shape controlled by pulling a set of N tendons of negligible inertia, elasticity and friction. In the first view point (external), Newton's law are first applied to the tendons and the rod alone, and action-reaction is then used to get the field of external wrench exerted by each tendon onto the rod [START_REF] Rucker | Statics and dynamics of continuum robots with general tendon routing and external loading[END_REF].

In the second (internal), the rod and tendons are considered as a single system to which Newton [START_REF] Renda | Screw-based modeling of soft manipulators with tendon and fluidic actuation[END_REF], or Lagrangian mechanics is applied [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]. In this later context, invoking kinematic invariance of virtual works between the space of tendon lengths and that of Cosserat strains, provides the expression of the internal stress-wrench field Λ ad as a function of the tensions T i along the tendons (the subscript "c" here means "cable"):

Λ ad (X, t) = N i=1 B T D i × Γ c,i Γ c,i T i (t) ∥Γ c,i ∥ , ( 17
)
where we introduced the notation Γ c,i = Γ + K × D i + D ′ i , with D i (X) the position of the intersection point of cable i with the X-cross section of the rod, in the X-cross-sectional frame. Note that although Λ ad depends on ϵ due to the presence of K in Γ c,i , more detailed computations show that this dependence vanishes as soon as the routings of the cables are parallel with the rod backbone, while due to small radius of TACRs, they are very small otherwise. In the following, the case of a TACR in which the ϵ-dependency of Λ ad is neglected, is used as an emblematic example of the approach. However, beyond this archetypal example, all the subsequent results can be applied to any continuous slender robot whose internal stress (elastic and actuated) can be modeled by a nonlinear version B T Λ = f (ϵ, τ (t)) of ( 16), with τ (t) a finite vector of control inputs (e.g., tendon tensions, chamber pressures...). The key to extending the approach to this broader context will be provided in a future remark numbered 4, the goal of which is to capture the ϵ dependence of (17) throughout the approach 3 . In summary, apart from remark 4, a continuum robot will in the following be considered as a Cosserat rod internally actuated by [START_REF] Bergou | Discrete elastic rods[END_REF], where Λ ad is a dependent function of (X, t) modeling a technological action, or an idealization of the muscular activity of a slender animal.

IV. FORWARD STATICS AND DYNAMICS OF CONTINUUM ROBOTS AS OCPS

We now address the forward statics and dynamics of continuum robots as some optimal control problems (OCP). For obvious reasons, the case of a locomotor is only considered in dynamics. In statics, the cost functional is the potential energy of the rod, in dynamics, it is the Gauss constraint.

A. Statics of a continuum robot

In statics, a material system subject to conservative forces is governed by the principle of minimum potential energy, which states that among all the configurations accessible to the system, the equilibrium configuration it occupies must make its potential energy stationary. In the case of a continuum manipulator subject to external and internal forces governed by [START_REF] Rucker | Statics and dynamics of continuum robots with general tendon routing and external loading[END_REF] and ( 16) respectively, its potential energy takes the form:

U = U int +U ext = 1 0 1 2 ϵ T H r ϵ+ϵ T Λ ad + Ū dX +U + . ( 18
)
Now, giving to the strain ϵ the role of a control variable u, and to g, that of a state variable, the principle of minimum potential energy can be reformulated as the following OCP, where the space variable X replaces the usual time in control:

• OCP1: Find the optimal control u that makes stationary the cost functional defined by the potential energy:

C(u) = 1 0 1 2 u T H r u + u T Λ ad + Ū dX + U + (19)
under the constraints:

g ′ = g(Bu + ξ o ) ∧ ⇔ (g -1 g ′ ) ∨ = (Bu + ξ o ) ⇔ ξ = Bu + ξ o , (20)
with g(0) = 1 4×4 , due to the fixed root cross-section • While [START_REF] Till | Elastic stability of Cosserat rods and parallel continuum robots[END_REF] defines the potential energy of the continuum manipulator, [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF] simply describes how the pause cross sections evolve with X. In particular, [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF] accounts for the internal kinematic constraints imposed by the choice of the selection matrix B, as its inextensibility or unshearibility. Since the control is unbounded, the necessary conditions that any optimal u needs to fulfill can be derived by applying usual variational calculus on the Lie group SE(3), to the augmented cost functional:

C + = 1 0 1 2 u T H r u+u T Λ ad + Ū+(ξ-ξ o -Bu) T Λ dX+U + , (21)
with Λ a field of Lagrange multipliers. This approach avoids the (Hamiltonian) Lie-Poisson reduction, in favor of the (Lagrangian) Euler-Poincaré one [START_REF] Marsden | Introduction to Mechanics and Symmetry[END_REF]. In this setting, the optimal control must satisfy the stationarity condition of the functional C + :

δC + = 1 0 δu T (Λ ad + H r u) dX+ (22)
+ 1 0 δ Ū + (δξ -Bδu) T Λ dX + δU + = 0
, where for any function f (g), variations δf are consequences of variations δu in [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF]. Now, since these variations do not affect X, they generate some pose variations δζ = (g -1 δg) ∨ which satisfy [START_REF] Anil | A survey of numerical methods for optimal control[END_REF], and ( 22) can be rewritten:

δC + = 1 0 δu T (Λ ad + H r u) + δ Ū dX+ 1 0 (δζ ′T + δζ T ad T ξ -Bδu) T Λ dX + δU + = 0, (23)
which gives, after by-part integration and with the definition of conservative external forces [START_REF] Rucker | Statics and dynamics of continuum robots with general tendon routing and external loading[END_REF]:

0 = 1 0 δu T (Λ ad + H r u -B T Λ)dX (24)
-

1 0 δζ T (Λ ′ -ad T ξ Λ + F )dX + δζ T (1)(Λ(1) -F + )
, where we used the fact that the BC at X = 0, imposes δζ(0) = (g -1 δg) ∨ (0) = 0. It would be tedious to determine the variation δζ produced by a given δu, so we choose Λ to cause the coefficients of δζ in [START_REF] Appell | Sur une forme générale des équations de la dynamique[END_REF] to vanish [START_REF] Bryson | Applied Optimal Control[END_REF]. After this choice, [START_REF] Appell | Sur une forme générale des équations de la dynamique[END_REF] only imposes the coefficient of δu to be zero, and the conditions of stationarity that any optimal trajectory must satisfy are (remind that ξ = (g -1 g ′ ) ∨ ):

• The two Euler-Lagrange equations :

g ′ Λ ′ = g(ξ o + Bu) ∧ ad T ξ Λ -F , (25)
• The optimality condition:

B T Λ = Λ ad + H r u, (26)
• The transversality condition:

Λ(1) = F + . (27)
Once supplemented with the geometric BC: g(0) = 1 4×4 , this set of equations defines a closed formulation of the statics of a continuum robot. Finally, solving this formulation, or solving OCP1, is equivalent. Achieving such a resolution, numerically at each loading step, i.e. when the internal actuation stress, the density of external wrench and the external wrench at the tip Λ ad , F and F + are updated, provides a simulation algorithm of the quasi-static evolution of a continuum manipulator.

Remark 1: In the passive case of Kirchhoff rods straight at rest, i.e. when Λ ad = 0, B = (1 3×3 , 0 3×3 ) T , and ξ 0 = (0 1×3 , 1, 0, 0) T , this formulation has been derived from optimal control theory of left-invariant systems on Lie groups in [START_REF]Geometric Control Theory[END_REF], and applied in robotics [START_REF] Bretl | Quasi-static manipulation of a kirchhoff elastic rod based on a geometric analysis of equilibrium configurations[END_REF] with a second geometric BC instead of ( 27), in order to address the difficult problem of controlling the shape of a rod manipulated at its two ends [START_REF] Wakamatsu | Knotting/unknotting manipulation of deformable linear objects[END_REF]. In continuum robotics it has been exploited in [START_REF] Ha | Elastic stability of concentric tube robots subject to extemal loads[END_REF] and [START_REF] Till | Elastic stability of Cosserat rods and parallel continuum robots[END_REF], in order to analyse the stability of concentric tubes robots (CTR), and continuum parallel robots (CPR) respectively, i.e. continuum robots constituted of several passive rods actuated by exerting localized torques and forces on their boundaries. In these two cases, the above first order optimality conditions are augmented of second order Legendre-Clebsh conditions, which once supplemented with a study of conjugate points (the so-called Jacobi conditions), allow to analyse the stability of these systems [START_REF] Levy | Classical mechanics with optimal control and variational calculus, an intuitive introduction[END_REF]. In the case of systems actuated in a distributed way, as TACRs, a similar BVP is considered for static simulation, but with actuation modelled as external forces (see section III), and with no reference to OC [START_REF] Rucker | Statics and dynamics of continuum robots with general tendon routing and external loading[END_REF].

B. Dynamics of a continuum robot

To extend the previous picture of statics to dynamics, we must introduce a little known variational principle proposed by Gauss in 1829 and known today as the "principle of least constraint". In words this principle can be stated as follows: Let us consider a constrained material system that has reached at a given time t its current state, then, among all the accelerations compatible with the constraints that the system can have at t, that it will have is the closest to that it would have if all the constraints would be instantaneously removed at that time [START_REF] Appell | Traité de mécanique rationnelle[END_REF]. Remarkably, this principle can ground dynamics as does Hamilton's principle in Lagrangian mechanics. However, in contrast to Hamilton's principle, the Gauss principle is not an extremal integral principle, but rather a minimal differential principle, that can be formally stated at any instant of the motion of a system subject to constraints (holonomic or not), in the form:

a = arg min ac 1 2 ∥a c -a f ∥ 2 K , (28)
where a c , a f ∈ R 3 stand for the acceleration field of the system, with, and without constraints respectively, while a stands for its actual acceleration field, and ∥.∥ K is the norm defined by its kinetic energy. The minimized functional is named the "Gauss constraint". Going further into details, consider a material system of volume element dv with mass density ρ, applying (28) to this system leads to:

a = arg min ac 1 2 D (a c -a f ) 2 ρ dv = arg min ac D ( 1 2 ρ a 2 c -a T c f )dv , (29)
where f = ρ a f denotes the volume density of all external and internal forces, except for the "reactive forces" produced by the constraints 4 , while the term a 2 f can be ignored since it does not depend on the optimization variable a c . In this equivalent formulation, the first integral term is named the "acceleration energy" by Gibbs and Appell [START_REF] Gibbs | On the fundamental formulae of dynamics[END_REF], [START_REF] Appell | Sur une forme générale des équations de la dynamique[END_REF], and here noted T acc . By extension, the opposite of the second term of (29) could be named the "acceleration power of external and internal non-reactive forces" and noted P acc . By "non-reactive" forces we here mean those which do not contribute to force the constraints. Practically, T acc and P acc can be easily computed by replacing first, the velocities by accelerations in the usual definitions of kinetic energy and (non-reactive) power, and second, by removing the terms that only depend on velocities, in the final expressions 5 . Applying these computational rules to a continuum robot, these definitions become (see Appendix 1):

T acc = 1 2 D ρ a 2 c dv = 1 0 1 2 ηT M η -ηT (ad T η Mη)dX, (30) and:
P acc = D a T c f dv = 1 0 ηT F + ξ T a (Λ ad -H r ϵ) dX + η(1) T F + . ( 31
)
if the robot is a manipulator, or:

P acc = D a T c f dv = 1 0 ηT F + ξ T a (Λ ad -H r ϵ) dX+ + η(0) T F -+ η(1) T F + , ( 32
)
if it is a locomotor. Note here, that the constraints being those imposed by the geometric BCs and the internal rod kinematics (e.g., inextensibility and unshearability of a Kirchhoff rod), the reactive forces they produce, do not appear in P acc . Applying the Gauss principle to the forward dynamics of continuum robot, leads to find at each time t of the robot motion, the strain acceleration ε that minimizes the functional:

1 2 D (a c -a f ) 2 ρ dv = T acc -P acc , (33)
with T acc and P acc defined by [START_REF] Marsden | Introduction to Mechanics and Symmetry[END_REF] and [START_REF] Boyer | Poincare's equations for cosserat media: Application to shells[END_REF][START_REF] Renda | Screw-based modeling of soft manipulators with tendon and fluidic actuation[END_REF], and where a c being compatible with Cosserat kinematics (see Appendix 1), η needs to fulfill the constraints on accelerations deduced by time-differentiating (3):

η′ = -ad ξ η -ad ξ η + Bε , η(0) = 0, (34)
where ε plays the role of a minimal set of accelerations parameterizing any acceleration field compatible with the internal constraints imposed by the rod model. Moreover, interpreting this field of acceleration strain as a control input u = ξa = ε, the principle turns to be an optimal control problem that can be stated as follows.

• OCP2: Find at each time t of the robot motion, the strain acceleration u that minimizes:

C(u) = 1 0 1 2 ηT M η -ηT ( F + ad T η Mη)dX + 1 0 u T (Λ ad -H r ϵ)dX -η(1) T F + , (35)
if the robot is a manipulator, or:

C(u) = 1 0 1 2 ηT M η -ηT ( F + ad T η Mη)dX + 1 0 u T (Λ ad -H r ϵ)dX -η(0) T F --η(1) T F + , (36)
if it is a locomotor. In all cases (manip. or locom.), this has to be done under the constraint:

η′ = -ad ξ η -ad ξ η + Bu , (37)
and the further one:

η(0) = 0, (38)
if the robot is a manipulator • Note that in this formulation, the mechanical state (g, η) of the robot is frozen at the current time t, which means that all the (g, η)-dependent functions of [START_REF] Levy | Classical mechanics with optimal control and variational calculus, an intuitive introduction[END_REF][START_REF] Appell | Traité de mécanique rationnelle[END_REF][START_REF] Athans | Optimal Control[END_REF] (including ϵ = B T (g -1 g ′ ) ∨ -ξ ao ), can be considered as some Xdependent functions. As a result η stands for the state vector of an optimal linear-quadratic (LQ)-OCP, in which, like in the static case, X plays the role of time. Using usual control notations, the linear state system6 is defined by [START_REF] Athans | Optimal Control[END_REF] and takes the form:

ẋ = A(t)x + B(t)u + c(t), (39)
with the correspondence between notations as indicated in table 1, and where the state-independent drift vector c, can be easily removed by a change of variable on the accelerations η as done in the discrete case in [START_REF] Damaren | The relationship between recursive multibody and discrete-time optimal control[END_REF]. As regards the quadratic cost [START_REF] Levy | Classical mechanics with optimal control and variational calculus, an intuitive introduction[END_REF] or [START_REF] Appell | Traité de mécanique rationnelle[END_REF], it is of the generic Bolza form [START_REF] Bryson | Applied Optimal Control[END_REF]:

C(u) = 1 0 1 2 x T M (t)x dt (40)
- 1 0 x T h(t) + u T f (t) dt + ϕ(x(0), x (1)
), where we used the notations of table 1. The control variable u being unbounded, one can still apply direct variational calculus to obtain the first order necessary conditions that any

OCP2 Gauss principle u ξa = ε t X h F + ad T η Mη x η f Λ ad -Hrϵ M M A -ad ξ B B ϕ (manip.) -η(1) T F + ϕ (locom.) -η(0) T F --η(1) T F + TABLE I: Table of correspondences
between the linear optimal control problem OCP2 and the Gauss "least constraint" principle.

optimal control u needs to fulfill [START_REF] Athans | Optimal Control[END_REF]. These conditions can be expressed with the (control) Hamiltonian of a continuum robot: [START_REF] Cardona | Time-integration of the equations of motion in mechanism analysis[END_REF] with L, the Lagrangian of the problem defined as the function under the integral of ( 35) or [START_REF] Appell | Traité de mécanique rationnelle[END_REF], and where, as in the static case, the stress field Λ defines the costate of the optimal problem. Then, remarking that the state η now belongs to a vector space, usual variational calculus provides the classical first order conditions [START_REF] Bryson | Applied Optimal Control[END_REF]:

H( η, Λ, u) = L( η, u) + Λ T (-ad ξ η -ad ξ η + Bu),
η′ = ∂H ∂Λ , Λ ′ = - ∂H ∂ η , ∂H ∂u = 0, (42)
as well as the transversality condition:

Λ(1) = ∂ϕ ∂ η(1) , (43)
for a manipulator, or:

Λ(0) = - ∂ϕ ∂ η(0) , Λ(1) = ∂ϕ ∂ η(1) , (44)
for a locomotor. From left to right, the two first equations of ( 42) are two Euler-Lagrange ODEs governing the state η and costate Λ vectors, while the third stands for an algebraic condition that an optimal control u must hold in any point of an optimal ( η, Λ) trajectory. Introducing (41) into these formulas, and changing the orientation of the stress, (i.e. Λ into -Λ and Λ ad into -Λ ad ), which is a matter of convention 7 , provides:

• The two Euler-Lagrange equations:

η′ Λ ′ = -ad ξ 0 M ad T ξ η Λ + -ad ξ η + Bu -ad T η Mη -F . ( 45
)
• The optimality condition:

B T Λ = Λ ad + H r ϵ. ( 46
)
7 Conventionally, in Cosserat theory, Λ(X) represents the wrench of the internal contact forces exerted by the piece of rod Y > X onto the piece Y < X, across the X-cross section, while in Gauss principle this is the opposite convention which holds.

• The transversality and geometric boundary conditions:

Λ(1) = F + , g(0) = 1 4×4 . ( 47
)
for a manipulator, or:

Λ(0) = -F -, Λ(1) = F + . (48)
for a locomotor. Solving this formulation is equivalent to solve OCP2, i.e. an optimal control problem, here based on Gauss principle.

Remark 2: The above formulation [START_REF] Holsapple | A modified simple shooting method for solving two-point boundary-value problems[END_REF][START_REF] Courant | On the partial difference equations of mathematical physics[END_REF][START_REF] Meirovitch | Dynamics and Control of structures[END_REF][START_REF] Boyer | Poincare cosserat equations for the lighthill three-dimensional large amplitude elongated body theory: Application to robotics[END_REF] holds at any fixed time. To use it continuously along a time interval, we need to add a further set of equations. To introduce this point, let us consider the problem of the dynamic simulation of a manipulator. If at a given time t, one has solved [START_REF] Holsapple | A modified simple shooting method for solving two-point boundary-value problems[END_REF][START_REF] Courant | On the partial difference equations of mathematical physics[END_REF][START_REF] Meirovitch | Dynamics and Control of structures[END_REF]. Then u(t) = ε(t) = ξa (t) is known and can be time-integrated twice with an explicit scheme, to give (ξ a , ξa )(t + ∆t). Then, to update (45-47) at t + ∆t (and resume the process), we need to reconstruct (g, η)(t + ∆t) from the knowledge of (ξ a , ξa )(t + ∆t). This is achieved by X-integrating from X = 0 to 1, the continuum kinematic models of transformations and velocities deduced from ( 2) and ( 3) respectively, and initialised with (g, η)(0) = (1 4×4 , 0):

g ′ η ′ = g ξ -ad ξ η + ξ , (49)
where remind that ξ = Bξ a + Bξ c and ξ = B ξa . As a result, to be used in a simulation time-loop, the above set of equations [START_REF] Holsapple | A modified simple shooting method for solving two-point boundary-value problems[END_REF][START_REF] Courant | On the partial difference equations of mathematical physics[END_REF][START_REF] Meirovitch | Dynamics and Control of structures[END_REF][START_REF] Boyer | Poincare cosserat equations for the lighthill three-dimensional large amplitude elongated body theory: Application to robotics[END_REF] needs to be supplemented with the reconstruction equations [START_REF] Lighthill | Aquatic animal propulsion of high hydromechanical efficiency[END_REF]. This is in contrast to OCP1 where the Euler-Lagrange equations contained all the information required by a (static) simulation, and can be explained by the fact that, while in statics the mechanical state and the control state are identical (and coincide with g), in dynamics, the control state is defined by the pose acceleration η, while the mechanical state is (g, η).

Remark 3: In bio-robotics, the formulation (45-49) with the reduced stiffness matrix H r = 0, has been used as a continuous Newton-Euler (NE) model, to inverse the dynamics of hyper-redundant locomotors inspired from fish [START_REF] Boyer | Macro-continuous computed torque algorithm for a three-dimensional eel-like robot[END_REF] and snakes [START_REF] Boyer | Macrocontinuous dynamics for hyperredundant robots: Application to kinematic locomotion bioinspired by elongated body animals[END_REF]. As for discrete rigid multibody systems (MBS), such a model consists of the NE equations of the rigid bodies (here the cross sections), and of recursions (here ODEs) on their pauses, velocities and accelerations 8 . In Section VI, a new solution to the inverse dynamics of continuous robots will be given. In [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF], a slightly different formulation is extensively used for the simulation of continuum robots. This formulation differs from the one derived above in that the model of η accelerations (the upper ODE of ( 45)) is ignored, while distributed actuation (e.g., by tendons) is not modeled by Λ ad , but rather by F , as discussed in Section III. Inherited from the ocean engineering community interested in submarine cables [START_REF] Burgess | Bending stiffness in a simulation of undersea cable deployment[END_REF], this alternative dynamic formulation does not derive from an OCP. Before addressing the resolution of OCP1 and OCP2, the role of mathematical relations and objects from the point of view of optimal control, are listed in statics and dynamics in Table 2, which thus allows the two contexts to be compared.

η ∈ se(3) ∼ = R 6 Costate Λ ∈ se(3) ⋆ ∼ = R 6 Λ ∈ se(3) ⋆ ∼ = R 6 State-eq. ( 25

V. RESOLUTION OF THE OCPS FOR SIMULATION OF CONTINUUM ROBOTS

Using indirect methods for solving OCPs [START_REF] Anil | A survey of numerical methods for optimal control[END_REF], the resolution progresses in two stages. The first consists in using the optimality condition to remove u from the state and costate equations. Once this is achieved, these two equations define an autonomous BVP, which can be solved by different numerical technics (collocation, shooting method, finite differences...). This numerical resolution defines the second stage of the resolution of the OCP. In fine, the optimality condition can be reused in an inverse way in order to compute u. We are now going to see how this resolution method can be applied to static and dynamic simulation of continuum robots. In section V.A and V.B we first apply the first stage of the approach in order to produce some autonomous BVPs for OCP1 and OCP2, whose numerical resolution is addressed with shooting method in section V.C.

A. Autonomous BVP for OCP1

In the static case, the above approach can be applied with no difficulty. Indeed, using the condition [START_REF] Bryson | Applied Optimal Control[END_REF] provides the elimination relation:

B T Λ = Λ ad + H r u ⇒ u = H -1 r (B T Λ -Λ ad ), (50)
which once introduced in (25) provides the autonomous BVP:

g ′ Λ ′ = g(BH -1 r (B T Λ -Λ ad ) + ξ o ) ∧ ad T ξ Λ -F , (51)
with BCs:

g(0) = 1 4×4 , Λ(1) = F + . (52)
Finally, to achieve the resolution of OCP1 governing statics of a manipulator, this nonlinear BVP needs to be numerically solved, e.g. with the shooting method as detailed in the section V.C.

Remark 4: When Λ ad depends on u = ϵ, the optimality condition (50) becomes an implicit relation and one cannot directly use it to eliminate u in the BVP. In such a case, one could use a further implicit solver, or more simply, reexpress the BVP [START_REF] Damaren | The relationship between recursive multibody and discrete-time optimal control[END_REF][START_REF] Bryson | Applied Optimal Control[END_REF][START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF] in terms of strains ϵ, instead of stress Λ, by directly introducing the full constitutive law [START_REF] Trivedi | Geometrically exact models for soft robotic manipulators[END_REF][START_REF] Bergou | Discrete elastic rods[END_REF]:

Λ = B(Λ ad + H r ϵ) + BΛ c , (53)
in the Euler-Lagrange equations [START_REF] Damaren | The relationship between recursive multibody and discrete-time optimal control[END_REF]. Then, remarking that Λ ad depends on X both explicitly and implicitly through ϵ, we have:

Λ ′ ad = (∂ ϵ Λ ad )ϵ ′ + ∂ X Λ ad . (54)
Using this relation as well as the complementarity relations on selection matrices B T B = ( BB T ) T = 0 na×nc , B T B = 1 na×na , BT B = 1 nc×nc , simple projections on the spaces of allowed and constrained stress, change [START_REF] Damaren | The relationship between recursive multibody and discrete-time optimal control[END_REF][START_REF] Bryson | Applied Optimal Control[END_REF] into the alternative equivalent form:

  g ′ Λ ′ c ϵ ′   =   g(ξ o + Bϵ) ∧ BT (ad T ξ Λ -F ) H-1 r (B T (ad T ξ Λ -F ) -F )   , (55)
where Hr = (H r +∂ ϵ Λ ad ), F = H ′ r ϵ+∂ X Λ ad , ξ and Λ are given by ( 8) and (53) respectively, while [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF] now becomes:

ϵ(1) = H r (1) -1 (B T F + -Λ ad (1)) , Λ c (1) = BT F + .
Finally, note that (55) is now an explicit differential system with respect to (g, Λ c , ϵ), and that such a process can be used for any continuous manipulator governed by an actuated nonlinear constitutive law of the form B T Λ = f (ϵ, τ (t)), where ∂f /∂ϵ is invertible, and τ (t) defines a set of timevarying control inputs.

B. Autonomous BVP for OCP2

1) Application of LQ optimal control theory to OCP2: A first approach, strictly consistent with Gauss principle, would consist in addressing the problem [START_REF] Levy | Classical mechanics with optimal control and variational calculus, an intuitive introduction[END_REF][START_REF] Appell | Traité de mécanique rationnelle[END_REF][START_REF] Athans | Optimal Control[END_REF][START_REF] Boyer | Macrocontinuous dynamics for hyperredundant robots: Application to kinematic locomotion bioinspired by elongated body animals[END_REF] as a LQ optimal control problem. However, it is worth noting that although C is quadratic with respect to the control state, it is only linear with respect to u. As a result, [START_REF] Courant | On the partial difference equations of mathematical physics[END_REF] does not provide the expression of u, and this LQ-OCP is said to be singular (see Appendix 2). Indeed, the expected dependence requires to differentiate the optimality condition of (42) twice w.r.t. to X which gives:

u = M -1 aa [(Λ ad + H r ϵ) ′′ -B T (D η + EΛ + I)], (56)
where M aa , D, E, and I are X-dependent matrices defined in Appendix (see eq.( 107)). Using (56) in [START_REF] Holsapple | A modified simple shooting method for solving two-point boundary-value problems[END_REF] provides an autonomous BVP. In contrast to the static case, this BVP is linear with respect to ( η, Λ) and it could be solved with some specific methods as the sweep method [START_REF] Bryson | Applied Optimal Control[END_REF], which avoids the iterations of the shooting method by postulating the existence of a linear relationship between state and costate:

∀X ∈ [0, 1] : Λ(X) = S(X) η(X) + s(X), (57)
where S and s define a field of 6 × 6 matrix and 6 × 1 vector respectively, both solutions of a set of Riccati ODEs given in Appendix 2. In spite of its apparent simplicity (due to linearity), this approach suffers from several drawbacks. First, the singular nature of the OCP requires further derivations of the constitutive law, which can be numerically difficult to implement. Second, (56) is ill-conditioned which makes the Riccati equations unstable. At last, the approach provides ξa , which then would require to be integrated with an explicit time-integrator (see remark 5), i.e. with integration schemes that are known to be less stable (than implicit schemes), when applied to the simulation of nonlinear structural dynamics.

Remark 5: D'Eleuterio and Damaren have shown in [START_REF] Damaren | The relationship between recursive multibody and discrete-time optimal control[END_REF] that the Newton-Euler forward dynamics algorithm of Featherstone [START_REF] Featherstone | The calculation of robot dynamics using articulatedbody inertias[END_REF], which allows to find the joint accelerations of a rigid multibody system from the knowledge of its joint state and torque variables, is the solution of a LQ problem similar to the above one, but discrete instead of continuous. This is in fact not surprising, since as mentioned earlier (remark 3), a Cosserat rod can be seen as a continuous multibody system with an infinite number of rigid bodies (the cross-sections), connected by "infinitesimal" joints (the usual vector q of joint angles being replaced by the field ξ a ). Usually, the NE forward dynamics algorithm of MBS is not deduced from optimal control, but rather by proving by induction, that a sweep-factorization of the type (57), holds from the tip body to the basis. To go a little further into details, in the discrete case, (57) becomes Λ j = S j ηj + s j where the bodies index j replaces X, and where in the NE terminology [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF], S j and s j denote the inertia matrix and the "bias-vector" of the "articulated body", whose "handle" is the body j. Based on this correspondence, one could expect to recover in S(X) and s(X) the continuous counterparts of these discrete concepts. Unfortunately, this is not the case because of the singular nature of OCP2. In contrast, we will see in section VI, that the resolution of the inverse dynamic problem does not suffer of the same difficulties, and that in this case, the correspondence between the NE inverse dynamic algorithm of MBS and that provided by the resolution of (45-48) when ξ a is imposed through a prescribed time-law, is entirely preserved.

2) Regularisation of OCP2 by using an implicit timeintegrator: In order to circumvent the difficulties raised by the singularity, one can directly use an implicit integration scheme from the beginning. Such an integration scheme is applied to the field ξ a , and takes the generic form:

ξ a,n+1 = a ξa,n+1 + f n , ξa,n+1 = b ξa,n+1 + h n , (58)
where a, b are two time-step dependent scalars, while f n and h n are some functions of the values of (ξ a , ξa , ξa ) at past steps t n , t n-1 ..., all these scalars and functions being defined by the specific scheme adopted. Now, reminding that ξa = u in OCP2, the integrator (58) imposes to any (ξ a , ξa ) possibly solution of the dynamic balance at t n+1 , to fulfil the constraints:

ξ a = au + f n , ξa = bu + h n ,
Moreover, thanks to the relations (8) between strains and the allowed X-rate ξ a , one has:

ϵ = au + f n -ξ ao , ε = bu + h n , ε = u. ( 59
)
Therefore, ϵ now depends on u, and the problem becomes regular since the optimality condition ( 46) can be explicitly used to express u= ε:

B T Λ -Λ ad = H r (au + f n -ξ ao ) ⇒ u = 1 a (H -1 r (B T Λ -Λ ad ) -f n + ξ ao ). (60)
Finally, thanks to the implicit integration and the constitutive law, the OCP becomes solvable. However, in contrast to the previous case, since ξ = Bϵ + ξ o and ξ = B ε, now depend on u through (59), the continuum kinematics (49) loose their status of reconstruction equations, to join those of the BVP [START_REF] Holsapple | A modified simple shooting method for solving two-point boundary-value problems[END_REF], which is no longer linear, but takes the full nonlinear form:

    g ′ η ′ η′ Λ ′     =     g ξ -ad ξ η + ξ -ad ξ η -ad ξ η + ξ ad T ξ Λ + M η -ad T η Mη - F     , (61)
with BCs:

(g, η, η)(0) = (1 4×4 , 0, 0) , Λ(1) = F + . ( 62
)
for a manipulator, or:

Λ(0) = -F -, Λ(1) = F + . (63)
for a locomotor. In all cases, note that ξ, ξ and ξ are removed from (61), by using the elimination relations:

ξ= Bϵ + ξ o = BH -1 r (B T Λ -Λ ad ) + ξ o , ξ= B ε = B( b a (H -1 r (B T Λ -Λ ad ) -f n + ξ ao ) + h n ), ξ= Bε = 1 a B(H -1 r (B T Λ -Λ ad ) -f n + ξ ao ), (64
) which are directly deduced from ( 8) and (59,60). Note that as this is expected, when removing all the velocities and accelerations from this formulation applied to a manipulator, the autonomous dynamic BVP (61,62) is changed into the static BVP [START_REF] Thandiackal | Emergence of robust self-organized undulatory swimming based on local hydrodynamic force sensing[END_REF][START_REF] Goh ; Dr | Necessary conditions for singular extremals involving multiple control variables[END_REF]. In the case of a locomotor, there are not enough BCs to fix the solutions of (61) and in this primary formulation, the BVP is undetermined. However, we shall see in section 3 how this indetermination can be removed with an implicit time-integration scheme of the net motion dynamics. Finally, as in the static case, these nonlinear BVPs will be solved with the shooting method in the next section.

Remark 6:

In the wake of works on simulation of towed cables and rods [START_REF] Sun | Modelling and simulation of low-tension oceanic cable/body deployment[END_REF], a similar but different approach was proposed to address the issue of continuum robots simulation in [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF]. In this alternative approach, the continuous model of accelerations, i.e. the third ODE (top-down) of (61) being ignored (see remark 3), η in the costate ODE of (61), is removed with an implicit scheme of the form η = c η + f η,n (with c and f η,n , a scalar and a past-dependent function of (η, η), fixed by the choice of the scheme). Otherwise, ξ is removed from the second ODE of (61) as above. This integration is hybrid since it involves strain (relative) and pause (absolute) accelerations, and hides the nature of the configuration space on which the model is based. In contrast, the above formulation based on Gauss principle, is consistently based on the strain configuration variables. Beyond these modeling considerations, we will see later that these two alternatives are not totally equivalent numerically.

Remark 7: In mechanics, an extensively used one-step implicit scheme is the Newmark implicit scheme. It has been originally developed for systems governed by Newtonian second order ODEs, and enriched along time of several adaptations (HHT, α-methods...), to tackle different issues related to nonlinear structural dynamics [START_REF] Cardona | Time-integration of the equations of motion in mechanism analysis[END_REF]. It is defined for any vector of generalized coordinates q ∈ R n , by (58), in which ξ a is replaced by q and where we impose:

a = β∆t 2 , b = γ∆t, f n = q n + ∆t qn + ∆t 2 1 2 -β qn , h n = qn + ∆t(1 -γ)q n , (65)
with ∆t is the time step, and (β, γ) are two constant parameters tuning stability and dissipation respectively, while (β, γ) = (1/4, 1/2) ensures second order accuracy with no damping, a choice that will be systematically adopted in the following.

Remark 8: Finally, the above regularization of OCP2 is based on the inversion of the constitutive law and the use of an implicit integrator of strain accelerations. Now, one can easily reflect the consequences of these two ingredients on the starting cost function. To this end, it suffices to introduce the first of the relations (59) in the term εT H r ϵ of ( 35) or [START_REF] Appell | Traité de mécanique rationnelle[END_REF], to realize that this regularization consists in adding to the Gauss cost functional, a quadratic term with respect to control of the form:

C r = au T H r u, ( 66
)
where a is proportional to ∆t 2 (for the Newmark scheme a = β∆t 2 ). Therefore, as soon as H r = 0, as for a continuous rigid chain, the problem becomes again singular and the approach aborts. For similar reasons, decreasing ∆t also put the approach in trouble, and there exists a lower bound of ∆t, noted ∆t c ("c" for "critical"), beyond which the method becomes unfeasible. From the point of view of optimal control, in such cases, the cost does not penalize enough the control which can take infinite values. This limit of the approach depends on the numerical methods used to solve the OCP, the machine accuracy, and the physical parameters of the rod as this will be illustrated later.

3) Implicit time-integration of a locomotor: In the case of a locomotor, the root cross-section X = 0 is free to move in space and the BVP (61,63) is undetermined, since it consists of 4 × 6 = 24 independent ODEs for 2 × 6 = 12 BCs. To remove this indetermination, it suffices to time integrate the dynamic of g(0) that we note g 0 , with an implicit time-integrator. As in the case of strains (see (58)), such an integrator allows the poses and velocities (g, η)(0) = (g 0 , η 0 ) to be deduced from the accelerations η(0) = η0 , and so the 3 × 6 kinematic ODEs of (61) require only 6 BCs. Then, since the remaining ODEs on Λ require 6 BCs, the 12 BCs (63) are sufficient in number to ensure BVP resolution. This integrator can be designed without resorting to any coordinate chart of SO(3). To that end, we first replace the root cross-section pauses g 0 of SE(3), by their orientation and position (R 0 , r 0 ) ∈ SO(3) × R 3 , considered as a Lie group with internal composition law

•, such that (R 1 , r 1 ) • (R 2 , r 2 ) = (R 1 R 2 , r 1 + r 2 )
. Then, we use the usual Newmark scheme on linear spaces for the positional component r 0 , i.e.:

r 0,n+1 = a r0,n+1 + f n , ṙ0,n+1 = b r0,n+1 + h n , (67)
with a, b, f n , h n defined by formulas (65) in which q is replaced by r 0 , while for R 0 , we use an extension of the same scheme on SO(3) originally proposed in [START_REF] Simo | A finite strain beam formulation. the three-dimensional dynamic problem. part i[END_REF]:

Θ 0,n+1 = a Ω0,n+1 + k n , Ω 0,n+1 = b Ω0,n+1 + l n , (68)
with (a, b) given by (65), and:

k n = ∆tΩ 0,n + ∆t 2 ( 1 2 -β) Ω0,n , (69)
l n = Ω 0,n + ∆t(1 -γ) Ω0,n , and where the vector Θ 0,n+1 is defined at any time step by: R 0,n+1 = R 0,n exp( Θ0,n+1 ),

with "exp" denoting the exponential map of SO(3). Note that this scheme can be simply deduced from the usual Newmark integrator in vector space defined by (67) and (65). To that end, it suffices to make appear the displacement r 0,n+1 -r 0,n between two steps in the first of the relations (67), and then when shifting from R 3 to SO(3), to change this displacement into the translation along the one-parameter subgroup of SO(3) defined by (70), while keeping all velocities and accelerations expressed in the mobile frame, consistently with left-invariance of rigid body dynamics on SO(3) [START_REF] Arnold | Mathematical methods in classical mechanics[END_REF]. Now, removing the index n + 1, we have the kinematic relations between SE(3) and SO(3) × R 3 :

g 0 = R 0,n exp( Θ0 ) r 0 0 1×3 1 , (71)
η 0 = Ω 0 R T 0 ṙ0 , η0 = Ω0 R T 0 r0 + (R T 0 ṙ0 ) × Ω 0 .
Finally, introducing (67) and ( 68) in these relations, allows expressing (g 0 , η 0 , η0 ) at any time beyond t n , as some functions of ν 0 = (Θ T 0 , r T 0 ) T ∈ R 6 only (the past values of pause, velocity and accelerations being fixed):

η 0 = A(ν 0 ) , η0 = B(ν 0 ) , g 0 = C(ν 0 ), (72)
where the third of these relations is simply given by (71), while the two others are detailed in Appendix 3. Note that, as with any implicit scheme, these relations can be used to transform the differential equations of motion (here the net motions), into nonlinear algebraic equations of positional variables only, that can be treated at each time-step of a simulation, with an iterative root-finder as the Levenberg-Marquardt (LM) algorithm.

C. Shooting method for OCP1 and OCP2

To solve the autonomous BVPs [START_REF] Thandiackal | Emergence of robust self-organized undulatory swimming based on local hydrodynamic force sensing[END_REF][START_REF] Goh ; Dr | Necessary conditions for singular extremals involving multiple control variables[END_REF] and (61-64), we apply the shooting method, which allows to solve a BVP through a sequence of initial value problems (IVPs) whose solution converges toward that of the BVP. Practically, one applies an initial guess of the unknown BCs at X = 0 (proximal end) that is corrected until the integrated solution of the ODEs of the BVP initialized with these proximal BCs (which so defines an IVP), matches with the known (distal) BCs at X = 1. This is achieved in the correction loop of the LM algorithm that formally reads, with σ the adaptive damping:

x k+1 = x k + (JJ T )(x k ) + σ1 -1 J T (x k )r es (x k ), ( 73
)
where k stands for the index of the iteration, x is the vector of the unknown proximal BCs, r es (x) the residual vector of the known distal BCs, J = (∂r es /∂x) its Jacobian, x and r es being both related by the numerical forward integration (i.e. from X = 0 to 1), of the X-ODEs of the BVP.

Subsequently, the method is applied to the BVP of OCP1, as well as to that of OCP2 for a manipulator and for a locomotor. Analysing the known and unknown BCs of each of these cases, and using the Newmark scheme on SO(3) × R 3 for a locomotor, allows to instantiate x and r es (x) according to table 3. Going into further details, to 51) from X = 0 to 1, starting with initial conditions:

(g, Λ)(0) = (1 4×4 , x), (74)
and then compute r es (x) = Λ(1)-F + . The residual of OCP2 (manip.) is calculated by integrating (61,64) from X = 0 to 1, starting from:

(g, η, η, Λ)(0) = (1 4×4 , 0, 0, x), (75)
and compute r es (x) = Λ(1) -F + . For the OCP2 (locom.), we do the same, but starting with initial conditions compatible with the constraints (72) imposed by the implicit integrator (67-70), i.e.:

(g, η, η, Λ)(0) = (C(x), A(x), B(x), -F -), (76)
and computing at the distal end: r es (x) = Λ(1) -F + . Finally, at any time-step (or "loading-step" in statics), once the LM loop has converged (i.e. ∥r es ∥ is below a given threshold), ξ in statics, and (ξ, ξ, ξ) in dynamics, are updated with (64). In dynamics, they are stored for the next step, in f n and h n of (65). The "loading" or time-step is then incremented, and the shooting algorithm resumes. The initial guess of this algorithm can be defined as follows.

For a manipulator, x = Λ(0) is initialized by its value at the previous loading or time-step, while for a locomotor, one can use one of the usual predictors of the Newmark scheme as the inertial (ballistic) one, which consists in choosing (r 0,n+1 , ṙ0,n+1 ) and (Θ 0,n+1 , Ω 0,n+1 ) such that r0,n+1 and Ω0,n+1 are forced to zero in ( 67) and ( 68) respectively.

To complete the picture, the LM loop (73) needs the Jacobian of the residual vector J = (∂r es /∂x), to be calculated. This can be achieved by first linearizing each of the IVP, to get their tangent IVP, that we note here TIVP. We now give these TIVP for OCP1, OCP2 (manip.) and OCP2 (locom.). The first one is given by the linearization (or second variation ∆) of (25-27), the second and the third by that of (61-64). Note that all these TIVPs are fed with different initial perturbative conditions at X = 0 that do not affect X in any case (i.e. ∆X = 0). Therefore, the second variation ∆ behaves as the first one δ, and fulfills the commutation relation [START_REF] Anil | A survey of numerical methods for optimal control[END_REF]. Defining ∆ζ such that ∆g = g∆ ζ, the TIVPs are defined as follows:

• The TIVP of OCP1 is defined by gathering (51) and:

∆ζ ′ ∆Λ ′ = -ad ξ ∆ζ + ∆ξ ad T ξ ∆Λ + ad T ∆ξ Λ -∆ Fext , ( 77
)
where from (64), and since ∆Λ ad = 0:

∆ξ = BH -1 r B T ∆Λ, (78)
and with ICs given by (74), and:

(∆ζ, ∆Λ)(0) = (0, ∆x).

(79)

• The TIVP of OCP2 is defined by gathering (61) and:

    ∆ζ ′ ∆η ′ ∆ η′ ∆Λ ′     =     -ad ξ ∆ζ + ∆ξ -ad ξ ∆η -ad ∆ξ η + ∆ ξ -ad ξ ∆ η -ad ξ ∆η M∆ η -ad T η M∆η     +     0 0 -ad ∆ ξ η -ad ∆ξ η + ∆ ξ -ad T ∆η Mη + ad T ξ ∆Λ + ad T ∆ξ Λ -∆ Fext     , (80)
with ∆ξ, ∆ ξ, and ∆ ξ, given by the variation of (64):

∆ξ = BH -1 r B T ∆Λ , ∆ ξ = b a ∆ξ , ∆ ξ = 1 a ∆ξ. ( 81
)
And with ICs given by (75), and:

(∆ζ, ∆η, ∆ η, ∆Λ)(0) = (0, 0, 0, ∆x).

(82)

in the case of a manipulator, and by (76) and:

(∆ζ, ∆η, ∆ η, ∆Λ)(0) = (83) ((∂C/∂x)∆x, (∂A/∂x)∆x, (∂B/∂x)∆x, -∆F -).

in the case of a locomotor, where we used the tangent maps (Jacobian matrices) to (72) detailed in Appendix 3. Finally, integrating these linearized systems of ODEs from X = 0 to 1, and keeping only the variation of the known BCs in X = 1, provides ∆r es = ∆Λ(1) -∆F + , for each of them, where ∆F + , like ∆F -and ∆ F , can be calculated from the variation (4) of a possibly state dependent model of the tip wrench F + (resp. F -and F ). By simple identification of this numerical computation with the explicit matrix relation ∆r es = (∂r es /∂x)∆x, one can infer the following computational process of J = (∂r es /∂x). Feeding the above linear ODEs with unit initial conditions ∆x = δ i , i = 1, 2, ..., 6, with δ i a vector of zero entries, except the i th which is fixed to 1, allows the i th column of the Jacobian to be computed. Thus resuming this process column after column, completely fills the matrix.

VI. INVERSE STATICS AND DYNAMICS OF CONTINUUM

ROBOTS

In the above development, we addressed the forward static and dynamic models, which can be formally defined as the input-output maps: ξ a = F SM (state, Λ ad ) and ξa = F DM (state, Λ ad ) respectively, where in statics the state of the rod is defined by g or ξ a (through integration of g ′ = g ξ), and in dynamics by (ξ a , ξa ), or equivalently (g, η) thanks to the reconstruction equations [START_REF] Lighthill | Aquatic animal propulsion of high hydromechanical efficiency[END_REF]. Inverting the role of inputs and outputs defines the inverse static and dynamic models: Λ a = ISM (state, ξ ad ) and Λ a = IDM (state, ξad ) respectively. Solving these inverse problems is much more simple than solving their forward counterparts. Indeed, in this case, u = ϵ is known in [START_REF] Damaren | The relationship between recursive multibody and discrete-time optimal control[END_REF], and u = ε is known in [START_REF] Holsapple | A modified simple shooting method for solving two-point boundary-value problems[END_REF], which so directly define two autonomous BVPs.

A. Inverse statics of a continuum manipulator

In the static case, it suffices to impose ξ a = ξ ad in the state ODE of ( 25) that is integrated forward, and to reintroduce g (which appears in F (g) and F + (g)) in the costate equation which is integrated backward from X = 1, where Λ(1) = F + (g), to X = 0. This provides the field Λ along the entire rod, which is then used to calculate Λ ad = B T Λ -H r ϵ. Note that this algorithm in two passes is the continuum version of the Luh computed torque algorithm (here in statics) of rigid manipulators [START_REF] Walker | On-line computational scheme for mechanical manipulator[END_REF].

B. Inverse dynamics of continuum robots

In the dynamic case, we need to find the two fields ( η, Λ) solution of the system of ODEs [START_REF] Holsapple | A modified simple shooting method for solving two-point boundary-value problems[END_REF] with BCs [START_REF] Meirovitch | Dynamics and Control of structures[END_REF] or [START_REF] Boyer | Poincare cosserat equations for the lighthill three-dimensional large amplitude elongated body theory: Application to robotics[END_REF] where u = ξa = ξad while other state-dependent fields are considered as functions of X only (e.g. updated at each time t of a time loop fed by a prescribed motion t → (ξ ad , ξad , ξad )(t)). This problem defines an autonomous linear BVP with state and costate ( η, Λ) in the generic form (108) of Appendix 2. As introduced in section V.B, one can then apply the backward sweep method, which in both cases (manipulator or locomotor), consists in exploiting the factorization (57). Then, using the identification and elimination process introduced in Appendix 2, one obtains that S and s are solutions of the Riccati ODEs:

S ′ = M + ad T ξ S + Sad ξ , s ′ = ad T ξ s -F + S(ad ξ η -B ξad ), (84)
where we introduced the notation F = F + ad T η Mη. These two ODEs need to be X-integrated backward with initial conditions S(1) = 0 and s(1) = Λ(1) = F + . Once S and s known in X = 0, if the robot is a locomotor, η(0) = η0 is an unknown of the problem that one has to compute from (57) in X = 0 and the first of the BCs of (48):

Λ(0) = -F -= S(0) η0 + s(0) ⇒ η0 = -S(0) -1 (s(0) + F -). (85)
If the robot is a manipulator, the same process can be applied but directly with η(0) = 0. Finally, this resolution can be implemented through the following (inverse) algorithm which proceeds in three passes. First integrate forward (from 0 to 1) the reconstruction equations ( 49), then integrate backward (from 1 to 0) the two Riccati's ODEs (84), starting with initial conditions S(1) = 0 and s(1) = Λ(1) = F + . Once S and s known over [0, 1], one can calculate η by forward integrating the state equation of ( 45) initialised with η(0) = 0, for a manipulator, and (85), for a locomotor. While integrating the state, calculate the costate field Λ with (57) an the actuated stress Λ ad = B T Λ -H r ϵ, which is the output of the inverse dynamics.

Remark 9: This inverse algorithm is new, and in particular different from, though equivalent to, the one in [START_REF] Boyer | Macro-continuous computed torque algorithm for a three-dimensional eel-like robot[END_REF]. When H r = 0 na×na , it is nothing more than a continuous version of the inverse algorithm of MBS based on the composite bodies [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF]. As its discrete homologous, it is structured into similar functional steps, where the 3 (2 forward and 1 backward) recursions on body indices of the discrete case, are replaced by 3 ODEs (2 forward and 1 backward) on the continuous label X. In the terminology of MBS [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF], S(X) and s(X), solutions of (84), are the exact continuous counterpart of the inertia matrices and externalinertia wrenches of the composite bodies of a MBS, i.e., they define the inertia matrix and wrench with respect to the X-cross-sectional frame, of a composite beam constituted of all the cross-sections included into [X, 1] and frozen in their current configuration. Going further, using standard relations between ad and Ad operators [START_REF] Murray | A Mathematical Introduction to Robotic Manipulation[END_REF], allows integrating (84), which yields:

S(X) = X 1 Ad -T h MAd -1 h dY , s(X) = Ad -T h(1) F + - X 1 Ad -T h ( F + S(B ξad -ad ξ η))dY, (86)
where h = g -1 (X)g(Y ) here represents the transformation mapping the X-cross-sectional frame onto the Y -one (Y ∈ [X, 1]). Examining (86) confirms that S(X) and s(X) represent the sum of all the cross-sectional inertia matrices and external-inertia wrenches along the rigid piece of beam [X, 1], consistently carried from their local Y -frame to the X-frame.

Remark 10: As formulated above, the inverse problem is related to the input-output map Λ ad → ξ a in statics, and Λ ad → ξa in dynamics. In practise, these inverse static and dynamic models need to be supplemented with the inversion of the map τ → Λ ad , where τ is a finite set of actuation inputs that depends on the technology of the actuation. For example, in the case of TACRs, τ = (T 1 , T 2 , ...T N ) T represents the vector of tendon tensions in the equation [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF], and this additional map can be inverted by generalized inversion as illustrated in [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF].

VII. NUMERICAL APPLICATIONS

To illustrate the above simulation approach, we consider four tests. We start with a tendon-actuated continuum manipulator statically bent by pulling one tendon (test 1), and suddenly released in gravity (test 2). In addition to providing a validation against other simulators, this second test allows to discuss the behavior of the dynamic simulator. The third test is inspired of the bench of the flying beam initially proposed by Simo to validate the model of rigid overall motions in his geometrically exact FEM (GE-FEM) [START_REF] Simo | A finite strain beam formulation. the three-dimensional dynamic problem. part i[END_REF]. This third test allows to validate our geometric integrator on SO(3) × R 3 as well as the OC-based shooting method applied to a rod free at both ends. In a last example (test 4), the approach is applied to the simulation of an undulatory swimmer at high Reynolds numbers internally actuated with a torque field. All the results are compared to those provided by another (totally different) simulator recently validated against GE-FEM in [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]. In this simulator, the strains are projected on a truncated basis of Legendre polynomials (modes), whose coefficients are governed by Lagrangian dynamics coupled to those of the pause of the root (parameterized with quaternions), all being explicitly time-integrated with ODE45 of Matlab. The same integrator (ODE45) is here used to X-integrate the IVPs and TIVPs of the shooting method. As regards time-integration, we use the previously Fig. 2: A rod is deformed by pulling one tendon of a convergent (a) and spiral (b) pair. Snapshots of static equilibrium configurations obtained by solving OCP1 with the shooting method applied to the BVP [START_REF] Thandiackal | Emergence of robust self-organized undulatory swimming based on local hydrodynamic force sensing[END_REF][START_REF] Goh ; Dr | Necessary conditions for singular extremals involving multiple control variables[END_REF].

introduced Newmark schemes with (β, γ) = (1/4, 1/2) which ensures second order accuracy and no damping.

A. Test 1: a TACR statically deformed

We consider a single piece TACR of length l = 0.4m and disk diameters 2R d = 0.04m, equipped with a pair of convergent and spiraled tendons [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]. Initially aligned with gravity (i.e. with the vertical), it is deformed by pulling one of the two tendons (indexed by 1, 2), in the quasi-static regime. Simulations are carried out by applying the shooting method to the autonomous BVP [START_REF] Thandiackal | Emergence of robust self-organized undulatory swimming based on local hydrodynamic force sensing[END_REF][START_REF] Goh ; Dr | Necessary conditions for singular extremals involving multiple control variables[END_REF] of OCP1 with the actuation model [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF] in which Γ = E 1 = (1, 0, 0) T , the influence of ϵ is neglected, while D 1 = (0, R d (1-(X/l)), 0) T and D 1 = (0, R d cos(2πX/l), R d sin(2πX/l)) T (1 st convergent and spiral routing). Figure 2 displays the converged equilibrium configurations when incrementing the pulling force T 1 step by step from 0 to 67.5N. The arm is modelled by a rod with ρ = 8000kg.m -3 and E = 207GPa, which are typical of continuum robots [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF]. In these examples, which are representative of all others here not reported, the two simulators fit exactly in the first case, and very well (error at the tip ≃ 1.4%), in the second. Numerically, the shooting method just requires a few iterations of the LM algorithm of Matlab fsolve.

B. Test 2: TACR bent and released in gravity

We reconsider the TACR of test 1 in static equilibrium under the effect of a convergent tendon with a tension T 1 . At t = 0s, the tendon is instantly cut and the rod is released under the effect of its stiffness and gravity. Solving the OCP2 of this manipulator at each time step, allows to simulate its temporal evolution. Figure 3(a) shows the time plots of the tip of the rod, here released from a weakly deformed static configuration (T 1 = 15N). The simulation is performed over 1s, with ∆t = 1ms. Once again, the results obtained with the shooting method (in blue), here applied to the BVP (61,62), are very close to those provided by the Lagrangian simulator of [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF] (green) which here needs 5 modes to achieve convergence. Beyond validation, the same example was tested under many other conditions in order to obtain an overview of the approach, which can be summarized as follows. First, we remarked that the shooting method is much more difficult to apply in dynamics than in statics and requires good initial guesses to converge. Otherwise, the IVPs defined by (61) can blow up before reaching the distal end. These difficulties increase in the regime of finite deformations and/or strong transients, and the method cannot generally be applied in a single shoot, but rather in several ones using the "multiple" or "modified shooting method" of [START_REF] Holsapple | A modified simple shooting method for solving two-point boundary-value problems[END_REF]. In all that follows, we have used a non adaptive version of the latter approach with 10 intermediate shoots. Second, as mentioned in remark 8, decreasing the time-step ∆t tends to degrade the numerical convergence of the LM algorithm. Calculating the Jacobian of the residual vector with the TIVPs as explained in section V.C, contributes to delay divergence, but there always exists a critical time-step ∆t c below which the method fails. In order to estimate the value of ∆t c , we started by considering the rod of test 1. For the sake of concision, we use the notations (EI, ρA, l) = (k, µ, l), while any quantity related to this reference rod is indicated with a star. Based on successive numerical trials and errors, we noticed that the numerical instability of the simulator is not a threshold instability, as can be a standard CFL condition [START_REF] Courant | On the partial difference equations of mathematical physics[END_REF], but rather an instability with a "transitional" regime. After a finer analysis, we obtained that 0.5ms ≲ ∆t ⋆ c ≲ 1ms. From this first result, we tried to determine how ∆t c varies when the physical parameters (k, µ, l) change. The results of this study led us to the following scaling relationships:

∆t c ∆t ⋆ c ∼ k ⋆ k , ∆t c ∆t ⋆ c ∼ µ µ ⋆ , ∆t c ∆t ⋆ c ∼ l l ⋆ 2 , (87)
which suggest that ∆t c follows a law of the form:

∆t c ∝ χ l 2 ρA EI , ( 88
)
where χ is a dimensionless pre-factor. It seems quite natural that ∆t c evolves in this way because we recognize in the factor of χ, the characteristic time of a linear Euler-Bernoulli beam [START_REF] Meirovitch | Dynamics and Control of structures[END_REF]. In this regard, note that such a result could also have been obtained by a dimensional analysis involving only the parameters k, µ and l. It can be seen, however, that the pre-factor χ seems to depend on other physical factors as the conditions of the test (imposed forces and motions), and also non-physical ones as the adopted numerical solver, and the machine accuracy. For example, in the conditions of the test considered here, the χ factor can be estimated at 1/100. It is also worth noting that the method proposed here is based on an OCP which tends to be singular when the matrix aH r of (66) becomes illconditioned (see remark 8). In the present context aH r is of the order of EI∆t 2 , which suggests once again that ∆t c evolves inversely with the square root of k = EI, as the first relation (87) does. We also noted that ∆t c is little influenced by the amplitude of the deformations. Figure 3(b) shows a few snapshots (every 15ms), of a simulation of the star rod released from a weakly deformed configuration with ∆t = 0.5ms< ∆t ⋆ c . Around 0.18s, the rod seems loosing its stiffness and some fast undulations appear at its distal tip and increase up to destabilize simulation. Based on remark 6, we also applied the same shooting approach but to the BVP defined by removing from (61) the continuous model of accelerations (the third ODE from top to bottom). To achieve comparison in equivalent conditions of time-integration, we used a one-step Newmark-like implicit approximation of time-derivatives as proposed in [START_REF] Sun | Modelling and simulation of low-tension oceanic cable/body deployment[END_REF], i.e., we approximate ε and η in (61) by:

ε = c ϵ + f ϵ,n , η = c η + f η,n , (89)
where c = (α∆t) -1 , f z,n = α(α -1) -1 żn -c z n , for z = ϵ and z = η, and with α a parameter that tunes numerical damping. In the results here reported we took α = 1/2, which ensures no damping [START_REF] Sun | Modelling and simulation of low-tension oceanic cable/body deployment[END_REF], as this is the case of the Newmark-integrator (58,65). Note that in this case, (89) also coincides with the trapezoidal approximation of [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF], which has a second order accuracy like our Newmark integrator. In Figure 4, we have plotted the results of a 1s simulation (snapshots and time plots of the tip position), performed with both simulators based on shooting, and with the Lagrangian approach of [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]. The rod is the reference (star) rod with a material one hundred times softer (k = k ⋆ /100). It is initially bent with a horizontal tip force of 0.4N, and released at t = 0s. As observed in these plots (which are representative of all others performed in the regime of finite deformations), the full BVP-based shooting approach fits perfectly with the Lagrangian simulator, while the second one (based on the BVP without the accelerations model) shows small discrepancies. Note that such differences also appear in the small deformation regime, but after many beats (see red plots of figure 3(a)). Despite this, the deformations of both approaches remain in good agreement with the reference simulator. Another key point of comparison between the two approaches is that they are constrained by critical time steps of almost identical values, suggesting that the interpretation of ∆t c based on Gauss least constraint principle and OCT also holds for the second shooting approach.

C. Test 3: a flexible flying stick

This test is directly inspired by the GE-FEM bench of the flying rod, recently simulated by applying the Lagrangian approach of [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]. In this test, a straight rod is initially tilted as illustrated in figure 5.a. Gravity is ignored, and at t = 0s, a force and a torque are applied to one of its tips (X = 1) according to the time-profiles of figure 5.a, which are entirely quantified by c 2,max . The is thus catapulted in space in a complex three-dimensional movement. The rod here considered has the material properties of the slender swimmer tested in the next example (1m length, 0.1m diameter, ρ = 10 3 kg.m -3 , E = G = 10 6 Pa). In Figures 5.b andc, we plotted the time evolution of the coordinates of the center of mass of the stick and a sequence of snapshots every 0.2s for a 5s simulation with c 2,max = 10.7Nm. It is obtained with the shooting method applied to the BVP (61,63) with the Kirchhoff rod model. As shown in these graphs, the results obtained with the shooting-based approach (here with ∆t = 10ms > ∆t c ≃ 3ms) are in very good agreement with those of the simulator of [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF], which thus confirms its applicability to three-dimensional floating basis systems, time-integrated with our implicit Newmarkscheme on SO(3) × R 3 .

D. Test 4: a continuum bio-inspired swimmer

We now apply the approach to a bio-inspired locomotor, namely a continuum swimmer mimicking a fish. The fish body is modelled as a neutrally buoyant 1m length Kirchhoff rod composed of elliptical cross sections as detailed in [START_REF] Boyer | Poincare cosserat equations for the lighthill three-dimensional large amplitude elongated body theory: Application to robotics[END_REF] and illustrated in figure 6. The hydrodynamic forces are modelled with Lighthill's theory [START_REF] Lighthill | Aquatic animal propulsion of high hydromechanical efficiency[END_REF], in the geometric setting of [START_REF] Boyer | Poincare cosserat equations for the lighthill three-dimensional large amplitude elongated body theory: Application to robotics[END_REF], [START_REF] Boyer | Fast dynamics of an eel-like robot-comparisons with navier-stokes simulations[END_REF]. In this context, we use the change of variable on stress:

Λ ⋆ = V 1 M a η - 0 3×1 T f E 1 , T f = 1 2 η T M a η, (90)
where V 1 is the first (axial) component of ṙ in the crosssectional frame, E 1 = (1, 0, 0) T , while M a stands for the 6 × 6 added mass matrix of the elliptic cross-sections, and T f is the corresponding density of kinetic energy along the body. With these notations, the dynamic model of the swimming fish is given by that of a Cosserat rod (in vacuum) in which Λ needs to be replaced by Λ ⋆ , M by M ⋆ = M + M a , and with the BCs:

Λ ⋆ (0) = 0 , Λ ⋆ (1) = V 1 M a η - 0 3×1 T f E 1 (

1). (91)

To generate a swimming gait, we take an active constitutive law [START_REF] Bergou | Discrete elastic rods[END_REF] in the form of the distributed proportional control:

Λ a = Λ a,d + H r ξ a = H r (ξ a -ξ a,d ), (92)
where (X, t) ∈ [0, 1] × R + → ξ a,d (X, t) ∈ R na defines a desired time-evolution of the shape. In the test here reported, we consider a planar swimming gait defined by ξ a,d (X, t) = (0, K d,2 , 0)(X, t) with:

K d,2 (X, t) = f r (t)f w (X, t), (93)
where we used two functions. One is a sinusoid ramp timefunction f r of the form: which is twice continuously differentiable and thereby guarantees smooth time transitions. The second, is a space-time (backward) wave function f w defined ∀(X, t) ∈ [0, 1]×R + :

f r (t) = 0 , 0 ≤ t < t i , f r (t) = 1 , t ≥ t f , (94)
f r (t) = t -t i t f -t i - 1 2π sin 2π t -t i t f -t i , t i ≤ t < tf,
f w (X, t) = (a 0 +a 1 X+a 2 X 2 ) sin 2π X λ - t T , (95)
in which λ is the wave length, T is the period, and a 0 , a 1 and a 2 are the coefficients of a second order polynomial amplitude envelop. To simulate this swimmer, we applied the shooting algorithm to the BVP (61,63) in which Λ is replaced by Λ ⋆ and F ± defined by (91). We assume the fish has no stiffness except that of the control law (92). Noting from (90) and B = (1 3×3 , 0 3×3 ) T (the fish is modelled by a Kirchhoff rod), that B T Λ = B T Λ ⋆ , the elimination relations are still (64), in which Λ ad = -H r ξ a,d (t), ξ a,o = 0 3×1 . To illustrate the approach on a representative example, we reported in figures 6(b) and 7, the time evolution of the head axial velocity V 1 (X = 0) (first component of ṙ(0) in the mobile frame of the cross-section X = 0), and a sequence of snapshots obtained with the shape law (93-95) with (a 0 , a 1 , a 2 ) = (1, 0.5, 2), (t i , t f ) = (0, 1), (λ, T ) = (1, 1) (SI Units). The time-step of the test is ∆t = 10ms > ∆t c ≃ 1ms. The control stiffness along the body is H r = diag(0, 50, 0), which is enough to ensure ξ a to perfectly track ξ ad . Hence, as illustrated in figure 6(b), the motion of figure 7 matches with that obtained by directly shape-controlling the swimmer with the inverse algorithm of section VI.B, i.e. by time-integrating (85) with ODE45, where S(0) and s(0) are computed at each time step, by X-integrating the Riccati ODEs (84).

VIII. CONCLUSION

In this paper, we have studied the relationship between modeling and simulation of continuous robots and optimal control theory. Based on the Cosserat model, we have shown that beyond the static case of passive rods actuated by external forces, one can integrate in the optimal control theory the modeling and simulation of continuous robots actuated in a distributed manner, both in the quasi-static and dynamic regimes. In statics, this extension is based on the minimum potential energy principle applied to a Cosserat rod in which the actuation is modeled by an internal stress field through an extended active constitutive law. In dynamics, it is achieved by invoking Gauss's principle of least constraint, a variational principle rarely used in rational mechanics, more oriented towards Hamilton's and D'Alembert's principle. Using these two principles, the statics and dynamics of continuous manipulators and bioinspired locomotors have been formulated into OCP based BVPs that have been solved at each step of a simulation loop with the shooting method, as this is generally done in the continuum robotics community. Remarkably, the OC perspective has shown that while the static optimal control problem is well-posed, the dynamic problem is inherently singular. In the context of numerical simulation, this singularity can be circumvented by using an implicit time integrator and the invertibility of the constitutive law. However, this regularization process imposes a certain limit to the time-step, below which the approach fails. Although this phenomenon has been noticed as one of the major difficulties of the shooting-based approach applied to dynamics, it has so far been interpreted as a numerical artifact of the method due to the rounding errors of the machine. On the contrary, the OC viewpoint shows that this limitation of the method is in fact due to the intrinsic structure of the dynamic OCP. In addition, OC also shows that the dynamic BVP must contain the continuous acceleration model, which is systematically omitted in the shooting-based approaches used so far to solve the forward dynamics of Cosserat rods. As our numerical tests show, adding this model to the dynamic BVP improves the accuracy of the simulations. Moreover, as a by-product of this improvement, the full BVP provided by OC turns to be a continuous version of the Newton-Euler model of rigid robots, thus linking the practices of continuous robotics with the discrete dynamics of rigid robots. Beyond the case of manipulators, the entire approach was extended to locomotors and applied to a continuum swimmer inspired by slender fish. In the context of bio-inspired robotics and biomechanics, this simulator is a first step towards studying the interactions of musculoskeletal and nervous systems with the ambient flow, on which is based the emergent synchronization of undulatory swimming [START_REF] Thandiackal | Emergence of robust self-organized undulatory swimming based on local hydrodynamic force sensing[END_REF]. Finally, let's end this article with some practical tips for the users. In statics, the shooting-based approach is efficient and always gives satisfaction. However, in dynamics, it is much more fragile and fails when the time-step is smaller than a critical value defined by (88). As shown in this relationship, this limitation becomes more and more restrictive as the robot becomes softer and softer, and for this reason we could not recommend using it (at least in its current state of the art) in soft robot dynamics. Despite this limitation, the shooting-based approach has several advantages. First, it does not require any reduction of the configuration space and, from this point of view, should be very accurate. Second, it is conceptually simple to program, at least for simple architectures (like those simulated in the paper). Third, the dimension of its residual vector is very small and independent of the space discretization, and when it works, the method is very time-efficient.

IX. APPENDIX 1: CALCULUS OF THE GAUSS CONSTRAINT FOR A CONTINUUM ROBOT

To calculate the acceleration energy of a continuum robot, we calculate the contribution dT acc of a single rigid cross-section noted S(X), and add all these contributions along the rod axis. Any material point of the rod is localized by its material coordinates (X, Y, Z) where X is the label of the cross-section containing the particle, and (Y, Z) are its coordinates in the plane of the cross-section. Then using the definition (30), we have:

dT acc = 1 2 S(X) ρ a 2 c dY dZ, (96)
where dY dZ is the area element of the section S(X), and a c : (X, Y, Z) ∈ R 3 → a c (X, Y, Z) ∈ R 3 is the acceleration field of the material points of the rod. For the sake of concision, we introduce the vector X ⊥ = (0, Y, Z) T such that (X, Y, Z) T = (X, 0, 0) T + X ⊥ . According to Gauss principle, a c needs to be compatible with the Cosserat kinematics, i.e., it can be detailed as:

a c (X, Y, Z) = φ(X, Y, Z) = (97) R(X)( V (X) + Ω(X)X ⊥ + Ω(X)(V (X) + Ω(X)X ⊥ )),
where φ is deduced by time-differentiating twice the Cosserat kinematics φ(X, Y, Z) = r(X) + R(X)X ⊥ . In these conditions, one has:

dT acc = 1 2 S(X) ρ φ2 dA = (98)
1 2 S(X) ρ(( V + ΩX ⊥ ) + Ω(V + ΩX ⊥ )) 2 dY dZ, which can be developed as:

dT acc = 1
2 S(X) ρ( V + ΩX ⊥ ) 2 dY dZ + 1 2 S(X) ρ( Ω(V + ΩX ⊥ )) 2 dY dZ + S(X) ρ( V + ΩX ⊥ ) T ( Ω(V + ΩX ⊥ ))dY dZ

= C 1 + C 2 + C 3 . ( 99
)
We now compute each of the integrals of (99), noted C 1 , C 2 and C 3 . Regarding C 1 , let us remark that the ordinary kinetic energy of a cross-section being defined by: dT = 1 2 S(X) ρ φ2 dY dZ = 1 2 S(X) ρ(V + ΩX ⊥ ) 2 dY dZ = 1 2 η T Mη, (100) we have for the same reasons (i.e. using the same calculations but with accelerations instead of velocities):

C 1 = 1 2 ηT M η. ( 101
)
Regarding C 2 , one can ignore it, since it only depends on velocities and brings no contribution to dT acc . It remains to calculate the double product C 3 . It can be detailed first as: Once more, developing this expression while ignoring the terms that only depend on velocities, and assuming that the cross-sectional frame is centered on the mass-center of the cross-section (i.e. S(X) ρ X ⊥ dY dZ = 0), one finds:

C 3 = S(X) ρ V T ( ΩV )dY dZ+ S(X) ρ( ΩX ⊥ ) T ( Ω( ΩX ⊥ ))dY dZ. ( 103
)
where using the definition of the mass ρA = S(X) ρ dY dZ and inertia matrix of the cross section ρJ = S(X) ρ X⊥ XT ⊥ dY dZ, one can show that:

C 3 = V T (Ω × ρAV ) + ΩT (Ω × ρJΩ) = -ηT (ad T η Mη), (104)
where we used the definition of ad η and M = diag(ρJ, ρA1 3×3 ). Finally, accounting for (101) and (104), with C 2 = 0, in (99), and integrating dT acc along the robot, provides the expression (30) of its "energy of acceleration". To calculate the acceleration power of non reactive force P acc , we first remark that these forces are: the external forces applied on the two tips (F -, F + ) and inside the domain of the rod ( F ), those generated by the exogenous activation (Λ ad ), and finally the internal restoring forces. Summing the usual definitions of power of all these forces, and changing velocities into accelerations, gives [START_REF] Boyer | Poincare's equations for cosserat media: Application to shells[END_REF][START_REF] Renda | Screw-based modeling of soft manipulators with tendon and fluidic actuation[END_REF], where remind that the usual (velocity) power of internal restoring forces is

P int = -d dt 1 2 1 0 ϵ T H r ϵdX = - 1 0
εT (H r ϵ)dX.

X. APPENDIX 2: SINGULAR LQ-OCP2

Even if the LQ optimal problem of the forward dynamics is singular and indeed numerically unfeasible due to the illconditioning of the Riccati equations (see section V.B.1), we here ignore these numerical considerations and formally address this problem. When applied to a cost functional quadratic w.r.t. state and control, the optimality condition (42-c) directly provides the expected expression of the control and the optimal control is said to be regular. In contrast, in our case, the optimal cost C is only linear w.r.t. the control, and our problem is singular. To derive the expected expression of ξa = u, we need to differentiate (42-c) further w.r.t. to X, up to make appear explicitly the control [START_REF] Bryson | Applied Optimal Control[END_REF]. As proved in [START_REF] Goh ; Dr | Necessary conditions for singular extremals involving multiple control variables[END_REF], the number of derivation required is even and noted 2p, where p defines the order of the singular arcs along which the state and costate evolve. Applying this process to our problem, gives for the zero and first order X-derivatives of (42-c):

∂H ∂u = 0 ⇒ B T Λ = Λ ad + H r ϵ , ∂H ∂u ′ = 0 ⇒ B T Λ ′ = Λ ′ ad + (H r ϵ) ′ , (105)
which does not make appear the control, since from (45.b): Λ ′ = M η + ad T ξ Λ -F . In contrast, because Λ ′′ makes appear η′ , and finally ξa = u through the state ODE (45.a), the second order X-derivative does provide the expected dependence:

Fig. 1 :

 1 Fig. 1: Twists (left) and wrenches (right) of a Cosserat rod.

  Residual vector res = Λ(1) -F +

Fig. 3 :

 3 Fig. 3: Rod of test 1 pulled and released from a weakly deformed configuration: (a) Time-evolution of the tip position obtained with the shooting method applied to the BVP of OCP2 with (blue), and without acceleration equation (red), and with the reference simulator (green). (b) Snapshots illustrating the numerical instability due to ∆t < ∆t ⋆ c .

Fig. 4 :

 4 Fig. 4: Rod of test 1 with k = k ⋆ /100, pulled with an horizontal force and released from a highly deformed configuration: Snapshots (a), and time-evolution of the tip coordinates (b). The color code is the same as in figure 3.a.

Fig. 5 :

 5 Fig. 5: Flying stick: (a) Conditions of the test. (b) Mass center coordinates vs time. (c) Snapshots over 5s. Blue/green: shooting/Lagrangian based simulator.

Fig. 6 :

 6 Fig. 6: (a) Morphology of the swimmer. (b) V 1 (t) given by the FDM (red/shooting) and the IDM (blue/Riccati ODEs).

Fig. 7 :

 7 Fig. 7: Snapshots of a continuum undulatory swimmer torque-controlled with an active constitutive law.

C 3 =

 3 S(X)ρ( V + ΩX ⊥ ) T ( Ω(V + ΩX ⊥ ))dY dZ. (102)

  ∂H ∂u′′ = 0 ⇒ B T Λ ′′ = Λ ′′ ad + (H r ϵ) ′′ = M aa u + B T (D η + EΛ + I),(106)where we introduced the notations:M aa = B T MB , D = M ′ + ad T ξ M -Mad ξ , E = ad T ξ ′ + ad T ξ ad T ξ , I = -F ′ -ad T ξ F -Mad ξ η, F = F + ad T η Mη.(107)

TABLE II :

 II Table of correspondences between the static and dynamic OCPs.

TABLE III :

 III Table of instantiation of the shooting method for our three OCPs.

calculate the residual vector of OCP1, one first integrates (

This choice will allow to model continuous swimmers inspired from fish as addressed in one of the numerical examples of section VII.

This assumption which is systematically made in the Cosserat rod theory typically means that the radius of curvature along the rod is of the order of its length.

Note that this context is also that of a certain class of fluid-actuated continuous robots where tendons are replaced by radially incompressible pressure-controlled tubular chambers[START_REF] Renda | Screw-based modeling of soft manipulators with tendon and fluidic actuation[END_REF].

In the Lagrangian terminology, these reactive forces are the Lagrange multipliers in charge of forcing the constraints.

These terms play no role in Gauss principle since they add meaningless constants to the Gauss constraint.

Referring to OC, where the time replaces X, this is a "time"-variant linear system.

One can deduce this continuous NE model from that of a rigid multibody system, by taking an infinite number of infinitely small bodies.

This publication is supported by the French ANR COSSE-ROOTS under Grants ANR-20-CE33-0001, and partly by the Khalifa University of Science and Technology under Grants CIRA-2020-074, RC1-2018-KUCARS. The first author would like to thank his colleague Georges Levey for their fruitful discussions on the direct dynamics of Cosserat beams in the early 2000s.

As a result, the singular arcs are of degree one, and the expression of u provided by inverting (106), is defined by (56). Then, using (56) in [START_REF] Holsapple | A modified simple shooting method for solving two-point boundary-value problems[END_REF] provides the autonomous BVP (indexes s and c mean "state" and "costate" respectively):

where we introduced the notations:

with G = BM -1 aa B T . But all the entries of P and p, only depend on (ξ, ξ) and not of the state η. As a result, they can be considered as X-dependent functions, and this BVP is linear with respect to ( η, Λ). As such, it can be solved with the "sweep method" [START_REF] Bryson | Applied Optimal Control[END_REF], which postulates the linear relationship between state and costate (57). Now, identifying Λ ′ = (S η + s) ′ with the costate equation of (108), and introducing into this identity, the state equation of (108), as well as the sweep relation (57), one finds:

But such a relation needs to be true for any η ̸ = 0, which provides the Riccati ODEs for S(.) and s(.):

S ′ = P cs -SP ss + P cc S -SP sc S,

Finally, the LQ resolution of OCP2 would progress as follows. First integrate forward (from 0 to 1) the reconstruction equations [START_REF] Lighthill | Aquatic animal propulsion of high hydromechanical efficiency[END_REF], then integrate backward (from 1 to 0) the two Riccati's ODEs, starting with initial conditions = 0 and s(1) = Λ(1) = F + . Once S and s known over [0, 1], one can calculate η by forward integrating the state equation of (108), while calculating the costate field Λ with (57). For a manipulator, this last integration is merely initialised with η(0) = 0, while for a locomotor, we use η(0) = S -1 (0)(Λ(0)-s(0)) = -S -1 (0)(F -+s(0)), which requires the invertibility of S(0).

XI. APPENDIX 3: EXPRESSIONS

AND THEIR JACOBIANS While C(ν 0 ) is directly given by (71), A(ν 0 ), B(ν 0 ) can be detailed as (remind that ν 0 = (Θ T 0 , r T 0 ) T ):

Differentiating the above expressions as well as (71), one can show that: