Frédéric Boyer
email: frederic.boyer@imt-atlantique.fr

Andrea Gotelli
email: andrea.gotelli@ls2n.fr

Philipp Tempel
email: philipp.tempel@ls2n.fr

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

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

Sébastien Briot
email: sebastien.briot@ls2n.fr.

Implicit time integration simulation of robots with rigid bodies and Cosserat rods based on a Newton-Euler recursive algorithm

  L'archive ouverte pluridisciplinaire

I. INTRODUCTION A. State of the art

With the rise of continuum and soft robotics, many recent robotic designs hybridize rigid and soft bodies in complex topologies actuated in a localized or distributed manner [START_REF] Guanlun | Experimental statics calibration of a multi-constraint parallel continuum robot[END_REF], [START_REF] Bottcher | Design of a reconfigurable parallel continuum robot with tendon-actuated kinematic chains[END_REF]. In this context, the statics and dynamics model of Cosserat rods is gradually becoming one of the standards for the analysis, control and design of these systems [START_REF] Boyer | Macro-continuous computed torque algorithm for a three-dimensional eel-like robot[END_REF], [START_REF] Dupont | Design and control of concentric-tube 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]. By Cosserat's rod, we mean a continuous stacking of infinitesimally thin rigid cross-sections along a material line [START_REF] Boyer | Poincare's equations for cosserat media: Application to shells[END_REF]. Cosserat rods theory is a sub-chapter of continuous media mechanics [START_REF] Antman | Ordinary differential equations of nonlinear elasticity i: Foundations of the theories of non-linearly elastic rods and shell[END_REF]. As is always the case, in this more general context, the medium is modelled by a closed set of partial differential equations (PDEs) or "strong form", which consists of the equilibrium of its stresses plus their boundary conditions, a definition of the strains, and the constitutive law that relates the stresses and strains [START_REF] Gurtin | Topics in Finite Elasticity[END_REF]. At this primary stage, the formulation is continuous and governs the evolution of the system in an infinite-dimensional configuration space of kinematic fields (for example, the absolute position or displacement field of a 3D medium). In the case of Cosserat rods, the material medium is onedimensional and we have two independent variables, namely time and a spatial variable, typically the arc-length along the rod. As a consequence of this restriction, whether for a system realised by serial (e.g. a Tendon Actuated Continuum Robot or TACR) or parallel (e.g. a Continuum Parallel Robot or CPR) connection of rods, its strong form defines a boundary value problem (BVP) in space and an initial value problem (IVP) in time. In this form, the simulation problem can be treated with standard numerical solution methods based on collocation, finite differences, spectral integration, shooting algorithm. In the shooting based approach, the forward static spatial BVP is transformed into a sequence of spatial IVPs whose unknown proximal boundary conditions (BCs), are iteratively approximated in a Newton loop until some of the distal BCs coincide with their required values. Applied at each loading step, this method is well suited to the quasi-static simulation of continuum and soft robots [START_REF] Rucker | Statics and dynamics of continuum robots with general tendon routing and external loading[END_REF]. After this success in statics, Till et al. extended the approach to dynamics [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF]. Inspired by the work of the ocean engineering community on towed submarine cables, at each time step of a simulation, the time derivatives of the kinematic fields are removed using an implicit integration scheme. The BVP-IVP in space-time then changes to a simple BVP in space, which can be processed at each of these time steps by the shooting algorithm, in a similar way to the static case. Recently, it has been shown in [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF] that: (1) the forward dynamics BVP on which this approach is based derives from a singular optimal control problem, regularised by the implicit time integration scheme; [START_REF] Bottcher | Design of a reconfigurable parallel continuum robot with tendon-actuated kinematic chains[END_REF] this regularisation works less and less well as the time step and/or beam stiffness decreases; (3) as a consequence, the approach fails in many practical circumstances encountered in soft robotics.

To overcome this issue, we can replace the strong formulation of the original PDEs by the weak formulation of the virtual works in the framework of Lagrangian mechanics [START_REF] Goldstein | Classical Mechanics[END_REF]. In the most employed method of this framework, the configuration of a rod is parameterized with a set of independent vector fields whose components are decomposed on a truncated basis of functions. The direct dynamics problem is then transformed into a finitedimensional optimization problem [START_REF] Meirovitch | Methods of Analytical Dynamics[END_REF], whose stationarity conditions are the Lagrange equations of the system with respect to the coefficients of the basis, which stand for a set of generalised coordinates. Well known as the Ritz, or Rayleig-Ritz, method [START_REF]Dynamics and Control of structures[END_REF], this reduction process is the basis of the finite elements method (FEM) of classical three-dimensional (3D) media as that implemented in SOFA [START_REF] Allard | Sofa-an open source framework for medical simulation[END_REF]. To be applied to Cosserat rods, it was extended from R 3 to the Lie group SO(3), by J. C. Simo and his successors in the framework of the geometrically exact FEM, or GE-FEM [START_REF] Simo | A finite strain beam formulation. the three-dimensional dynamic problem. part i[END_REF], [START_REF] Boyer | Finite element of slender beams in finite transformations: A geometrically exact approach[END_REF], [START_REF] Sonneville | Geometrically exact beam finite element formulated on the special euclidean group se(3)[END_REF]. An efficient Lagrangian alternative to GE-FEM has been developed in the computer graphics community. Called discrete elastic rod approach or DER, it has been designed for fast interactive simulation of hairs and other filaments [START_REF] Bergou | Discrete elastic rods[END_REF], [START_REF] Miklos | Discrete viscous threads[END_REF]. Unlike other Lagrangian approaches, in DER, the discretization (reduction) process is not applied to fields defined on a continuous rod, but from the outset to the rod itself, which is transformed into a discrete rod, i.e. a set of edges separated by vertices [START_REF] Grinspun | Discret Differential Geometry: An Applied Introduction[END_REF]. By redefining all the geometric objects on this discrete manifold and introducing them into the Lagrange equations, we obtain the expected reduced dynamics in a discrete form that resembles that provided by finite differences [START_REF] Bergou | Discrete elastic rods[END_REF]. However, in contrast to usual finite differences, the approach preserves exactly all the intrinsic properties of the original continuous formulation [START_REF] Grinspun | Discret Differential Geometry: An Applied Introduction[END_REF]. First developed for Kirchhoff rods [START_REF] Bergou | Discrete elastic rods[END_REF], [START_REF] Miklos | Discrete viscous threads[END_REF] possibly connected to rigid bodies by the fast projection method (FPM) of [START_REF] Goldenthal | Efficient simulation of inextensible cloth[END_REF], the DER has recently been extended to Cosserat rods [START_REF] Gazzola | Forward and inverse problems in the mechanics of soft filaments[END_REF]. This context is now implemented in PyElastica, a free and open-source software package for the simulation of slender body assemblies in Python [START_REF] Gazzola | Pyelastica[END_REF].

More recently, some of the authors of this article have proposed an alternative Lagrangian approach to GE-FEM and DER. In this approach based on a Ritz reduction, each of the strain fields of a Cosserat rod (stretch, shear, curvature and torsion) is developed on a truncated basis of assumed strain modes [START_REF] Renda | Discrete cosserat approach for multisection soft manipulator dynamics[END_REF], [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF], [START_REF] Mathew | Sorosim: A matlab toolbox for hybrid rigid-soft robots based on the geometric variable-strain approach[END_REF]. Named GVS, for Geometric Variable Strain method in [START_REF] Mathew | Sorosim: A matlab toolbox for hybrid rigid-soft robots based on the geometric variable-strain approach[END_REF], the approach has the advantage of providing models in the usual matrix form of rigid manipulators, while offering good accuracy with a small number of modal (strain) coordinates [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]. So far, the approach has been implemented in two different algorithms, depending on whether the generalised forces of the Lagrangian model are calculated with kinematic Jacobian matrices [START_REF] Renda | A geometric variable-strain approach for static modeling of soft manipulators with tendon and fluidic actuation[END_REF], [START_REF] Mathew | Sorosim: A matlab toolbox for hybrid rigid-soft robots based on the geometric variable-strain approach[END_REF], according to a projective matrix process, often referred to as Kane's method [START_REF] Kane | The use of kane's dynamical equations in robotics[END_REF], or with a Newton-Euler (NE) inverse dynamic algorithm [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]. Named IDM for "inverse dynamic model", this algorithm generalises the computed torque algorithm used in the 1980s to solve the inverse dynamics of rigid manipulators [START_REF] Luh | On-line computational scheme for mechanical manipulator[END_REF]. While in [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF] the approach is restricted to rods serially fixed together, in [START_REF] Mathew | Sorosim: A matlab toolbox for hybrid rigid-soft robots based on the geometric variable-strain approach[END_REF], open, tree and closed loop systems are treated. In the case of kinematic loops, the resulting Lagrangian model takes the form of a set of differential and algebraic equations or DAEs, where the algebraic equations are the geometric constraints of the loop closure [START_REF] Cardona | Rigid and flexible joint modelling in multibody dynamics using finite elements[END_REF]. Despite these differences, when applied to forward dynamics (simulation), all these approaches based on modal strain reduction use explicit time integration schemes. That is, schemes known to be much less stable than implicit schemes when applied to the generally "stiff problems" of nonlinear structural dynamics [START_REF] Cardona | Time-integration of the equations of motion in mechanism analysis[END_REF].

B. Contributions of the article

As [START_REF] Mathew | Sorosim: A matlab toolbox for hybrid rigid-soft robots based on the geometric variable-strain approach[END_REF], the present article addresses the dynamic modeling and simulation of hybrid systems composed of Cosserat rods and rigid bodies in arbitrary topologies with the modal strain reduction. However, unlike all previous work based on this approach, the forward dynamics of soft systems are here integrated in time using an implicit unconditionally stable scheme. This choice strongly structures the simulation algorithm as it requires not only to compute the dynamic model of the robot, but also its tangent dynamics. The first is required to calculate the residual vector of the original DAEs constrained by the integration scheme, and the second to calculate the Jacobian of this vector. This extra effort, compared to explicit integration, has been accomplished by GE-FEM [START_REF] Simo | A finite strain beam formulation. the three-dimensional dynamic problem. part i[END_REF] and DER [START_REF] Miklos | Discrete viscous threads[END_REF]. In the case of strainparameterized models here considered, this extension is particularly challenging. Indeed, the use of relative variables (strains) instead of the absolute variables of GE-FEM or DER, increases the non-linearities of the model, which are then transferred from the stiffness matrix to the mass matrix. To address this issue, the article refers to rigid robots dynamics, for which efficient algorithms working in relative (joint) coordinates have been developed since the 1980s in the well-known Newton-Euler framework [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF]. To this end, the IDM of [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF] will be hybridized with that of rigid multibody-systems [START_REF] Luh | On-line computational scheme for mechanical manipulator[END_REF] in a single unified algorithm that generalizes the Lie group recursive formulation of rigid robot dynamics [START_REF] Park | A lie group formulation of robot dynamics[END_REF], [START_REF] Kim | Lie group formulation of articulated rigid body dynamics[END_REF]. Going one step further, applying an exact differentiation to this IDM considered as an inputoutput map, provides a second algorithm named T IDM for "tangent inverse dynamic model". By feeding these two new algorithms with inputs compatible with the implicit time integration scheme, in this case a one-step integrator of the Newmark class, both the residual vector and its Jacobian matrix can be computed in the Newton correction loop of a general predictor-corrector algorithm. Like in the GE-FEM of [START_REF] Cardona | Rigid and flexible joint modelling in multibody dynamics using finite elements[END_REF] and the FPM of [START_REF] Goldenthal | Efficient simulation of inextensible cloth[END_REF], the case of closed loops is treated in its index-3 formulation1 , which, compared to Baumgart's index-1 method [START_REF] Baumgarte | Stabilization of constraints and integrals of motion in dynamical systems[END_REF], used in [START_REF] Mathew | Sorosim: A matlab toolbox for hybrid rigid-soft robots based on the geometric variable-strain approach[END_REF] after [START_REF] Armanini | Discrete cosserat approach for closed-chain soft robots: Application to the fin-ray finger[END_REF], has the advantage of providing solutions that do not depend on the user's choice of some stabilisation parameters, while satisfying the constraints exactly [START_REF] Cardona | Numerical integration of second order differential-algebraic systems in flexible mechanism dynamics[END_REF]. At the end, the proposed dynamic simulation algorithm generalizes the quasi-static simulation algorithm of [START_REF] Briot | A geometrically exact assumed strain modes approach for the geometrico-and kinemato-static modelings of continuum parallel robots[END_REF] and [START_REF] Tummers | Cosserat rod modeling of continuum robots from newtonian and lagrangian perspectives[END_REF], recently proposed for CPRs and TACRs respectively. In particular, like in [START_REF] Tummers | Cosserat rod modeling of continuum robots from newtonian and lagrangian perspectives[END_REF], the differential properties of the IDM and T IDM are exploited in order to achieve all the space integrations of generalized forces with spectral methods [START_REF] Trefethen | Spectral methods in MATLAB[END_REF]. The general algorithm of the article is illustrated through numerical examples, both inspired from the GE-FEM literature of Cosserat rods and of some recent robotics applications.

C. Stucture of the article

The article is structured as follows. Section II presents the Lagrangian parametrisation and the corresponding dy- namic model of systems discussed in the paper systems.

In Section III, we present the implicit time integration scheme used by the approach, while section IV presents the general prediction-correction algorithm in which this scheme is integrated for the purpose of dynamic simulation. The central calculation of this algorithm is a Newton loop that requires the calculation of the residual vector of the dynamics and its Jacobian matrix (constrained by the integrator) as explained in Section V. These calculations are based on a Newton-Euler recursive inverse dynamics algorithm, and its differential, or tangent inverse dynamics recursive algorithm. The NE model is presented in Section VI, while the two algorithms are detailed in Section VII and used in section VIII to compute the residual vector and its Jacobian. These algorithms require to space integrate the continuous kinematics and stress along the structure as explained in section IX. Numerical illustrations are reported in Section X, while the paper ends with some perspectives in the conclusion section XI.

II. LAGRANGIAN MODEL OF A SMMS

A. Systems addressed

We consider a soft mobile multibody system (SMMS) (see figure 1). Such a system consists of rigid bodies and Cosserat rods connected by fixed or revolute joints 2 which can be passive or torque actuated. The rods can be passive or internally actuated through distributed actuation systems as cables or pressurized fluids. The system can have a simple open, tree-like or closed topology, with kinematic loops. The system can be a manipulator or a locomotor depending whether it has one of its bodies (named the basis) connected to the ground, or not.

B. Lagrangian parametrization of a SMMS

If the considered SMMS contains some kinematic loops (see figure 1 (b,d)), then we start by virtually cutting3 each of these loops at the level of a passive joint (that can be fixed) 4 . Then, the configuration of the so defined tree-like SMMS is parameterized as follows. Any localized joint of revolute type is parameterized by one angle, and the set of all these angles is gathered in the (n r × 1) vector denoted q r . Regarding the rods, we use the reduced strain parameterization of [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF] i.e., we decompose the allowed strain field ϵ j of each Cosserat rod of index j and length at rest l j , on a Ritz basis according to:

ϵ j (X) = Φ j (X)q ϵ,j , ∀X ∈ [0, l j ], (1)
with X the arc-length along the rod in a resting configuration, Φ j a matrix of functions (typically polynomials), and q ϵ,j ∈ R nϵ,j , the vector of strain (generalized) coordinates of rod j. Gathering all the strain vectors q ϵ,j defines an (n ϵ × 1) vector, noted q ϵ . When the SMMS is a manipulator, its configurations or "shapes", are entirely parameterized by the generalized coordinates vector q = (q T r , q T ϵ ) T of size (n × 1) with n = n r + n ϵ . In contrast, if the SMMS is a locomotor, it is not only subject to time-variations of its internal degrees of freedom (d.o.f) parameterized by q, but also to some net rigid motions. These additional external d.o.f require a further set of coordinates that parameterize the pose (position-orientation) of a frame F 0 attached to one of the rigid bodies of the SMMS, that takes the meaning of a reference body B 0 . This rigid body can be a full (3D) rigid body, or a rigid cross sections at one of the tips of a Cosserat rod. The pose of this reference rigid body is parameterized by a homogeneous transformation g 0 ∈ SE(3). Finally, in the most general case, the set of Lagrangian coordinates is defined as a pair (g 0 , q) i.e., an element of the configuration space SE(3) × S with S ∼ = R n the shape space of the SMMS. With this definition of the configuration space, the velocities of the SMMS are described at any time t, with a vector (η T 0 , qT ) T of R 6+n , where η 0 = (g -1 0 ġ0 ) ∨ ∈ se(3) ∼ = R 6 is the twist of B 0 in its mobile frame F 0 , and a dot indicates d./dt.

Remark 1: Note that a same closed-loop SMMS can be modelled by different tree-like systems depending on the cuts chosen. To illustrate this, consider a planar CPR like the one in figure 1 (d), where one of the upper joints is actuated, while all the others are passive. The system can be cut at the upper passive joint (shown in red in figure 1 (d)) and the ground defined as the base B 0 of a twoarmed manipulator. Alternatively, all the lower joints can be cut off (shown in blue in figure 1 (d)) and the upper rigid platform defined as the reference body B 0 of a two legged locomotor ■

C. Lagrangian dynamic model of a SMMS

Once the configuration space of a general SMMS is defined, applying one of the variational principles of dynamics as D'Alembert's principle of virtual works, or Hamilton's principle of least action, provides its dynamics as a set of differential-algebraic equations of the form [START_REF] Cardona | Rigid and flexible joint modelling in multibody dynamics using finite elements[END_REF]:

0 6×1 Q ad = M 00 M 0q M q0 M qq η0 q + F Q , Φ(g 0 , q) = 0 m×1 . (2)
The bottom (algebraic) equations define a set of m independent constraints imposed by the closure loops, which in the general case can depend on both g 0 and q depending on the topology of the system after cuts and the choice of B 0 (see Remark 1). The time-differential of these geometric constraints defines the kinematic form of constraints:

Φ = J 0 J q η 0 q = 0 m×1 , (3)
where J q (g 0 , q) and J 0 (g 0 , q) are some kinematic Jacobian matrices defined by the usual derivation in R n , for the first:

J q q = ∂Φ ∂q q, (4)
and by the directional derivative on SE(3) for the second:

J 0 η 0 = d dϵ ϵ=0 Φ(g 0 exp(ϵη 0 ), q), (5)
with "exp", the exponential map of SE(3). In the top (differential equations) of (2), from left to right, one finds: (1) The vector of generalized internal actuation forces Q ad = (τ T d , Q T adϵ ) T including the localized joint torques τ d (t) as well as the distributed actuation generalized forces Q T adϵ (q ϵ , t) of a set of cables for instance. [START_REF] Bottcher | Design of a reconfigurable parallel continuum robot with tendon-actuated kinematic chains[END_REF] The q-dependent symmetric matrix of generalized inertia. (3) The vector of generalized accelerations. (4) The vector

(F T , Q T ) T = (F T v , Q T v ) T + (F T c , Q T c ) T , with (F T v , Q T v )
T (q, η 0 , q) the velocity-dependent generalized forces including the effects of Coriolis and centrifugal accelerations, and (F T c , Q T c ) T , the vector of configurationdependent forces. This vector can be detailed as:

F c Q c = F g Q g + Q e - J T 0 J T q λ, (6)
where (F T g , Q T g ) T (g 0 , q) is the vector of gravity forces, while Q e = (0 1×nr , Q T ϵ ) T stands for the internal restoring (elastic) forces, with Q ϵ (q ϵ ) = K ϵϵ q ϵ , and K ϵϵ , the matrix of generalized (constant) stiffness. In (6), J T 0 λ (respect. J T q λ) is the wrench in se(3) * ∼ = R 6 (respect. the vector of internal generalized forces in R n ) induced by the reaction forces λ imposed by the m constraints of loops.

Remark 2: Although limited to time-independent holonomic constraints of the form (2), all the results of the article can be extended without conceptual difficulty to constraints of the more general form [START_REF] Cardona | Rigid and flexible joint modelling in multibody dynamics using finite elements[END_REF]:

Φ(g 0 , η 0 , q, q, t) = 0 m×1 , (7)
where the components of Φ that depend on velocities (η 0 , q) can be non-integrable with respect to time i.e., non-holonomic [START_REF] Boyer | Reduced dynamics of the nonholonomic whipple bicycle[END_REF] ■ Remark 3: Using notation χ = (g 0 , η 0 , η0 , q, q, q, λ), the integration of the dynamics of the SMMS from a set of initial conditions at t = t 0 , consists in finding a timeevolution:

t ∈ [t 0 , +∞[ → χ(t) ∈ R 3×6+3n+m , solution of: R(χ(t), t) = 0 (6+n+m)×1 , (8)
where R defines the dynamic residual vector of the SMMS, which can be deduced from (2) as:

R =   R 0 R q R λ   =   M 00 η0 + M 0q q + F M q0 η0 + M qq q + Q -Q ad (t) Φ   .
(9) Note that this residual vector is continuous in time and needs to be discretized for numerical simulation purpose. This discretization is based on an implicit integration approach as detailed in next section ■

III. TIME INTEGRATION

In the context of simulation, a time-integrator allows approximating (8) along a sequence of time instants {t k } k∈N + , with t k+1 = t k + ∆t and ∆t a time-step that we consider constant. For reasons of stability and simplicity, we adopt an implicit one-step scheme of the Newmark type [START_REF] Hughes | Finite Element Method-Linear Static and Dynamic Finite Element Analysis[END_REF]. Specifically designed for second order ODEs of Newtonian mechanics, this scheme has been refined over the years to meet the specific needs of structural dynamics [START_REF] Cardona | Time-integration of the equations of motion in mechanism analysis[END_REF]. Defined at the origin on a vector space, it can be directly applied to the generalized coordinates q ∈ R n of the internal d.o.f. of a SMMS. In this case, the scheme requires any (q, q, q) candidate to be a solution of (8) at a time t n+1 , to satisfy the usual implicit relations:

q = a(q -q (n) ) + f (n) q , q = b(q -q (n) ) + h (n) q . ( 10
)
Here q (n) denotes the value of q at t n (a notational convention that will be systematically used in what follows), a, b are two scalars that depend on the time-step ∆t, while f

(n) q = f ( q(n) , q(n) ), h (n) q
= h( q(n) , q(n) ), with f and h two vector functions fixed by the scheme and reminded in Appendix 1. As detailed in [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF], this integrator can be extended to the external d.o.f. of g 0 ∈ SE(3). To that end, we impose to any (g 0 , η 0 , η0 ) candidate to be solution of (8) at t n+1 , to fulfill the Newmark scheme on SO(3) × R 3 :

Ω 0 ṙ0 = a Θ 0 d 0 + f (n) θ f (n) r , Ω0 r0 = b Θ 0 d 0 + h (n) θ h (n) r , (11)
where

g 0 = (R 0 , r 0 ), R 0 ∈ SO(3), r 0 ∈ R 3 , η 0 = (Ω T 0 , V T 0 ) T ,
with Ω 0 and V 0 = R T 0 ṙ0 the angular and linear velocities of B 0 in the body frame, while we introduced a new vector

ν 0 = (Θ T 0 , d T 0 ) T ∈ so(3) × R 3 ∼ = R 6 : R 0 = R (n) 0 exp( Θ0 ) , r 0 = r (n) 0 + d 0 , (12)
with "exp" here denoting the exponential map of SO(3).

Omitting the upper indices, in ( 11), (f θ , h θ ) and (f r , h r ) are defined similarly to (f q , h q ) of ( 10), but with (Ω 0 , Ω0 ) and ( ṙ0 , r0 ) replacing ( q, q) respectively (see Appendix 1). Introducing ( 11) and ( 12) into the kinematic definitions of (g 0 , η 0 , η0 ) in terms of R 0 and r 0 , allows expressing them at any time beyond t n , in terms of ν 0 only [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF]:

η 0 = A(ν 0 ) , η0 = B(ν 0 ) , g 0 = C(ν 0 ), (13)
where the detailed expressions of A, B, C are reminded in Appendix 1. Finally, thanks to the Newmark integrator, the search for (g 0 , η 0 , η0 , q, q, q, λ) at any time t n+1 , is reduced to that of (ν 0 , q, λ).

Remark 4: In the subsequent numerical resolution, one will need to use the increments of velocities and accelerations compatible with the Newmark integration. To get them, it suffices to apply the variation ∆ to (10):

∆ q = a∆q , ∆q = b∆q. (14)
Similarly, defining the differential of g 0 as ∆ζ 0 = (g -1 0 ∆g 0 ) ∨ ∈ se(3), the ∆-variation applied to ( 13) gives [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF]:

∆η 0 = ∂A ∂ν 0 ∆ν 0 , ∆ η0 = ∂B ∂ν 0 ∆ν 0 , ∆ζ 0 = ∂C ∂ν 0 ∆ν 0 , (15)
whose detailed expressions are reminded in Appendix 1 ■

IV. NUMERICAL RESOLUTION

Based on this approximation, the numerical resolution is achieved by induction as follows. Assuming we know χ (n) compatible with the constraints (10-13) and solution of (8) at t n , we have to find a χ (n+1) compatible with the same constraints, and solution of (8) at t n+1 . To that end, we first introduce (10-13) into ( 8). This changes the system of DAEs [START_REF] Antman | Ordinary differential equations of nonlinear elasticity i: Foundations of the theories of non-linearly elastic rods and shell[END_REF], into the algebraic system:

R(n) ( χ, t n+1 ) = 0 (6+n+m)×1 , (16)
whose root χ = (ν 0 , q, λ) ∈ R 6+n+m defines χ(n+1) , and the over-bar indicates that the residual vector is now constrained by the integrator. In details, we have:

R(n) = R(n)T 0 , R(n)T q , Φ T T , (17)
with upper index (n) keeping trace of the dependence to (g 10) and [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF]. Now remark that ( 16) is a square system of algebraic nonlinear equations that can be addressed with an iterative root finder, • Backward dynamics : eq. (42-44), [START_REF] Khalil | Modeling Identification and Control of Robots[END_REF] • Projections of wrenches: eq. (63,64).

(n) 0 , η (n) 0 , η(n) 0 ) and (q (n) , q(n) , q(n) ) in (
• Tangent forward kinematics : eq. (48-50)

• Tangent backward dynamics : eq. (51-54), (56-59)

• Projections of wrenches variations: eq. (65,66) ? Update from with ( 10), [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF] • Loop closure kinematics : eq. [START_REF] Pogorelov | Differential-algebraic equations in multibody system modeling[END_REF][START_REF] Armanini | Discrete cosserat approach for closed-chain soft robots: Application to the fin-ray finger[END_REF] Fig. 2: Flowchart of the "predictor-corrector" simulation algorithm. The calculation of the residual vector and its Jacobian are based on the IDM and T IDM . The overbar indicates that these algorithms are fed by inputs that fulfill the constraints (10-13) and [START_REF]Dynamics and Control of structures[END_REF][START_REF] Allard | Sofa-an open source framework for medical simulation[END_REF], imposed by the integration scheme. The lower star defines some adaptations of these two algorithms allowing to calculate directly R(n) ( χ, t n+1 ) and its differential, as detailed in section VIII.

as the Newton algorithm. In this latter case, the algorithm solves at each iteration the linearized system tangent to ( 16):

χ+ = χ - ∂ R(n) ∂ χ -1 R(n) ( χ, t n+1 ), (18)
where the index "+", denotes the value of χ corrected by the current Newton iteration. In this linear system, (∂ R(n) /∂ χ) is the Jacobian matrix of the residual vector constrained by the integrator, which details as:

∂ R(n) ∂ χ =     ∂ R(n) 0 ∂ν0 ∂ R(n) 0 ∂q J T 0 ∂ R(n) q ∂ν0 ∂ R(n) q ∂q J T q J 0 J q 0     . (19)
Note that after convergence of the Newton loop, the full set χ is updated from the converged χ by using [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF][START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF][START_REF] Goldstein | Classical Mechanics[END_REF][START_REF] Meirovitch | Methods of Analytical Dynamics[END_REF]. Furthermore, at each time step t n , (g 0 , q) of χ = (g 0 , q, λ) can be initialized in the Newton algorithm, with one of the usual predictors of the Newmark scheme, as for instance, the "inertial" or "ballistic" predictor, which is defined by imposing ( Ω0 , r0 , q) = (0 3×1 , 0 3×1 , 0 n×1 ) in ( 10) and ( 11), while for λ, one can use its value at the previous step t n-1 . Finally, the dynamic simulator is structured by two nested loops, one global time-loop (the Newmark loop) and the other (Newton loop) that solves [START_REF] Simo | A finite strain beam formulation. the three-dimensional dynamic problem. part i[END_REF] (see figure 2). Note that while this secondary loop runs, the time is frozen and incremented by the Newmark loop only after the Newton loop has converged.

Remark 5: At each iteration of the Newton loop, the resolution of ( 18) requires the computation of R(n) and

(∂ R(n) /∂ χ).
In order to avoid the heavy symbolic computations, we propose to compute R(n) implicitly, by applying the Newton-Euler (NE) approach of robotics [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF]. To do so, we will use the inverse algorithm of SMMS, named IDM for "inverse dynamic model". Moreover, although the Jacobian (∂ R(n) /∂ χ) can be evaluated numerically by finite difference approximations (as done by defaults, by Matlab's fsolve function, for instance), we propose here a more accurate computation of this matrix, based on another Newton-Euler algorithm named T IDM , for "tangent inverse dynamic model" ■

V. CALCULATION OF THE RESIDUAL VECTOR AND ITS JACOBIAN MATRIX

To introduce the NE computational process of the residual vector and its Jacobian matrix, let us reconsider the upper ODEs of (2), in which Q e is moved from the right to the lefthand side, and the vector (0 1×6 , (Q ad -Q e ) T ) T , is replaced by a full vector of fictitious actuation forces (F T a , Q T a ) T . This defines an (inverse) Lagrangian model of the form:

F a Q a = M 00 M 0q M q0 M qq η0 q + F v + F g -J T 0 λ Q v + Q g -J T q λ . ( 20
)
As this is the case of rigid manipulators [START_REF] Luh | On-line computational scheme for mechanical manipulator[END_REF], we will see soon (Section VII) that the Lagrangian model ( 20) can be realized alternatively by an inverse Newton-Euler algorithm in two pass, which formally reads:

F a Q a = IDM (χ), (21)
where remind that χ = (g 0 , η 0 , η0 , q, q, q, λ). Physically, this IDM computes for any state (g 0 , η 0 , q, q), the wrench F a and the full vector of internal forces Q a (applied on all the entries of q = (q T r , q T ϵ ) T ), that would be required to ensure the SMMS, considered as fully actuated, to accelerate at ( η0 , q), while being subject to the external forces λ applied at the tip of the cut branches. Now using the identity ( 21) in ( 16), the two top block-components of the residual vector of the SMMS constrained by the Newmark integrator read:

R(n) 0 R(n) q ( χ, t n+1 ) = IDM ( χ) - 0 6×1 Υ(q, t n+1 ) , (22)
where the overbar indicates that the IDM is now fed with inputs compatible with the integrator constraints [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF][START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF] i.e., with inputs that depend on χ only, while we used the further notation:

Υ(q, t) = Q ad (q ϵ , t) -Q e (q ϵ ). (23)
In summary, one can compute ( R(n) 0 , R(n) q )( χ, t n+1 ) with the IDM . To this end, it suffices to force its inputs to fulfill the constraints of the integrator [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF][START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF], and to remove (0 1×6 , Υ(q, t n+1 ) T ) T from its vector of outputs. Remarkably, the same idea can be applied to the computation of the top two block-rows of (∂ R(n) /∂ χ). Indeed, differentiating [START_REF] Goldenthal | Efficient simulation of inextensible cloth[END_REF] provides the identity:

∆ R(n) 0 ∆ R(n) q =   ∂ R(n) 0 ∂ χ ∂ R(n) q ∂ χ   ∆ χ = T IDM ( χ, ∆ χ) - 0 6×1 ∆Υ(q, ∆q, t n+1 ) , (24)
where:

∆Υ(q, ∆q, t) = ∆Q ad (q ϵ , ∆q ϵ , t) -∆Q e (q ϵ , ∆q ϵ ), [START_REF] Renda | Discrete cosserat approach for multisection soft manipulator dynamics[END_REF] with ∆Q e = K ϵϵ ∆q ϵ , and ∆Q ad = (∆τ d (t) T , ∆Q T adϵ ) T = (0 1×nr , ∆Q T adϵ ) T , since the time is frozen in the correction Newton's loop, while Q adϵ can depend on q ϵ [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]. In [START_REF] Gazzola | Pyelastica[END_REF], T IDM denotes the tangent algorithm to the IDM or T IDM , with the over-bar indicating that it is fed with inputs compatible with the constraints [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF][START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF] and [START_REF]Dynamics and Control of structures[END_REF][START_REF] Allard | Sofa-an open source framework for medical simulation[END_REF] imposed by the integrator. Formally, this further NE algorithm is defined by the differential of the input-output map [START_REF] Grinspun | Discret Differential Geometry: An Applied Introduction[END_REF]:

∆F a ∆Q a = T IDM (χ, ∆χ), (26)
which for any χ = (g 0 , η 0 , η0 , q, q, q, λ), allows to compute the variations of outputs ∆F a and ∆Q a required by imposing variations ∆χ = (∆ζ 0 , ∆η 0 , ∆ η0 , ∆q, ∆ q, ∆q, ∆λ) as inputs. Now let us define δ i ∈ R 6+n+m as the unit vector, with zero entries, except the i th , which is equal to one. Then, the identity [START_REF] Gazzola | Pyelastica[END_REF] shows that feeding T IDM with χ and ∆ χ = δ i , i = 1, 2...., 6 + n + m, and removing from its outputs, (0 1×6 , ∆Υ T ) T fed with the same inputs (see next remark), allows to compute column after column, the top two block-rows of (∂ R(n) /∂ χ) in any χ.

Remark 6: Before detailing the IDM and T IDM algorithms of a SMMS, it is worth mentioning that if we have at our disposal these two algorithms, exploiting the two identities ( 22) and ( 24) to compute their left-hand sides, requires to calculate Υ(t n+1 ) = Q ad (t n+1 ) -Q e and ∆Υ(t n+1 ) = ∆Q ad (t n+1 )-∆Q e (for the sake of concision, we will often indicate the time dependency, only). Moreover, using the identity ( 22) only provides R(n) 0 and R(n) q in (9), and not R(n) λ = Φ(g 0 , q). On the other hand, owing to the symmetries of the block matrix [START_REF] Bergou | Discrete elastic rods[END_REF], calculating its two top rows with the identity ( 24) is enough to reconstruct the full Jacobian (∂ R(n) /∂ χ). As we will see later (see section VIII), the missing vectors Υ(t n+1 ), ∆Υ(t n+1 ), and R(n) λ = Φ(g 0 , q) can also be computed with some of the subparts of the IDM and T IDM . Therefore, these two algorithms are the key of the computation of the residual vector and the Jacobian of [START_REF] Sonneville | Geometrically exact beam finite element formulated on the special euclidean group se(3)[END_REF]. Since they are both based on the Newton-Euler model of SMMS, we will now introduce this model ■

VI. NEWTON-EULER MODEL OF A SMMS

As per usually, a rigid link is modelled by a rigid 3D body of arbitrary shape (not necessarily rod-shaped). On the other hand, in order to easily hybridize the IDM of rigid multibody systems [START_REF] Luh | On-line computational scheme for mechanical manipulator[END_REF], with that of Cosserat rods [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF] into a single generalized IDM , it is convenient to consider a Cosserat rod as two tip cross-sections (i.e., two rigid bodies with no thickness and no inertia), connected by a distributed flexible joint. Therefore, the continuous model of poses, velocities, acceleration and stress of Cosserat rods is used in the model of joints and not bodies. As we will soon see, by adopting this choice, the resulting generalized IDM will, at the highest level, take a form similar to that of the rigid case.

A. Segmentation of a SMMS

Using the above viewpoint, the tree-like SMMS of section II is first segmented into a rigid body sequence consisting of the original 3D rigid bodies and the tip sections of the Cosserat rods. All these rigid bodies are indexed following the usual Newton-Euler conventions i.e., from the reference body B 0 , which is the fixed basis for a manipulator, or the free-floating one of a locomotor, to B N , with bodies B 0 , B 1 , ...B N numbered increasingly while descending the branches. As in any tree-like system, a body has only one antecedent and possibly several successors. In the subsequent developments, j preferentially denotes the current body index of the algorithm, while k = a(j) is its antecedent and l is the index of one of its successors i.e., l ∈ s(j), where s(j) is the set of the indexes of all the successors of B j . As for rigid systems, any two contiguous bodies are separated by a joint. Indexing each of these joints with the index of its succeeding body, one can form the set of joints J 1 , J 2 , ...J N , some of them being rigid lumped joints (fixed or revolute) as those met in rigid systems, while others are soft distributed joints modelled by Cosserat rods. This segmentation process is illustrated in figure 3.

B. Frames

According to the standard uses of NE's approach, each of the rigid bodies that belongs to the SMMS is equipped with a frame rigidly attached to it and positioned according to some conventions. With the segmentation introduced above, this concerns both the rigid bodies (3D or rod end cross-sections), and the distributed joints modelled by Cosserat rods. Regarding distributed joints, we adopt for each of them, say J j , the convention of Cosserat rods and attach to each cross-section labelled by its reference arclength X ∈ [0, l j ], a frame F j (X) = (O j , s j , n j , a j )(X) where O j (X) is centered on the X-cross-section, and s j (X) is its unit normal. Regarding rigid bodies, if B j is a 3D body, we adopt the usual Newton-Euler conventions of rigid systems [START_REF] Khalil | Modeling Identification and Control of Robots[END_REF] i.e., a body frame F j = (O j , s j , n j , a j ) is positioned on J j , with a j , supported by the joint axis if J j is revolute. If B j is a 2D body, this is one of the two end cross-section of a Cosserat rod, and its body frame is that used in the model of the corresponding distributed joint. If a body B j is at the end of a branch resulting of the virtual cut of a loop, a second frame, other than F j , noted F + j = (O + j , s + j , n + j , a + j ) is attached to B j with O + j located on the cut, and a + j supported by the axis of the cut joint, when it is revolute. Finally, the Euclidean space is equipped with an inertial frame F e . In the rest of the article, for any vector (or tensor) V mechanically related to a body B j , i V j denotes the matrix of its components in the frame F i , while for the sake of concision, the upper-left index is omitted when F i = F j i.e., j V j = V j . For similar reasons of concision, the pose of a frame F j with respect to another F k is denoted k g j ∈ SE(3), while when F k = F e , the index e is omitted i.e., e g j = g j , and the pose is said "inertial".

Remark 7: Note that some conflicts between the frame placement conventions of Cosserat beams and those of rigid robots may occur, e.g. when a rod is preceded by a rotary joint of negligible inertia. However, such conflicts can always be removed by declaring a fictitious rigid body supported by the revolute joint and fixed to the first cross section of the following rod. This explains why we have introduced the a priori unnecessary fixed joints in our description ■

C. Newton-Euler model of rigid bodies

In the Newton-Euler approach, the configuration of each rigid body B j is defined by the inertial pose of its body frame i.e., g j = (R j , r j ) ∈ SE(3), with R j and r j the orientation matrix and origin's position of F j , in F e . Introducing the inertial velocity twist of B j in the body frame η j = (g -1 j ġj ) ∨ ∈ R 6 , and applying Newton's laws and Euler's theorem to each body B j isolated in the structure, provides the usual Newton-Euler equations that govern the time evolution of the g j s on SE(3):

• NE dynamic balance (ODEs) of rigid bodies:

M j ηj -ad T ηj M j η j = F ext,j + F j - l∈s(j) j F l . (27)
In these dynamic balances, M j ∈ R 6×6 is the (screw) inertia matrix of an arbitrary shaped rigid body B j (equal to zero if B j is a rod end cross-section), F ext,j ∈ R 6 is the wrench of external forces (e.g gravity, external contacts) exerted on it, F j ∈ R 6 is the wrench of internal contacts exerted by the antecedent body B k , (k = a(j)) onto B j , all these vectors and tensors being expressed in the mobile body frame F j . With these definitions and our notational conventions, the wrenches in the sum of ( 27) are contact wrenches transmitted by B j to all its successors B l through the joints that connect them. Therefore they are first defined in the F l frames by F l , and then expressed in F j through j F l , while action-reaction principle motivates the minus sign.

Remark 8: When a rigid body B j is preceded by an actuated lumped (revolute) joint J j , the projection of the contact wrench F j transmitted by the joint from B j-1 to B j , along the joint axis a j , is no more than the motor torque:

Q ar,j = A T j F j , (28)
where A j = (0, 0, 1, 0, 0, 0) T ∈ R 6 is the unit twist supporting the localized rotation of the revolute joint. Note that according to the common uses in rigid robotics, (28) stands for a constitutive law of the joint, that can be supplemented with a model of friction [START_REF] Khalil | Modeling Identification and Control of Robots[END_REF].

D. Newton-Euler model of soft joints

In the rest of the article, the temporal and spatial derivations ∂./∂t and ∂./∂X are designated by a "dot" and a "prime" respectively. In the Newton-Euler approach, the configuration of a Cosserat rod J j is defined by the inertial pose of all its cross-sectional frames g j (X) = (R j , r j )(X) ∈ SE(3), ∀X ∈]0, l j [, whose time and space variations are described by the two twists (in R 6 ): η j (X) = (g -1 j (X) ġj (X)) ∨ , and ξ j (X) = (g -1 j (X)g ′ j (X)) ∨ , both being expressed in the cross-sectional frames. Then, applying Newton's laws and Euler's theorem along such a rod, provides the following closed formulation that governs the time-evolution of g j (.):

• NE dynamic balance of X-cross sections (PDEs), ∀X ∈ ]0, l j [:

M j ηj -ad T ηj M j η j = Fext,j + Λ ′ j -ad T ξj Λ j , (29)
• Boundary conditions:

Λ j (0) = -F a(j) , Λ j (l j ) = -F j . (30)
• Active constitutive law along [0, l j ]:

B T j Λ j = B T j Λ d,j (t) + (B T j H j B j )ϵ j . (31)
In these equations, ϵ j (X) = B T j (ξ j -ξ o j )(X) is the strain field at X, with B j a matrix (with zero and unit entries) selecting the strain components allowed by the adopted beam kinematics (e.g. Reissner, Kirchhoff...), and ξ o j (X) the value of ξ j (X) in a reference (stress-less) configuration indexed by o. Fext,j (X) ∈ R 6 , M j (X) ∈ R 6×6 and H j (X) ∈ R 6×6 are the density of external wrenches, this of inertia (screw) matrix, and the stiffness (screw) matrix along the rod at X, while Λ j (X) is the wrench of stress exerted by the piece of rod [X, l j ] onto the piece [0, X] across the X-cross section, all these vectors and tensors being expressed in the cross-sectional frames of the rod. Finally, Λ d,j (X, t) ∈ R 6 , is a stress wrench modelling the effect of the distributed actuation (if any), across the X-cross-section of J j as illustrated by the following remark.

Remark 9: In the case of a rod J j actuated by a set of p j tendons of index (α = 1, 2..., p j ), with routings defined by their positions D j,α (X) ∈ R 3 in the X-cross-sectional frames, Λ d,j can be detailed as [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]:

Λ d,j = pj α=1 1 ∥Γ j,α ∥ D j,α × Γ j,α Γ j,α T j,α (t), (32)
where T j,α is the tension exerted on the tendon α, and

Γ j,α = Γ j + K j × D j,α + D ′ j,α , with ξ j = (K T j , Γ T j ) T .
In general, the model of Λ d,j depends on ϵ j and needs to be derived on the case by case basis ■ Remark 10: The above constitutive law can be adapted to approximate the material friction between cross-sections with a simple viscous damping model. To this end, it suffices to change [START_REF] Cardona | Rigid and flexible joint modelling in multibody dynamics using finite elements[END_REF] into:

B T j Λ j = B T j Λ d,j (t) + (B T j D j B j ) εj + (B T j H j B j )ϵ j , (33
) where D j (X) ∈ R 6×6 is the damping matrix along the rod at X. Using the Kelvin-Voigt model, it is convenient to impose D j = µ j H j , with µ j a scalar damping coefficient ■

E. Kinematic model of joint connections

Using the segmentation of Section VI.A, a joint connecting any two consecutive rigid bodies B k and B j imposes the relation between their poses:

g j = g k k g j . (34)
In details, according to the standard uses of rigid robots, when J j is a rigid lumped joint, the relative pose k g j in ( 34) is a function of q rj or is fixed by design, depending whether the joint is revolute or fixed. When J j is a (distributed) soft joint, following [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF], the internal rod kinematics (Reissner, Kirchhoff...) imposes to any two infinitely close crosssections along the joint, the differential relation between their poses:

g ′ j = g j (ξ o j + B j ϵ j ) ∧ , (35)
where the strain field ϵ j (.) of ( 35) is reduced on a strain (polynomial) basis with the Ritz approximation (1) in which remind that the vector of strain coordinates of J j q ϵ,j ∈ R nϵ,j , defines a sub-part of q ϵ . In the following NE algorithm, [START_REF] Park | A lie group formulation of robot dynamics[END_REF] and ( 35) are used to relate the inertial poses g j and g j (.) to the SMMS generalized coordinates q = (q T r , q T ϵ ) T of the Lagrangian parametrization of Section II. Finally, remark that the cut revolute joint of a closed loop will not introduce such kinematic parameters, but rather Lagrange multipliers in charge of forcing a set of closure kinematic constraints that we are now going to detail.

F. Kinematic model of loop closures

Let us consider two terminal bodies B p and B s connected by a revolute joint virtually cut when opening a loop. Then, using the convention that when s > p, F p+ is the leader frame and F s+ is the follower one, a virtually cut revolute joint imposes to the relative pose p+ g s+ to be compatible with the five scalar geometric constraints:

Φ s|p = ĀT p+ log( p+ R s+ ) p+ r s+ = ĀT p+ 0 3×1 0 3×1 , (36)
where the log-map stands for the inverse of the exponential in SO(3) and Āp+ ∈ R 6×5 is the complementary matrix to the virtually cut revolute joint axis A p+ = (0, 0, 1, 0 1×3 ) T i.e., it is such that A T p+ Āp+ = 0 1×5 , and ĀT p+ Āp+ = 1 5×5 . This context holds for any type of cut joint where it suffices to replace the 5 constraints above by any number of constraints depending on the nature of the joint. In particular, virtually cutting a fixed joint introduces six constraints instead of five (i.e. Āp+ = 1 6×6 ). Time-differentiating [START_REF] Pogorelov | Differential-algebraic equations in multibody system modeling[END_REF] provides the kinematic form of the closure loop constraints:

Φs|p = J s|p η s+/p+ = ĀT p+ 0 6×1 , (37)
where η s+/p+ = ( s+ g p+ p+ ġs+ ) ∨ = η s+ -Ad s + gp + η p+ is the relative twist of the follower frame F s+ with respect to the leader one F p+ , while J s|p , is a kinematic Jacobian matrix defined by:

J s|p = ĀT p+ T (Θ s|p ) -1 0 3×3 0 3×3 p+ R s+ , (38)
which depends on Θ s|p = log( p+ R s+ ) ∈ R 3 . Finally, note that if the loop is connected to the ground, we simply have s = 0.

Remark 11: Gathering all the equations from ( 27) to [START_REF] Armanini | Discrete cosserat approach for closed-chain soft robots: Application to the fin-ray finger[END_REF] defines the Newton-Euler formulation of the SMMS dynamics. It is here expressed on the Lie group SE(3) as this was the case for rigid robots in [START_REF] Park | A lie group formulation of robot dynamics[END_REF], [START_REF] Kim | Lie group formulation of articulated rigid body dynamics[END_REF]. This formulation is entirely equivalent to the Lagrangian one defined by the set of DAEs (2). As announced in Section V, we are now going to exploit this equivalence in order to calculate the residual of the Lagrangian formulation [START_REF] Boyer | Finite element of slender beams in finite transformations: A geometrically exact approach[END_REF] and its Jacobian matrix [START_REF] Bergou | Discrete elastic rods[END_REF]. This is achieved with two recursive algorithms based on the NE formulation that are detailed in the next section ■

VII. RECURSIVE IDM AND T IDM OF A SMMS A. Newton-Euler IDM of a SMMS

Like for a rigid manipulator [START_REF] Luh | On-line computational scheme for mechanical manipulator[END_REF], the IDM proceeds in two passes. The first, descending (from the base to the ends of the branches), calculates the inertial poses, velocities and accelerations along the branches from the knowledge of (g 0 , η 0 , η0 , q, q, q). The second, ascending, calculates the interbody wrenches transmitted across the rigid and soft joints from the knowledge of λ and the inertial kinematics calculated by the first pass. These interbody wrenches are then projected on the d.o.f of joints to calculate Q a of (20), while F a is the last "interbody" wrench computed by the second pass. As introduced before, thanks to the declaration of rods as distributed joints, at a first (high) level, this algorithm takes the usual form of that of rigid systems, but here applied to the segmentation of section VI.A. The only difference lies in a second (low) level, where the usual discrete forward and backward transfers on inter-body kinematics (poses, velocities, accelerations) and inter-body wrenches, respectively, are replaced by continuous transfers obtained by integrating forward [START_REF] Kim | Lie group formulation of articulated rigid body dynamics[END_REF] (and its first and second temporal derivatives), and backward [START_REF] Kane | The use of kane's dynamical equations in robotics[END_REF], when the joint is a soft distributed one.

1) Forward kinematics: Time differentiating twice [START_REF] Park | A lie group formulation of robot dynamics[END_REF] provides the kinematic model that relates the pose, velocities and accelerations of all the rigid bodies B j (including the tip cross sections of soft links) in the form of a recursion on body indexes, initialized by (g 0 , η 0 , η0 ):

For j = 1, 2, ..., N, do : k = a(j), g j = g k k g j , η j = Ad j g k η k + η j/k , ηj = Ad j g k ηk + ad ηj η j/k + ηj/k , (39)
where from now on, η j/k = k g -1 j k ġj ∨ ∈ R 6 denotes the twist of relative velocities between B j and its antecedent B k expressed in the mobile frame of B j . Now, when computing (39), we have two cases:

• If J j is a lumped rigid one, one uses the usual relations:

k g j = k g j (q rj ) , η j/k = A j qrj , ηj/k = A j qrj , (40)
where A j is the unit twist supporting the localized rotation of a revolute joint as introduced in remark 8, while if the joint is fixed, q rj is replaced by a constant angle, that depends on the design.

• If J j is a soft joint, ( k g j , η j/k , ηj/k ) is the terminal value of the forward integration from X = 0, where ( k g j , η j/k , ηj/k )(0) = (1 4×4 , 0 6×1 , 0 6×1 ), to l j , of the following system of space ODEs, deduced from (34) and its time derivatives:

d dX   k g j η j/k ηj/k   =    k g j ξj -ad ξj η j/k + ξj -ad ξj ηj/k -ad ξj η j/k + ξj    , (41)
where using the strain based parametrization of (1), (ξ j , ξj , ξj ) = (ξ o j + B j Φ j q ϵ,j , B j Φ j qϵ,j , B j Φ j qϵ,j ). To summarize, if J j is rigid, the joint kinematics between B k and B j are directly given by the discrete (lumped) relations [START_REF] Briot | A geometrically exact assumed strain modes approach for the geometrico-and kinemato-static modelings of continuum parallel robots[END_REF], while if it is soft, they are defined by the continuum composition of poses, velocities and accelerations along the distributed joints, with [START_REF] Tummers | Cosserat rod modeling of continuum robots from newtonian and lagrangian perspectives[END_REF]. These discrete and continuous kinematic transfers along the joints are illustrated in the top two diagrams of the figure 4.

2) Backward dynamics: The model that relates the wrenches transmitted between the B j s (including the tip cross-sections of the rods) consists of the backward recursion on body indexes: For j = N, N -1, ...1, do :

(42)

F j = M j ηj -ad T ηj M j η j + F ext,j + l∈s(j) j F l ,
which are initialized by the wrenches applied to the tip of the branches as detailed in the Remark 12 below. Now when computing (42), we have two cases:

• If J l is rigid, we use the usual relations:

j F l = Ad T l gj F l , (43)
• If J l is soft, j F l = -Λ l (0) is the result of the backward integration from X = l l to 0 of:

Λ ′ l = ad T ξ l Λ l + M l ηl -ad T η l M l η l -Fext,l , (44)
starting from initial conditions Λ l (l l ) = -F l . In words, if the J l joint is rigid, F l is directly transmitted to B j through the rigid transport of (43), while if it is soft, the contact wrench -F l is transmitted to B j through the continuum transport of the wrench along the rod that connects B j to B l according to its dynamic balance [START_REF] Hughes | Finite Element Method-Linear Static and Dynamic Finite Element Analysis[END_REF]. This context is illustrated in figure 4 (bottom).

Remark 12:

The initial conditions of the backward dynamics enter through the sum term in (42), when j is the index, say p, of a terminal body of a branch. In particular, if such a terminal body does not support any cut joint, it does not contribute to the initial conditions in [START_REF] Trefethen | Spectral methods in MATLAB[END_REF]. Otherwise, let us consider two terminal bodies B p and B s connected by a cut revolute joint as in Section II. Then, using the kinematic model of constraints [START_REF] Armanini | Discrete cosserat approach for closed-chain soft robots: Application to the fin-ray finger[END_REF] and the duality between twists and wrenches, we have:

F s+ = J T s|p λ s|p , F p+ = -Ad T s + gp + F s+ , (45)
where λ s|p ∈ R 5 is a vector of reaction generalized forces, dual of Φs|p in [START_REF] Armanini | Discrete cosserat approach for closed-chain soft robots: Application to the fin-ray finger[END_REF], which defines five components of the Lagrangian vector λ in (6), while λ s|p ∈ R 6 if the cut joint is fixed ■ Remark 13: Note that in [START_REF] Hughes | Finite Element Method-Linear Static and Dynamic Finite Element Analysis[END_REF], one needs to use the inertial fields (g l , η l , ηl )(.), that can be computed with the formulas: where the inter-body kinematic fields ( k g j , η j/k , ηj/k )(.) along the rods are known after the first pass through the integration of (41) ■

g l (X) = g j j g l (X), (46) η l (X) = Ad j g l (X) η j + η l/j (X), ηl (X) = Ad j g l (X) ( ηj + ad η l/j (X) η j ) + ηl/j (X),
3) Summary of the IDM : The IDM algorithm progresses as follows. First, a forward pass initialized by (g 0 , η 0 , η0 ) and fed by (q, q, q), computes from the basis B 0 to all the tip branches, the inertial poses, velocities and accelerations through the composition of the kinematic model of rigid bodies and soft joints [START_REF] Cardona | Numerical integration of second order differential-algebraic systems in flexible mechanism dynamics[END_REF][START_REF] Briot | A geometrically exact assumed strain modes approach for the geometrico-and kinemato-static modelings of continuum parallel robots[END_REF][START_REF] Tummers | Cosserat rod modeling of continuum robots from newtonian and lagrangian perspectives[END_REF] and using [START_REF] Orekhov | Solving cosserat rod models via collocation and the magnus expansion[END_REF]. These kinematic variables are used to feed a second backward pass that computes the inter-body wrenches from the tip branches to the basis B 0 through the composition of the dynamic model of rigid bodies and soft joints [START_REF] Trefethen | Spectral methods in MATLAB[END_REF][START_REF] Boyer | Reduced dynamics of the nonholonomic whipple bicycle[END_REF][START_REF] Hughes | Finite Element Method-Linear Static and Dynamic Finite Element Analysis[END_REF]. According to [START_REF] Khalil | Modeling Identification and Control of Robots[END_REF], this second pass is initialized by the contact forces λ transmitted by the cut joints, that are part of the inputs to the algorithm. While running, the second pass provides all the contact wrenches F j and Λ j (.) transmitted across the rigid bodies and soft joints to which we apply the projective relations:

Q ar,j = A T j F j , Q aϵ,j = - lj 0 Φ T j B T j Λ j dX, (47)
for all lumped revolute and soft joints. Then all the values of ( 47) are used to fill Q a = (Q T ar , Q T aϵ ) T . Moreover, if the system has a free floating basis, the algorithm also provides F 0 = F a which completes the outputs (F T a , Q T a ) T of the input-output map [START_REF] Grinspun | Discret Differential Geometry: An Applied Introduction[END_REF].

B. Newton-Euler T IDM of a SMMS

The T IDM is derived by calculating the linear perturbations of the outputs of the IDM produced by linear perturbations of its inputs. Practically, it is simply deduced from the IDM by applying the variation ∆ to all the above expressions and exploiting the properties of the Ad and ad maps. Like the IDM , the T IDM consists of two recursions on the body indexes, one (tangent) forward, computes the variations of kinematics, the second, (tangent) backward, computes those of the internal wrenches transmitted along the structure.

1) Tangent forward kinematics: It is deduced from the variation ∆ of (39):

For j = 1, 2, ..., N, do :

(48) k = a(j), ∆ζ j = Ad j g k ∆ζ k + ∆ζ j/k , ∆η j = Ad j g k ∆η + ad (Ad j g k η k ) ∆ζ j/k + ∆η j/k , ∆ ηj = Ad j g k ∆ ηk + ad (Ad j g k ηk ) ∆ζ j/k -ad η j/k ∆η j +ad ηj ∆η j/k + ∆ ηj/k , which is initialized by the basis kinematic variations (∆ζ 0 , ∆η 0 , ∆ η0 ), and where:

• If J j is a lumped rigid joint, one simply has:

(∆ζ j/k , ∆η j/k , ∆ ηj/k ) = A j (∆q rj , ∆ qrj , ∆q rj ). (49)
• If J j is soft, (∆ζ j/k , ∆η j/k , ∆ ηj/k ) is the terminal value of the forward integration:

d dX   ∆ζ j/k ∆η j/k ∆ ηj/k   =   -ad ξj ∆ζ j/k -ad ξj ∆η j/k + ad η j/k ∆ξ j -ad ξj ∆ ηj/k -ad ξj ∆η j/k   +   ∆ξ j ∆ ξj ad η j/k ∆ ξj + ad ηj/k ∆ξ j + ∆ ξj   , (50)
with initial conditions (∆ζ j/k , ∆η j/k , ∆ ηj/k )(0) = 0 6×3 , and (∆ξ j , ∆ ξj , ∆ ξj ) = B j Φ j (∆q ϵ,j , ∆ qϵ,j , ∆q ϵ,j ).

2) Tangent backward dynamics: Applying the variation ∆ to [START_REF] Trefethen | Spectral methods in MATLAB[END_REF] gives:

For j = N, N -1, ...1, do : (51) ∆F j = M j ∆ ηj -ad T ηj M j ∆η j -ad T ∆ηj M j η j +∆F ext,j + l∈s(j) ∆ j F l ,
which is initialized by the variations of the wrenches applied onto the terminal bodies of the branches i.e., through some ∆ j F l when B j is a terminal body. These variations take different expressions depending whether the terminal body supports a virtually cut joint or not (see section below).

Once again, we have two cases:

• If J l is a rigid joint, one has:

∆ j F l = Ad T l gj ∆F l + ∆Ad T l gj F l , (52)
which can be detailed as:

∆ j F l = Ad T l gj ∆F l -Ad T l gj ad T ∆ζ l/j F l , (53)
• If J l is soft, ∆ j F l = -∆Λ l (0) is the result of the backward integration from X = l l to 0 of:

∆Λ ′ l = M l ∆ ηl -ad T η l M l ∆η l -ad T ∆η l M l η l +ad T ξ l ∆Λ l + ad T ∆ξ l Λ l -∆ Fext,l , (54)
starting from initial conditions ∆Λ l (l l ) = -∆F l . In these ODEs, ∆ξ l = B l Φ l ∆q ϵ,l , while ∆η l , ∆ ηl , and even ∆ζ l (which can be required by ∆ Fext,l ), are all deduced by varying [START_REF] Orekhov | Solving cosserat rod models via collocation and the magnus expansion[END_REF], that provides formulae similar to ( 50) with (l, j) instead of (j, k).

Once all the ∆F j s and ∆Λ j (.)s known, one can calculate the outputs of the T IDM i.e.,:

∆Q ar,j = A T j ∆F j , ∆Q aϵ,j = - lj 0 Φ T j B T j ∆Λ j dX, ( 55
)
depending whether it is lumped (rigid) or distributed (soft). Finally, if B 0 is a free floating basis, ∆F a = ∆F 0 , and the algorithm feeds back all the outputs (∆F T a , ∆Q T a,r , ∆Q T a,ϵ ) T of the input-output map [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF].

3) Initialization of tangent backward dynamics: Using the notations of Remark 11, if a terminal body supports a cut, this body can be a leader B p or a follower B s i.e., one can have (j, l) = (p, p + ) or (s, s + ) in (51). Then, since s g s+ and p g p+ are constant, the initial conditions of (51) are of the form ∆ p F p+ = Ad T p+ gp ∆F p+ and ∆ s F s+ = Ad T s+ gs ∆F s+ . Thus, we need to calculate ∆F p+ and ∆F s+ . Regarding ∆F s+ , applying ∆ to the left expression of ( 45), gives first:

∆F s+ = J T s|p ∆λ s|p + (∆J s|p ) T λ s|p , (56)
with ∆λ s|p a sub-vector of ∆λ, and where we used the variation of the Jacobian matrix [START_REF] Armanini | Discrete cosserat approach for closed-chain soft robots: Application to the fin-ray finger[END_REF]:

∆J s|p = ĀT p+ ∆T (Θ s|p ) -1 0 3×3 0 3×3 p+ R s+ T (Θ s|p ) , (57)
with ∆T the second differential of the exponential map of SO(3) [START_REF] Sonneville | Geometrically exact beam finite element formulated on the special euclidean group se(3)[END_REF]. Regarding ∆F p+ , using the properties of Ad and ad, the variation ∆ applied to the right expression of [START_REF] Khalil | Modeling Identification and Control of Robots[END_REF] gives:

∆F p+ = -Ad T s+ gp+ (∆F s+ -ad T ∆ζ s+/p+ F s+ ), (58)
which involves the relative variation:

∆ζ s+/p+ = ∆ζ s+ -Ad s+ gp+ ∆ζ p+ , (59)
where ∆ζ s+ = Ads+ gs ∆ζ s , ∆ζ p+ = Adp+ gp ∆ζ p are deduced from ∆ζ s and ∆ζ p computed by the forward tangent kinematics [START_REF] Boyer | Fast dynamics of an eel-like robot-comparisons with navier-stokes simulations[END_REF]. Therefore, if (j, l) = (s, s + ), (51) is initialized with (56,57), and if (j, l) = (p, p + ), with (58,59), both being function of (λ, ∆λ). At last, if a terminal body of a branch does not support any virtual cut, we simply take ∆ j F l = 0 6×1 as initial condition.

VIII. CALCULATION OF R(n) AND ∂ R(n) /∂ χ Referring to Remark 5, the calculation of R(n) ( χ, t n+1 ) and (∂ R(n) /∂ χ)( χ, t n+1 ) requires that of R(n) λ = Φ(g 0 , q
) along with Υ(q, t n+1 ) and ∆Υ(q, ∆q, t n+1 ) in ( 22) and [START_REF] Gazzola | Pyelastica[END_REF]. These additional computations are by-products of IDM and T IDM which we will now detail.

A. Calculation of R(n) λ = Φ(g 0 , q) For each loop virtually cut between B p and B s , one can compute a vector of the type Φ p|s by feeding the general formula (36) with p+ g s+ = g -1 p+ g numerically calculated by the forward kinematics [START_REF] Cardona | Numerical integration of second order differential-algebraic systems in flexible mechanism dynamics[END_REF] along each branch of the SMMS. Note that each of the Φ p|s -vectors defines a set of nonlinear algebraic equations depending in general on (g 0 , q). Gathering all of these Φ p|s -vectors, provides the expected Φ of the original system of DAEs (2).

B. Calculation of Υ(t n+1 ) and ∆Υ(t n+1 )

To finish the computation of the residual vector [START_REF] Goldenthal | Efficient simulation of inextensible cloth[END_REF], it remains to compute

Υ(t n+1 ) = Q ad (t n+1 )-Q e . As regards Q e , we have Q e = (0 T 1×nr , Q T ϵ ) T , with Q ϵ obtained by gathering the vectors of R nϵ,j : Q ϵ,j = lj 0 Φ T j (B T j H j B j )Φ j dX q ϵ,j = K ϵϵ,j q ϵ,j , (60)
for all soft J j s. Remarking that K ϵϵ,j ∈ R nϵ,j ×nϵ,j is constant, we have Q ϵ = K ϵϵ q ϵ where K ϵϵ = diag(K ϵϵ,j ) is the constant stiffness matrix of the entire structure. As regards Q ad , we have

Q ad (t n+1 ) = (τ T d , Q T adϵ ) T (t n+1 ), where Q adϵ (t n+1
) is obtained by gathering the vectors Q adϵ,j ∈ R nϵ,j , defined by [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]:

Q adϵ,j (t n+1 ) = - lj 0 Φ T j B T j Λ d,j (t n+1 )dX, (61)
for all soft J j s. Similarly, one can compute ∆Υ(t n+1 ) = ∆Q ad (t n+1 ) -∆Q e by differentiating (60) and (61). The former variation is merely ∆Q ϵ = K ϵϵ ∆q ϵ , while the latter is ∆Q ad = (0 1×nr , ∆Q T adϵ (t n+1 )) T , with:

∆Q adϵ,j (t n+1 ) = - lj 0 Φ T j B T j ∆Λ d,j (t n+1 )dX. (62)
C. Adaptation of the IDM and T IDM to compute the residual and its Jacobian Finally, to calculate the two top entries of R(n) ( χ, t n+1 ), one can numerically integrate by quadrature (60) and (61), and remove Υ(t n+1 ) from IDM ( χ), or more directly, replace in the IDM , the left side equation of ( 47) by:

R j (χ, t) = A T j F j -τ d,j (t), (63)
if J j is rigid, and the right side one by:

R j (χ, t) = (64) lj 0 Φ T j B T j (Λ d,j (t) + H j B j Φ j q ϵ,j -Λ j )dX, if it is soft.
Similarly, one can calculate the two top rows of (∂ R(n) /∂ χ)( χ), by replacing in the T IDM , the left-side expression of (55) by:

∆R j (χ, ∆χ, t) = A T j ∆F j , (65)
if J j is rigid, and the right-hand side one by:

∆R j (χ, ∆χ, t) = (66) lj 0 Φ T j B T j (∆Λ d,j (t) + H j B j Φ j ∆q ϵ,j -∆Λ j )dX,
if J j is soft. Finally, feeding the IDM and T IDM , with inputs compatible with the Newmark scheme (10,13-15) and using as outputs (63,64) and (66,65) instead of ( 47) and (55) for every J j , allows to compute directly R(n) and ∆ R(n) . This defines two further algorithms, noted IDM ⋆ and T IDM ⋆ in figure 2, where the IDM ⋆ also includes the calculation of R(n) λ = Φ(g 0 , q) of Section VIII.A.

IX. SPACE INTEGRATION

The algorithm needs to forward (respect. backward) integrate a sequence of space initial value problems (IVPs) defined by (41) (respect. ( 44)), along the tree, where the initial conditions of one joint, are fixed by the final conditions imposed by its antecedent body (respect. by its successors). This can be achieved with standard finite-difference integrators as ODE45 of Matlab [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]. Nevertheless, let us remark that (ξ j , ξj , ξj )(.) being fixed by the inputs (q r,j , qr,j , qr,j ) of the algorithm, they can be considered as constants in [START_REF] Tummers | Cosserat rod modeling of continuum robots from newtonian and lagrangian perspectives[END_REF] which turn to be linear ODEs with respect to (g j , η j , ηj )(.). Similarly, once the first pass is achieved, (g j , η j , ηj )(.) can be considered as fixed in the backward ODEs [START_REF] Hughes | Finite Element Method-Linear Static and Dynamic Finite Element Analysis[END_REF], which become linear with respect to the Λ j (.)s. Parameterizing the rotational component R j (.) of g j (.) = (R j , r j )(.), with a unit quaternion field5 H j (.) : X ∈ [0, l j ] → H j (X) ∈ R 4 , this property of the IDM allows us to apply efficient spectral methods to the integration of ( 41) and [START_REF] Hughes | Finite Element Method-Linear Static and Dynamic Finite Element Analysis[END_REF]. These methods can be optimized by exploiting a further property of the IDM related to its differential structure. Indeed, if one details all the fields of [START_REF] Tummers | Cosserat rod modeling of continuum robots from newtonian and lagrangian perspectives[END_REF][START_REF] Hughes | Finite Element Method-Linear Static and Dynamic Finite Element Analysis[END_REF] into their angular and linear components, with notations: (R j , r j ) = (H j , r j ), ξ j = (K T j , Γ T j ) T , η j = (Ω T j , V T j ) T , ηj = ( ΩT j , V T j ) T and Λ j = (C T j , N T j ) T , it is straightforward to show that each of these two sets of ODEs enjoys a block triangular differential structure which can be solved in cascade, by integrating block after block in an ordered manner, a sequence of linear systems of the canonical state-form:

x ′ = A(X)x + b(X) , X ∈ [0, l j ], (67)
where for each soft J j , the state vector x ∈ R 3 or R 4 , successively takes the meaning of H j , r j , Ω j , V j , Ωj , Vj for the forward ODEs [START_REF] Tummers | Cosserat rod modeling of continuum robots from newtonian and lagrangian perspectives[END_REF], and of N j , C j for the backward one [START_REF] Hughes | Finite Element Method-Linear Static and Dynamic Finite Element Analysis[END_REF]. Respecting this order, at each step of the cascade resolution, one uses the result of the previous step to feed A and b in (67).Therefore, owing to this differential property of the IDM , spectral integration based on collocation over a Chebyshev grid, allows to change ODEs [START_REF] Tummers | Cosserat rod modeling of continuum robots from newtonian and lagrangian perspectives[END_REF][START_REF] Hughes | Finite Element Method-Linear Static and Dynamic Finite Element Analysis[END_REF] into algebraic linear systems of triangular structure, that can be solved efficiently with respect to the successive finite dimensional vectors of the values of the fields (H j , r j , Ω j , V j , Ωj , Vj , C j , N j )(.) at the Chebyshev nodes of the grids. Finally, by adding to the linear backward ODE systems, the expressions (64) reformulated as ODEs of the form (67) with A = 0, one completes the spatial integrations required by the computation of the residual vector R (n) , while a strictly similar process can be applied with the T IDM in order to compute ∆R (n) , and finally the expected Jacobian of the residual vector. Before concluding this section, let us note that while all the formulas required by IDM and T IDM were obtained by symbolic on the Lie group SE(3), in their numerical implementation, these expressions are integrated on SO(3)×R 3 with SO(3) parameterized by usual quaternions, as was done in [START_REF] Boyer | Macro-continuous computed torque algorithm for a three-dimensional eel-like robot[END_REF], [START_REF] Rucker | Statics and dynamics of continuum robots with general tendon routing and external loading[END_REF].

X. ILLUSTRATIVE EXAMPLES

We now illustrate the above general algorithm on a set of numerical examples and refer the reader to Appendix 2 for a symbolic illustration. We start by two elementary benches well known from the literature on geometrically exact FEM where they are used to validate new numerical methods in the field [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF]. These first examples consists of one single rod in different conditions of connection offered by the algorithm. They are here used to compare several methods recently proposed in soft robotics. Once this is achieved, the section continues with three examples more related to robotics. Note that the algorithm presented in the article has been programmed in Matlab, but is not yet optimised, as mentioned in the conclusion.

A. Single rod benchmarks

We consider single rods in different conditions of connection with the ground. In the first case, a soft and a stiff rod is cantilevered i.e., fixed at one end and free at the other [START_REF] Boyer | Finite element of slender beams in finite transformations: A geometrically exact approach[END_REF]. In the second, another two soft and stiff rods are kinematically free at both ends according to the so-called flying rod bench [START_REF] Simo | A finite strain beam formulation. the three-dimensional dynamic problem. part i[END_REF]. Note that these tests are complementary since they illustrate the case of a fixed and a free base system (locomotor and manipulator), while closed loop systems will be illustrated later with a two rod system. These two first benches were used to validate the strain-based parametrization of (1) against GE-FEM in [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF] and to study the relationships between the shooting based resolution and optimal control in [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF]. We use them here to provide the reader with a very first idea of the robustness and time-consumption of the algorithm in comparison to the explicit time-integration methods of [START_REF] Gazzola | Pyelastica[END_REF], [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF] and [START_REF] Mathew | Sorosim: A matlab toolbox for hybrid rigid-soft robots based on the geometric variable-strain approach[END_REF], as well as to the shooting-based approach of [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF] and [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF] based on implicit integration. Recall that although all these approaches are based on the same model (Cosserat rods), [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF] and [START_REF] Mathew | Sorosim: A matlab toolbox for hybrid rigid-soft robots based on the geometric variable-strain approach[END_REF] use the modal strain reduction (1), while [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF], [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF] and [START_REF] Gazzola | Pyelastica[END_REF] require no configuration space reduction beyond nodal discretization (based on finite differences in the first two cases, and on DER in the third).

1) Cantilever rod: The soft rod is that of [START_REF] Boyer | Finite element of slender beams in finite transformations: A geometrically exact approach[END_REF] with l = 10 m, ρ = 7800 kg.m -3 , E = 10 5 MPa, and a circular cross section with 0.01 m diameter. The stiff rod is that of [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF], with l = 0.4 m, ρ = 8000 kg.m -3 , E = 2 × 10 3 MPa and 0.002 m diameter. Each of them is first subject to a horizontal static force (f, 0, 0) T in the inertial frame, and released in the vertical gravity at t = 0 s. In figure 5, we reported a set of snapshots simulated with f = 50 N over 10 s, and f = 10 N over 1 s, for the soft and stiff rods respectively. Simulations based on the strain parametrization are performed with the Kirchhoff model and 3 bending modes. Note that the results of all methods that converged are indistinguishable.

2) Flying rod: Each of the two rods floats in vacuum without gravity. They are initially inclined and loaded in the inertial plane (o, e 1 , e 2 ) as indicated in figure 6 (a) and (b). The soft beam is that of Simo [START_REF] Simo | A finite strain beam formulation. the three-dimensional dynamic problem. part i[END_REF] i.e., l = 10 m, H = diag(5, 5, 5, 10 2 , 10 2 , 10 2 )10 2 , and M = diag(10, 10, 10, 1, 1, 1) (all in IF units). The stiff rod parameters are those of [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF], i.e l = 1 m, ρ = 10 3 kg.m -3 , E = 1 MPa, for a diameter of 0.1 m. Strain parametrization is performed with the Kirchhoff model with 3 modes for the two curvatures and for torsion. Figures 6 (b) and 6 (c) show a few snapshots of the two rods simulated over 10 s. Note that the results of all the methods that converged are indistinguishable.

With the exception of PyElastica [START_REF] Gazzola | Pyelastica[END_REF], which is programmed in Python, all other methods were implemented in Matlab. All simulations were run on a single computer 6 . In addition, the tuning parameters of each method (time and space-steps, Chebychev grid, quadrature order, etc.) were chosen in such a way as to obtain an accuracy similar to that of the GE-FEM, without penalising any of them in terms of efficiency. In details ∆t = 0.01 s for the implicit integrators of [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF] and that of the paper, tolerance of ODE45 in the timeintegration of [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF] and [START_REF] Mathew | Sorosim: A matlab toolbox for hybrid rigid-soft robots based on the geometric variable-strain approach[END_REF] is 10 -6 , while the spatial grids of [START_REF] Mathew | Sorosim: A matlab toolbox for hybrid rigid-soft robots based on the geometric variable-strain approach[END_REF], PyElastica and the present algorithm have 20 , 50 and 20 discretization points respectively. Finally, note that in the case of soft rods, the shooting approach of [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF] as well as its extension to floating base systems of [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF], fail for intrinsic reasons related to the singular character of the underlying optimal control problem from which the spatial BVP is derived [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF]. As expected, the simulation times for methods based on explicit integration increase with stiffness since in this case, the time step must be reduced to preserve stability. In TABLE I: "Simulation-time over real-time" ratio, for cantilever and flying rod benches obtained with [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF], [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF], [START_REF] Gazzola | Pyelastica[END_REF] (PyElastica), [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF], [START_REF] Mathew | Sorosim: A matlab toolbox for hybrid rigid-soft robots based on the geometric variable-strain approach[END_REF] and the algorithm of the article.

contrast, the article's implicit integrator can be run with a large time step in both cases, which explains why simulation time is independent of rod stiffness. By way of illustration, the critical time step beyond which PyElastica is no longer stable is 3.4 × 10 -6 s for the stiff cantilevered rod, and must be divided by √ α when E is multiplied by α ∈ [0.01, 100], whereas our implicit integrator remains stable over this range of stiffness with a time step of 0.1s. Finally, it should be remembered that these initial estimates of calculation times are far from definitive, and will be systematically studied in the future. For the moment, they tend to show that the extra computations of the Jacobian of the residual vector required by implicit integration are well compensated by the width of the time steps it allows, as it is often the case in nonlinear structural dynamics [START_REF] Subbaraj | A survey of direct time-integration methods in computational structural dynamics-ii. implicit methods[END_REF].

B. Example 1: A mass attached to a rod moved by two flying drones at its two ends

We consider the case of a locomotor, namely a cuboid payload carried by two identical hexarotor flying drones through two identical rods. The ends of the two rods are connected by fixed joints to the drones (UAVs), and to the payload. They are modelled as 3D rigid bodies subject to a lift force of controlled direction and strength. According to our general setting, this SMMS is segmented as indicated in figure 7 i.e., it contains seven rigid bodies B 0,1,...6 , namely three 3D ones and the four tip cross-sections of the rods. These bodies are connected by six joints J 1,2...,6 , namely four fixed lumped ones and two distributed soft ones. One of the two drones is considered as the reference body B 0 , and all the other rigid bodies are numbered increasingly along this open chain. The dynamics of the UAV rotors are assumed to be fast enough to impose instant velocity control of external wrenches:

j = 0, 6 : F ext,j = k P e ηj + k I e ηj dt, (68)
with k P and k I two diagonal 6 × 6 matrices of proportional and integral gains respectively, and e ηj ∈ R 6 the velocity error defined by:

e ηj = Ad (g -1 j g d j ) η d j -η j , (69)
with t → η d j (t) = ((g d j ) -1 ġd j ) ∨ a desired velocity timeevolution. In figures 8, 9, we reported the results of a simulation of this system obtained with η d j (t) = (0 1×3 , V dT j (t)) and V d 0 (t) = (0, 0, sin(πt)) T , V d 6 (t) = (0, 0, sin π 2 t ) T . The simulation is performed over 10s with a time-step ∆t = 0.01 s and requires on average, 3 Newton's steps per time-step. The two rods J 2 and J 5 , are Kirchhoff, straight at rest, and parameterized with 3 Chebyshev polynomials for each of the three components (two curvatures and torsion) of ϵ j = K j , j = 2, 5. Their geometrical and physical parameters are l = 1 m, ρ = 950 kg.m -3 , diameter = 0.07 m, E = 0.25 MPa. For the sake of simplicity, gravity is neglected, while we did not seek to optimize the controllers but rather to reveal the richness of the dynamic interactions between the drones and the rods. To this end, taking (k P , k I ) = (diag [START_REF] Miklos | Discrete viscous threads[END_REF][START_REF] Miklos | Discrete viscous threads[END_REF], diag(2.5, 2.5)) in (68), defines a soft controller that keeps the velocity errors limited, while not reacting too much to the elastic restoring torques induced by the rods on the two drones. In Figure 8, snapshots and trajectories of the two drones and the payload B 3 (see also the multimedia attachment for a video) show how the rods move B 3 on an almost cyclic circular path. The time evolution of the displacement of these 3 bodies is displayed in figure 9.

C. Example 2: continuum parallel robot (CPR)

This second example is a planar continuum parallel manipulator with two identical legs connected at both ends by friction-less revolute joints, the proximal ones are actuated, while the distal one connecting the two legs together, is passive. As imposed by the modelling approach, the closed loop of the CPR is first opened by virtually cutting the passive joint, and the resulting tree-like system is segmented as indicated in figure 10 i.e., it consists of two identical branches, each with two bodies (rod's end cross-sections) B 1,2 and B 3,4 , and four joints J 1,2,3,4 , J 1,3 being revolute, J 2,4 being soft. The ground defines the fixed reference body B 0 . The design parameters of this CPR are as follows. The legs are two identical 1-meter Kirchhoff rods with section such that I = 1.5708×10 -8 m 4 and A = 3.1416×10 -6 m 2 . They are made of an ordinary steel, of Young modulus E = 2.1 × 10 5 MPa and volume mass ρ = 7800 kg.m -3 . In this example, the closure loop introduces 2 constraints with two Lagrange multipliers. As for all closed loop systems, any dynamics simulation needs to be preceded with a static mounting phase which consists in closing the loop in statics, starting from an open configuration of the two branches. In this case, this reference configuration is such that the two Revolute joint angles q r,1 and q r,2 vs time. knowledge about stiff systems of DAEs, for which it is well known that the geometric constraints of the residual balance (8) introduce high-frequency parasitic (numerical) modes, that can compromise the stability of simulations [START_REF] Cardona | Time-integration of the equations of motion in mechanism analysis[END_REF].

D. Example 3: A bio-inspired swimmer actuated with tendons

In this last example, we consider a bio-inspired locomotor, namely an undulatory swimmer of 1m length and nonuniform elliptical cross-sections along its rostro-caudal axis. Its geometry is that of [START_REF] Boyer | Fast dynamics of an eel-like robot-comparisons with navier-stokes simulations[END_REF] with maximum lateral width of 0.1 m. However, in contrast to [START_REF] Boyer | Fast dynamics of an eel-like robot-comparisons with navier-stokes simulations[END_REF], we here consider it as being constituted of three segments serially fixed to each other from the head to the tail (see figure 14). The first segment (the head) is a rigid body, the second is a rod internally actuated by a pair of antagonistic tendons as those currently used for TACRs [START_REF] Rucker | Statics and dynamics of continuum robots with general tendon routing and external loading[END_REF], the third is a passive elastic rod. The swimmer is neutrally buoyant i.e., ρ = 10 3 kg.m -3 , and the first and second rod are Kirchhoff with a bending stiffness EI = 4.9 Pa.m 4 and EI = 4.9 × 10 -2 Pa.m 4 respectively. The tendons are parallel to the robot axis and located at a distance D = 0.05 m from it. According to our general setting, this system is segmented in 5 bodies B 0,1,2,3,4 , with B 0 , a 3D rigid body modelling the head, and all the others standing for the rod's end cross-sections. All these bodies are connected by four joints J 1,2,3,4 , with J 1,3 two lumped fixed joints, and J 2,4 two distributed soft ones. The hydrodynamic forces are modelled with Lighthill's Large Amplitude Elongated Body Theory (LAEBT), here coupled to the Cosserat model as proposed in [START_REF] Boyer | Fast dynamics of an eel-like robot-comparisons with navier-stokes simulations[END_REF]. The planar swimming of this bio-inspired system can be achieved by applying to the two tendons, the tensions:

T ± (t) = T 0 + T 1 sin ωt ± π 2 , (70)
where indexes + anddenote the left and right tendons respectively, T 0 > 0 is a constant antagonistic component ensuring that T ± (t) > 0 for all t, and T 1 > 0 is the amplitude of an harmonic component of pulsation ω, responsible of the body undulations. In figures 15 and 16, we reported some simulation results carried out over 10 s with ∆t = 0.01 s, and 5 Chebychev polynomials per rod curvature. 1.65 s. Figure 16 shows the time evolution (over the first 3 s) of the control torque τ (t) = D(T + -T -)(t), and of the elastic torque C = (EI 3 K Z,3 )(0) at the root of the third (elastic) body B 3 , where the subscript Z, indicates a component taken along the normal to the plane of the swim.

XI. CONCLUSION AND PERSPECTIVES

In this paper, we have proposed a new algorithm for solving the forward dynamics of soft and continuum robots consisting of rigid bodies connected in arbitrary topologies by localised joints and/or soft links, possibly actuated or not. The soft joints are modelled as Cosserat rods parameterized with strain modes. Unlike the literature approaches for simulating these models, our approach is designed to work with an implicit time integration scheme. We choose the Newmark scheme which is known to be among the best suited for stiff problems of non-linear structural dynamics. Using such a scheme allows changing the DAEs of the robot into a system of nonlinear algebraic equations. At each timestep, we solve this system with a Newton-Raphson scheme, requiring the residual and its Jacobian. To this aim we poposed a new recursive Newton-Euler algorithm. Although the results are encouraging, in its current state the approach has been implemented and tested as a proof of concept. Therefore, we are currently optimising it with the short-term objective of building a toolbox 7 , dedicated to simulation (and control) of soft and continuum robots. We strive for improvements in terms of robustness, computational efficiency and programming simplicity.

A. Perspectives for numerical robustness

As expected, the implicit integrator makes the algorithm robust and stable even for large time steps. However, the geometrical constraints introduce high frequency parasitic modes. As a result, in (2) some physical damping has been introduced into the constitutive laws [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF]. This artificial damping could in general affect the low frequency content of the response of an SMMS. Another solution consists in replacing the Newmark integrator by the HHT method or the α-method which are designed to solve this problem [START_REF] Cardona | Time-integration of the equations of motion in mechanism analysis[END_REF], [START_REF] Arnold | Convergence of the generalized-α scheme for constrained mechanical systems[END_REF].

B. Perspectives for numerical efficiency

Due to the intrinsic predictive-corrective structure of the algorithm, the simulation time is directly proportional to the number of iterations of the Newton loop. The computational efficiency of the algorithm can therefore be improved in two ways. Firstly, by reducing the number of Newton iterations (i.e. by improving the convergence of the loop), and secondly, by optimising the calculation of the 7 So far, a first version of the Matlab toolbox (for SMMS without closed loops) can be found at the following address: https: //gitlab.univ-nantes.fr /cosseroots /library/matlab. Likewise, the code used to generate the results of section X.B can be found at: https://gitlab.univ-nantes.fr /cosseroots /example /t-ro-2023. residual and, above all, of its Jacobian and its inversion. Regarding the last point, thanks to the strong reduction power of the strain-based parametrization [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF], the Jacobian matrix remains moderate in size for typical continuum and soft robotics designs, and its inversion remains reasonable in terms of computation time.

1) Reducing the number of Newton iterations: In the case of closed-loop systems, we can add penalties to the augmented Lagrangian in order to increase the convexity of the original variational formulation and improve the convergence of the Newton loop. Technically, these penalties simply add restoring forces and torques to the Lagrange multipliers that initialise the IDM . Finally, several other additional improvements based on the scaling of the original DAE system and its tangent linearization, will be applied in order to optimize the conditioning of the residual vector and its Jacobian matrix [START_REF] Cardona | Numerical integration of second order differential-algebraic systems in flexible mechanism dynamics[END_REF], [START_REF] Bottasso | On the optimal scaling of index three daes in multibody dynamics[END_REF].

2) Improving spatial integration: A detailed analysis on the computation times showed that the spatial integration of the forward and backward ODEs of the Cosserat rods [START_REF] Tummers | Cosserat rod modeling of continuum robots from newtonian and lagrangian perspectives[END_REF][START_REF] Bottasso | On the optimal scaling of index three daes in multibody dynamics[END_REF], and (44,54) take more than 90% of the total computation time, of which 70% is spent on the forward kinematics, and 30% on the backward dynamics. Although numerical integration by spectral methods reduces the time required, compared to the usual finite difference integration [START_REF] Boyer | Dynamics of continuum and soft robots: A strain parameterization based approach[END_REF], we are considering other options to further reduce this bottleneck. A first step will be to implement the integrations in a compiled language like C++. A second front of improvements will be the investigation of other integration methods like those based on quadrature and Magnus expansions [START_REF] Orekhov | Solving cosserat rod models via collocation and the magnus expansion[END_REF].

3) Parallelization: Newton-Euler algorithms are naturally suited to parallelization [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF]. As a first example, we can define the relative kinematics of every joint in a parallel loop before entering the forward kinematics routine. In addition, the updates of Remark 13, which are necessary before entering the backward dynamics, can be executed in a parallel loop too. Moreover, depending on the structure of the robot, if we proceed with a breadth-first algorithm to move along the robot tree, we can also parallelise the backward dynamics of every layer of the tree. Beyond this parallelization based on NE formulation, the computation of the Jacobian matrix is the main source of slowdown, since it requires the T IDM to be called several times. As this matrix is calculated column-wise, the parallelization of its calculation should considerably reduce the overall calculation time.

C. Programming simplicity

As any Newton-Euler algorithm, the computational complexity of our algorithm is O(n), meaning that it increases linearly with the number n of joints [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF]. This scalability in design and implementation allows it to be applied to complex systems with a large number of joints or complicated

Fig. 1 :

 1 Fig. 1: Different types of SMMS discussed in the article. The ellipsoids and thick lines represent rigid bodies and Cosserat rods, respectively. From left to right: (a) Treeshaped manipulator. (b) Manipulator with a closed loop. (c) Locomotor in tree form. (d) Manipulator modelled as a locomotor connected to the ground (see Remark 1). The dashed lines represent the location of the virtual cuts.

•

  Forward kinematics : eq. (39-41),[START_REF] Orekhov | Solving cosserat rod models via collocation and the magnus expansion[END_REF]

Fig. 3 :

 3 Fig. 3: Segmentation of a SMMS for IDM and T IDM . The two Cosserat rods are declared as joints J 3 and J 6 . Their two tip-cross sections are declared as rigid bodies (B 2 ,B 3 ) and (B 5 ,B 6 ).

Fig. 4 :

 4 Fig. 4: Forward transport of poses (top) and backward transport of wrenches (bottom) over a rigid (left) and a soft joint (right). The joints are coloured green, the rigid bodies red and purple.

Fig. 5 :

 5 Fig. 5: Clamped soft (a) (blue) and stiff (b) (red) beam released after bending with a horizontal tip force f = 50 N and 10 N respectively. (a) snapshots every 0.5 s for 10 s of simulation. (b) selected snapshots for 1 s of simulation.

Fig. 6 :

 6 Fig. 6: Top: Conditions of the test for the soft (a) (blue) and stiff (b) (red) rods. Snapshots of the soft (middle) and stiff (bottom) flying rod projected on x -z (left), and y -z (right) planes.

Fig. 7 :

 7 Fig. 7: Segmentation of a cuboid mass attached to two rods moved by two flying drones. The two Cosserat rods are declared as joints J 2 and J 5 , respectively. Their two tip-cross sections are declared as rigid bodies (B 1 , B 2 ) and (B 4 , B 5 ), respectively.

Fig. 8 :

 8 Fig. 8: From left to right and top to bottom: Snapshots and trajectories of the two drones and the cuboid payload between t = 0 s and t = 1.68, 4.22, and 9.44 s

Fig. 9 :

 9 Fig. 9: Displacements of body 0, 3 and 6 vs time in the inertial frame, projected onto the x (top), y (middle) and z-axis (bottom).

Fig. 11 :

 11 Fig. 11: (a) Actuation torques τ d,1 and τ d,2 vs time. (b)Revolute joint angles q r,1 and q r,2 vs time.

Fig. 12 :

 12 Fig. 12: (a) Two components of λ in the inertial frame vs time. (b) Inertial position (x, y) of the tip of the robot (passive revolute joint) vs time.

Fig. 13 :

 13 Fig. 13: (a) Snapshot every 0.5 s of the robot between: (a) 1 and 2 s, and (b) 4 and 7 s.

Fig. 14 :

 14 Fig. 14: Segmentation of a bio-inspired swimmer actuated with tendons. The two Cosserat rods are declared as joints J 2 and J 4 , respectively. Their two tip-cross sections are declared as rigid bodies (B 1 , B 2 ) and (B 3 , B 4 ), respectively.

Fig. 15 :

 15 Fig. 15: Snapshots every 1.65 s of a continuum undulatory swimmer internally controlled with a single pair of tendons.

Fig. 16 :

 16 Fig. 16: Time evolutions of τ (t) = D(T + -T -)(t), and C = (EI 3 K Z,3 )(0) along the first 3 s of swim.

The index of a system of DAEs is roughly defined as the number of times we need to differentiate the constraints in order to change the DAEs into ODEs (ordinary differential equations). For multibody systems, differentiating once (resp. twice) the geometric constraints, changes the index-3 formulation into an index-2 (resp. index-1) formulation[START_REF] Pogorelov | Differential-algebraic equations in multibody system modeling[END_REF].

This is assumed for the sake of simplicity, but the approach can be extended with no difficulty to other joints as prismatic or universal ones.

A virtual cut is a purely mathematical process. Real cuts, such as the detachment of two bodies, are not covered in this article.

Once again, this condition is imposed for the sake of simplicity, but the approach can be extended to virtual cuts applied to active joints.

Alternatively, one could use quadrature integration based on Magnus expansion in order to remain in SO(3) as this is done in[START_REF] Renda | A geometric variable-strain approach for static modeling of soft manipulators with tendon and fluidic actuation[END_REF],[START_REF] Orekhov | Solving cosserat rod models via collocation and the magnus expansion[END_REF].

A PC dell (DESKTOP-5F02EKM), Processeur Intel(R) Core(TM) i9-9880H CPU @ 2.30 GHz, RAM 32.0 Go)

We thank Frédéric Jourdan for the instructive discussions on the geometric content of this article, and dedicate this work to the memory of our colleague and friend Wisama Khalil, Newton-Euler dynamics specialist, who died accidentally on 15 November 2017. This work was supported by the French ANR COSSEROOTS (ANR-20-CE33-0001), the French project CominLabs Mambo, the Office of Naval Research Global under Grant N62909-21-1-2033, and by Khalifa University under Grant RC1-2018-KUCARS.

links form the sides of a triangle symmetrical about the horizontal axis (x) (see figure 13 (a)). Note that this initial configuration is a resting one i.e., it is free of internal stress. Once this static phasis achieved, the dynamics is started by imposing a prescribed time-evolution of the joint torques t → τ d (t) = (τ d1 , τ d2 ) T (t) defined as indicated in Table II, and plotted in figure 11 (a). The two torques start with zero values and then increase linearly with time, before reaching a constant value that is maintained until one of the two torques oscillates and finally recovers its constant value. In Table II, k = 0.5 Nm/s is the ramp coefficient, τ hold is the maximum torque reached by the ramp i.e., when t = 2 s. The harmonic phase is defined by k s = 7 Nm and θ(t) = ω(t -t hold ) with ω = 2cπ ts-thold , c = 3 the number of cycles, and t hold = 4 s and t s = 7 s, the ending time of the holding and harmonic phase respectively. Note that the sinusoidal addendum to the torque is not symmetric but ranges from 0 to 2. Figures 11 (b) and 12 (a,b) respectively show the time evolution of the joint angles, the Lagrange multipliers in the inertial frame, and the inertial position of the tip (the passive joint) for a ∆t = 0.01 s and 5 Chebyshev polynomials for each rod curvature. Figure 13 (a,b) display a sequence of snapshots over [START_REF] Guanlun | Experimental statics calibration of a multi-constraint parallel continuum robot[END_REF][START_REF] Bottcher | Design of a reconfigurable parallel continuum robot with tendon-actuated kinematic chains[END_REF] s and [START_REF] Dupont | Design and control of concentric-tube robots[END_REF][START_REF] Boyer | Poincare's equations for cosserat media: Application to shells[END_REF] s. As observed in these snapshots and the attached video, with the torque law of Table II, the CPR first bends symmetrically with its tip getting closer to the base, then because of the oscillating component added to τ d,1 , the CPR tilts upwards. While the torque makes three cycles, the up and down movement of the robot makes only two. This is due to inertia effects. When we control the robot by imposing torques, the inertia of the robot can affect the joint angles, as shown in figures 12 (a) and (b). Note that as expected, one can observe structural (harmonic) oscillations superimposed with overall deformations imposed by the torque time variations. This simulation is representative of many others we tested. In any case, we observed that using the constitutive law [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF] of Remark 9 applied to J 2 and J 4 , a slight internal damping (here µ 2 = µ 4 = 10 -4 ), is required to stabilize the algorithm. This observation is fully consistent with the architectures. Among other advantages, it allows for scalable software architectures, being well suited to object-oriented programming.

XII. APPENDIX 1: DETAILED EXPRESSIONS FOR THE NEWMARK SCHEME

The implicit Newmark scheme takes the generic form [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF], where a, b and the two functions f and h are defined for any pair of vectors of same dimension (x, y), by:

Where (β, γ) are two constant parameters such that (β, γ) = (1/4, 1/2) ensures second order accuracy with no damping, a choice that is systematically adopted in the numerical examples. Taking (x, y) 72) and (73) defines f 10) and ( 11) respectively. With these notations, the matrices C, A, B of ( 13), can be detailed as [START_REF] Boyer | Statics and dynamics of continuum robots based on cosserat rods and optimal control theories[END_REF]:

Differentiating the above expressions, one can show that:

where

XIII. APPENDIX 2: APPLICATION OF THE ALGORITHM TO AN ILLUSTRATIVE EXAMPLE

To illustrate the simulation algorithm, we apply it to the simple case of the cantilevered rod of section X (see figure 17). The rod has a length l, and a circular cross-section of area and axial moment of inertia A and I respectively. Its Young's modulus is E, and its density is ρ. According to the segmentation of section VI.A, the rod corresponds to two cross-sections B 1 and B 2 connected together through a soft joint J 2 . This soft joint is modeled as a Kirchhoff rod moving in the (e 1 , e 2 ) plane and straight at rest, i.e. B 2 = (0, 0, 1, 0, 0, 0) T and ξ o 2 = (0, 0, 0, 1, 0, 0) T , where B 2 selects the curvature field K 2,Z (.) as the system's unique Fig. 17: Segmentation and parametrization of a cantilevered rod. The rod is fixed to the ground through J 1 . Its tipcross sections define rigid bodies (B 1 , B 2 ), its internal domain is a soft joint J 2 . The basis B 0 is fixed. F e = F 0 = (O, e 1 , e 2 , e 3 ) is the inertial frame. K 2,Z (.) the curvature field of the rod deformed in the plane (e 1 , e 2 ). γ g = (9.81, 0, 0) T is the gravity acceleration field in F e . degrees of freedom. Once reduced on the basis Φ 2 of the first n ϵ Chebyshev polynomials, this field is replaced by the vector q ϵ,2 = q 2 of the n ϵ modal curvature coordinates of the rod. Referring to the general context of section IV, we thus have χ = (q 2 , q2 , q2 ) and χ = q 2 , while the residual vector (constrained by the scheme) and its Jacobian are R = R2 and (∂ R/∂ χ) = (∂ R2 /∂q 2 ) respectively. According to the flowchart 2, the predictor-corrector algorithm consists of an inner Newton correction loop included in an outer time-loop. At each new time step t n+1 , the state (q 2 , q2 ) is first predicted by forcing q2 = 0 nϵ×1 in the Newmark scheme (10) (inertial predictor), before entering the Newton loop, whose correction equation [START_REF] Sonneville | Geometrically exact beam finite element formulated on the special euclidean group se(3)[END_REF] requires computing both R2 and (∂ R2 /∂q 2 ).

The computation of R2 is performed with the IDM ⋆ algorithm of section VIII.C. Since J 2 is soft, R2 is defined by (64), where j = 2, Λ d,2 = 0 6×1 (no internal actuation), ,I,I,A,A,A) and Fext,2 = (0 1×3 , (R T 2 γ g ) T ) T (gravity). The integration of ( 44) requires first to compute the inertial kinematic fields along the rod (g 2 , η 2 , η2 )(.). Since J 1 is fixed, one can take F 1 = F e , and (g 2 , η 2 , η2 )(.) = ( 1 g 2 , η 2/1 , η2/1 )(.) is obtained by forward integrating [START_REF] Tummers | Cosserat rod modeling of continuum robots from newtonian and lagrangian perspectives[END_REF], starting from

) and ( q2 , q2 ) function of q 2 , and (q

2 ) through the Newmark scheme [START_REF] Till | Real-time dynamics of soft and continuum robots based on cosserat-rod models[END_REF]. The computation of (∂ R2 /∂q 2 ) is performed with the T IDM ⋆ algorithm of section VIII.C. This algorithm calculates the variation ∆ R2 of the outputs of the algorithm IDM ⋆ , due to a variation ∆q 2 of its inputs. Exploiting the identity ∆ R2 = (∂ R2 /∂q 2 )∆q 2 shows that by imposing ∆q 2 = δ α , α = 1, 2...n ϵ , we can calculate all the columns of (∂ R2 /∂q 2 ) one after the other. This algorithm proceeds as the previous one except that (66) replaces (64) and the back-Algorithm 1: Simulation of the cantilevered rod.

1 Initial conditions: 2 q2 = q2 init, q2 = q2 = 0n ϵ×1 ; 3 Boundary conditions: 1 g2(0) = 14×4, η 2/1 (0) = η2/1 (0) = Λ2(l) = 06×1; 4 ∆ζ 2/1 (0) = ∆η 2/1 (0) = ∆ η2/1 (0) = ∆Λ2(l) = 06×1; 5 for n = 0 . . . (t f -t0)/∆t do 6 q (n) 2

Prediction (Newmark scheme s.t. q2 = 0n ϵ ×1):

Correction: R2 = 1; Integrate forward from X = 0 to l Eq. ( 41) to get: ward and forward ODEs ( 44) and ( 41) are replaced by ( 54) and ( 50), the first being initialized with ∆Λ 2 (l) = 0 6×1 , and the second by ∆ζ 2/1 (0) = ∆η 2/1 (0) = ∆ η2/1 (0) = 0 6×1 . In (54) ∆ Fext,2 = (0 1×3 , (∆R T 2 γ g ) T ) T with ∆R T 2 γ g = ((R T 2 γ g ) ∧ , 0 3×3 )∆ζ 2 . In (50), (∆ q2 , ∆q 2 ) = (a∆q 2 , b∆q 2 ). This simulation algorithm is summarized by the pseudocode Alg. 1, where all space-integrations of ODEs are achieved in cascade with a standard spectral method as indicated in section IX. Note also that in this particular case, J 1 being fixed and the cross-sections of the rod ends B 1 and B 2 being massless, their model, used in the boundary conditions, introduces trivial twist and wrench transfers.

XIV. ACKNOWLEDGMENT