Analytical derivatives of strain-based dynamic model for hybrid soft-rigid robots

Abstract

Algorithms that use derivatives of governing equations have accelerated rigid robot simulations and improved their accuracy, enabling the modeling of complex, real-world capabilities. However, extending these methods to soft and hybrid soft-rigid robots is significantly more challenging due to the complexities in modeling continuous deformations inherent in soft bodies. A considerable number of soft robots and the deformable links of hybrid robots can be effectively modeled as slender rods. The Geometric Variable Strain (GVS) model, which employs the screw theory and the strain-parameterization of the Cosserat rod, extends the rod theory to model hybrid soft-rigid robots within the same mathematical framework. Using the Recursive Newton-Euler Algorithm, we developed the analytical derivatives of the governing equations of the GVS model. These derivatives facilitate the implicit integration of dynamics and provide the analytical Jacobian of the statics residue, ensuring fast and accurate computations. We applied these derivatives to the mechanical simulations of six common robotic systems: a soft cable-driven manipulator, a hybrid serial robot, a fin-ray finger, a hybrid parallel robot, a contact scenario, and an underwater hybrid mobile robot. Simulation results demonstrate substantial improvements in computational efficiency, with speed-ups of up to three orders of magnitude (1000x). We validate the model by comparing simulations done with and without analytical derivatives. Beyond static and dynamic simulations, the techniques discussed in this paper hold the potential to revolutionize the analysis, control, and optimization of hybrid robotic systems for real-world applications.

1. Introduction

Rigid-body algorithms have been developed for the mechanical analysis of multi-body systems, enabling the modeling of dynamic response and static equilibrium of robots (Featherstone, 2008; Murray et al., 1994). Over the years, these algorithms have undergone remarkable improvements, enabling faster-than-real-time simulations (Newbury et al., 2024). One of the most significant advancements has been the incorporation of gradient information into these algorithms (Carpentier et al., 2019; Howell et al., 2023; Giftthaler et al., 2017a; Geilinger et al., 2020). The ability to accurately and efficiently compute the derivatives of governing equations with respect to the system’s state, model parameters, and control variables has been pivotal for implicit integration, design optimization, trajectory optimization, and optimal control. The impact of these advancements is exemplified by leading humanoid and quadrupedal robots, such as those developed by Boston Dynamics and Unitree (Bjelonic et al., 2022). These robots leverage the gradient information for trajectory optimization and model-predictive control (MPC), enabling real-time control, enhanced stability, and adaptability in complex environments (Sukhija et al., 2023; Neunert et al., 2018; Wensing et al., 2024).

Several methods exist for computing the derivatives of governing equations, each with its trade-offs (Newbury et al., 2024). The most straightforward approach is the numerical scheme of finite difference. The finite difference method can offer simplicity and ease of parallelization but often falls short in accuracy (Todorov et al., 2012). Automatic differentiation (AutoDiff) computes derivatives by recognizing that even complex functions are built from fundamental operations and functions. It systematically applies the chain rule to these operations within the algorithm, allowing the program to automatically calculate derivatives alongside the original calculations (Tedrake and the Drake Development Team, 2019; Giftthaler et al., 2017b). However, AutoDiff involves intermediate computations that are generally hard to simplify. Analytical methods take a manual approach by directly applying the chain rule to recursive algorithms like the Recursive Newton-Euler Algorithm (RNEA) (Carpentier and Mansard, 2018; Singh et al., 2022). By exploiting the inherent structure and spatial algebra at the core of rigid-body dynamics algorithms, analytical derivatives (AD) can simplify computations and achieve greater computational efficiency than automatic differentiation methods. Efficient implementation of analytical derivatives can lead to faster and more resource-efficient computations with improved accuracy and provide deeper insight into the mathematical structure of the derivatives. However, deriving analytical derivatives manually can be complex and time-consuming, making them challenging to implement (Singh et al., 2022).

Deriving analytical derivatives in soft robots is significantly more challenging than in rigid-body systems. The primary difficulty stems from the complex nature of deformable bodies, which undergo large, continuous deformations, making it highly challenging to derive closed-form equations for their dynamics. The general class of soft robots, which cannot be modeled as a system rods, require numerical methods based on 3D continuum mechanics such as Finite Element Methods (FEM) (Duriez, 2013) or Material Point Method (MPM) (Hu et al., 2018). Analytical derivatives of FEM in robotics have been explored in several works, including Hoshyari et al. (2019); Hafner et al. (2019); Geilinger et al. (2020); Bächer et al. (2021); Jatavallabhula et al. (2021); Du et al. (2022). On the other hand, MPM is often referred to as naturally differentiable due to its particle-grid representation and the smooth interpolation between particles and the grid, which makes gradient computation more efficient (Spielberg et al.. 2023; Huang et al., 2021). FEM and MPM use maximal coordinate representations for rigid bodies, increasing the degrees of freedom (DoF) and the computational cost for hybrid soft-rigid robots.

A large portion of soft robots and the compliant links of hybrid soft-rigid robots can be effectively modeled as slender, rod-like structures, making them well-suited for analysis using the Cosserat rod theory (Armanini et al., 2023). The Cosserat rod is a 1D continuum mechanics object that can model deformations of slender bodies, including twisting, bending in two axes, stretching, and shearing in two axes (Cao and Tucker, 2008). Four distinct research communities have leveraged Cosserat rod theory to address their specific challenges, each producing specialized numerical methods tailored to their needs. Ranking them by their order of appearance, these communities are: the geometrically exact FEM community, the ocean engineering community, the computer graphics community, and the robotics community. The geometrically exact FEM community has focused primarily on developing FEM software that can predict the movements and stresses of mechanisms undergoing large deformations (Simo and Vu-Quoc, 1988; Meier et al., 2017; Eugster and Harsch, 2023). Meanwhile, the ocean engineering community has applied Cosserat rod models to the simulation of towed submarine cables, addressing the unique challenges posed by underwater environments (Burgess, 1992; Tjavaras et al., 1998). On the other hand, the computer graphics community has prioritized computational speed for interactive simulations of filament-like structures, such as hair, using the Discrete Elastic Rod (DER) approaches (Bergou et al., 2008; Gazzola et al., 2018). Finally, the robotics community has concentrated on the simulation and control of soft or continuum robots to safely interact with their surroundings (Rucker and Webster, 2011; Till et al., 2019; Boyer et al., 2022).

To cater specifically to the needs of robotics, a novel parameterization of the configuration space of Cosserat rods has been proposed, focusing on strain fields rather than the traditional pose-based approach used in FEM and DER. This strain-based approach, referred to as the Geometric Variable Strain (GVS) method (Renda et al., 2020; Boyer et al., 2020), is geometrically exact and aligns well with the demands of soft robotic applications. The GVS approach provides a reduced-order formulation with the least number of generalized coordinates for modeling their dynamic and compliant behavior. Among the various models based on the Cosserat rod, the GVS model stands out for its ability to merge the screw theory-based formulation of rigid robots with the strain-parameterization of the rod. Its Lagrangian mechanics formulation with minimal generalized coordinates enables efficient analysis of hybrid soft-rigid robotic systems within a unified mathematical framework (Figure 1). The implementation of the GVS model, based on the approximate Magnus expansion of the rod’s strain field, makes the soft body computationally equivalent to multidimensional, discrete rigid joints (Mathew et al., 2024). Hence, the strain-parameterization of our formulation aligns more closely with traditional rigid-body models than alternative methods, ensuring compatibility with established robotics frameworks. The model has been extensively validated through comparisons with analytical models, FEM, and other rod models (Boyer et al., 2023; Mathew et al., 2022a; Tummers et al., 2023), as well as experimental studies (Alkayas et al., 2025; Feliu-Talegon et al., 2024; Tummers et al., 2024). In Mathew et al. (2022a, 2024), we implemented the GVS model for hybrid soft robots using a built-in implicit integration scheme in MATLAB called ode15s. When analytical derivatives (Jacobian) are not supplied, ode15s internally compute the Jacobian of the governing equations using a finite difference scheme. However, this can lead to longer computational times or cause the solver to stall due to errors introduced by the numerical approximation, particularly in high-DoF systems with constraints. The objective of this work is to develop and implement an analytical derivative for the GVS model, aiming to improve computational efficiency and robustness in the simulation of hybrid soft-rigid robots.

Figure 1.

In the GVS model, a rigid joint is equivalent to a fictitious constant strain Cosserat rod and a soft body is equivalent to an infinite series of rigid joints. The infinitesimal change in configuration is captured by the local velocity η and strain twists ξ . The hybrid soft-rigid robot shown in the figure is adapted from Mathew et al. (2025a).

Related works: The Piecewise Constant Strain (PCS) model, which discretizes the continuous deformation of a Cosserat rod into a finite number of segments with constant strain, is a subclass of the GVS model. Based on the Comprehensive Motion Transformation Matrix (a Lie group of coordinate transformations of displacement, velocity, and acceleration), an analytical gradient of the PCS model was derived in Ishigaki et al. (2024). A differentiable soft robot simulation environment called DisMech was introduced based on the implicit DER method (Choi et al., 2024). DisMech employs a finite difference scheme to compute the necessary gradients for the implicit integration. Recently, a new algorithm for the implicit dynamics of robots with rigid bodies and Cosserat rods has been proposed (Boyer et al., 2023). By applying an exact symbolic differentiation of the robot’s RNEA inverse dynamics (named IDM in Boyer et al. (2023)), a new RNEAlgorithm, called the tangent inverse dynamics model (TIDM), has been derived. This algorithm is then fed with unit inputs to numerically calculate the Jacobian of the inverse dynamics. This calculation is performed at the continuous level, that is, directly on the partial differential equations (PDEs), and before spatial integration, for which spectral methods have been employed instead of Magnus expansion. Although the approach adheres to the true definition of geometrically exact methods—where discretization of time and space occurs only after all analytical calculations—due to its implicit nature, it does not directly provide Jacobian matrices in analytical matrix form, which can be advantageous for control and fast simulation of robots with MATLAB. In contrast, the approach here presented lies in its analytic and explicit formulation, which was made possible by the Magnus expansion.

Contributions of this work: By leveraging the “pseudo-rigid joint” formulation of GVS and building upon established methods for analytical derivatives in rigid-body systems (Carpentier and Mansard, 2018; Singh et al., 2022), we developed analytical derivatives for soft and hybrid soft-rigid robots with slender soft bodies. While Mathew et al. (2024) formulated the governing equations of the GVS model for a hybrid soft-rigid robot, focusing on various strain-parameterizations and the computational procedure, this paper focuses on deriving their analytical gradients and developing an efficient algorithm for their computation. We implemented the derivatives in two implicit integration algorithms: ode15s of MATLAB and Newmark-β scheme for dynamics and provided the analytical Jacobian for efficient static equilibrium computation. Our method significantly improved the computational speed of implicit integration for dynamic simulations, achieving speed-ups of up to three orders of magnitude compared to traditional methods without analytical derivatives. Similarly, for static analysis, we observed substantial improvements in computational efficiency. Six common robotic systems are considered for the simulation study. Additionally, we implemented the theory developed in this paper into our GUI-based MATLAB toolbox, SoRoSim, making it a fully differentiable robotics toolbox capable of modeling generic hybrid soft-rigid robots with slender soft rods. (Mathew et al., 2025b). To the best of the authors’ knowledge, this is the first work to derive and implement analytical derivatives for the mechanical analysis of hybrid soft-rigid robotic systems of this kind.

Summary of Contributions:

  (1) Derivation of the analytical gradient of the GVS model.

  (2) Integration of the gradient information for static and dynamic simulations.

  (3) Analysis of different robotic systems validating the analytical gradient and demonstrating computational efficiency.

  (4) Integration of the theory into the SoRoSim toolbox, making it a differentiable tool for modeling hybrid soft-rigid robots.

Organization of the paper: A summary of the GVS model is presented in Section 2. Section 3 details the implementation of analytical derivatives in dynamic and static algorithms for fast and accurate computations. In Section 4, we focus initially on a single soft body, applying the RNEA and ID framework to derive the analytical derivatives of the governing equations. This framework is extended to hybrid multi-body systems in Section 5. We address two typical constraints in multi-body systems: joint actuation via joint coordinates and closed-chain (CC) systems. Section 6 discusses the derivation of analytical derivatives for systems subjected to three common external forces: point forces, contact loads, and hydrodynamic forces. Simulations and validations across six robotic systems are presented across these sections, demonstrating the effectiveness of the approach.

Table 1 lists all the important symbols used in this paper. Readers are encouraged to refer to the Supplemental Videos to visualize the dynamic simulation results presented in this work.

Table 1.

List of Symbols and Their Descriptions.

Symbol	Description
n dof	Total degrees of freedom
n a	Number of actuators
N	Number of Cosserat rods (including rigid joints)
n p	Number of computational points
q	Generalized coordinates
$\dot{\mathit{q}}$	Generalized velocities
$\ddot{\mathit{q}}$	Generalized accelerations
x	Generalized robot state. $\mathit{x}={[{\mathit{q}}^T{\dot{\mathit{q}}}^T]}^T$
h	Time step
t	Time
X	Curvilinear abscissa of the soft body, X ∈ [0, L]
FD	Forward dynamics
ID	Inverse dynamics
M	Generalized mass matrix
F	Generalized external and coriolis force
τ	Generalized internal force
B	Generalized actuation matrix
u	Actuator strength
K	Generalized stiffness matrix
D	Generalized damping matrix
ξ	Screw strain
Φ ξ	Strain basis
(•) α	Quantity at joint or computational point α
(•) B	Quantities computed during the forward pass
(•)C,S	Quantities computed during the backward pass
S α	Joint motion subspace matrix
${\mathcal{F}}_k$	Local wrench
${\mathcal{M}}_k$	Screw inertia matrix
η k	Velocity twist
${\dot{\mathit{\eta }}}_k$	Acceleration twist
$\mathcal{G}$	$\mathcal{G}={[{\mathbf{0}}^T{\mathit{a}}_g^T]}^T$ , where a g is the acceleration due to gravity
g	Homogeneous transformation matrix in SE (3)
J	Geometric (kinematic) Jacobian matrix
Ad(⋅), ${\mathrm{A}\mathrm{d}}_{(\cdot )}^{*}$	Adjoint maps in SE (3)
ad(⋅), ${\mathrm{a}\mathrm{d}}_{(\cdot )}^{*}$ , ${\overline{\mathrm{a}\mathrm{d}}}_{(\cdot )}^{*}$	Adjoint operators in $\mathfrak{s}\mathfrak{e}(3)$

2. Summary of the GVS model

The GVS model introduces generalized coordinates ( q ) using a variable strain-parameterization of the Cosserat rod:

${\mathit{\xi }}_i(X,{\mathit{q}}_i)={\mathbf{\Phi }}_{{\xi }_i}(X){\mathit{q}}_i+{\mathit{\xi }}_i^{*}(X)$

(1)

where X ∈ [0, L _i ] is the curvilinear abscissa of the rod, $\mathit{\xi }\in {\mathbb{R}}^6$ is the screw strain, ${\mathbf{\Phi }}_{{\xi }_i}\in {\mathbb{R}}^{6\times {n_{\mathit{dof}}}_i}$ ( ${n_{\mathit{dof}}}_i$ is the degrees of freedom) is a strain basis, and ξ * is the reference strain. The subscript i indicates the i^th Cosserat rod. In the GVS formulation, a rigid joint is equivalent to a fictitious Cosserat rod spanning from X = 0 to X = 1 (Mathew et al., 2024). For a rigid joint, the strain twist is equivalent to the joint twist in ${\mathbb{R}}^6$ and is independent of X.

For a hybrid robot with N Cosserat rods (including rigid joints and soft bodies), the state of the robot x is governed by the generalized coordinates $\mathit{q}\in {\mathbb{R}}^{n_{\mathit{dof}}}$ $(n_{\mathit{dof}}={\sum }_{i=1}^N{n_{\mathit{dof}}}_i)$ and its time derivative $\dot{\mathit{q}}$ . Using q , $\dot{\mathit{q}}$ and basis functions, ξ and $\dot{\mathit{\xi }}$ can be computed at each rigid joint and at any computational point on the soft bodies.

Using these, a recursive scheme that employs the exponential map of the Lie algebra of SE (3) is implemented to compute the robot’s forward and differential kinematics (Appendix A). This yields g _i ∈ SE (3), the homogeneous transformation matrix mapping from the inertial frame to the local frame, ${\mathit{J}}_i\in {\mathbb{R}}^{6\times n_{\mathit{dof}}}$ , the geometric Jacobian in the local frame, and its time derivative ${\dot{\mathit{J}}}_i$ at discrete computational point along the rod domains.

$\begin{array}{l}\hfill {\mathit{g}}_{i,\alpha +1}={\mathit{g}}_{i,\alpha }\exp \left({\widehat{\mathbf{\Omega }}}_{i,\alpha }\right)\end{array}$

(2a)

$\begin{array}{l}\hfill {\mathit{J}}_{i,\alpha +1}={\mathrm{A}\mathrm{d}}_{\exp \left({\widehat{\mathbf{\Omega }}}_{i,\alpha }\right)}^{-1}\left({\mathit{J}}_{i,\alpha }+\left[{\mathbf{0}}_{6\times n_i^{-}}{\mathit{S}}_{\alpha }{\mathbf{0}}_{6\times n_i^{+}}\right]\right)\end{array}$

(2b)

$\begin{array}{l}\hfill {\dot{\mathit{J}}}_{i,\alpha +1}={\mathrm{A}\mathrm{d}}_{\exp \left({\widehat{\mathbf{\Omega }}}_{i,\alpha }\right)}^{-1}({\dot{\mathit{J}}}_{i,\alpha }+[{\mathbf{0}}_{6\times n_i^{-}}{\dot{\mathit{S}}}_{\alpha }+{\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_{i,\alpha }}{\mathit{S}}_{\alpha }{\mathbf{0}}_{6\times n_i^{+}}])\end{array}$

(2c)

where, Ω_i,α( q _i ) is an approximation of the Magnus expansion of ξ _i , representing an equivalent joint twist from α to α + 1, ${\mathit{S}}_{\alpha }({\mathit{q}}_i)\in {\mathbb{R}}^{6\times {n_{\mathit{dof}}}_i}$ is the joint motion subspace matrix, and ${\dot{\mathit{S}}}_{\alpha }({\mathit{q}}_i,\dot{{\mathit{q}}_i})$ is its time derivative. ${\mathit{\eta }}_{i,\alpha }={\mathit{J}}_{i,\alpha }\dot{\mathit{q}}$ is the screw velocity in the local frame. The matrix dimensions $n_i^{-}={\sum }_{k=0}^{i-1}{n_{\mathit{dof}}}_k$ and $n_i^{+}={\sum }_{k=i+1}^N{n_{\mathit{dof}}}_k$ . $\widehat{(\cdot )}$ is an isomorphism from ${\mathbb{R}}^6$ to $\mathfrak{s}\mathfrak{e}(3)$ and Ad_(⋅) represents the Adjoint map in SE (3). Note that these equations also apply to rigid joints, providing a mapping from X = 0 to X = 1. Readers may refer to Appendixes A and C for their analytical expressions.

Based on D’Alembert’s principle, the free dynamics of soft links and rigid bodies are projected onto the generalized coordinate space using the geometric Jacobian (Kane method (Kane and Levinson, 1983)), yielding the generalized mass matrix M , the generalized internal force τ , and the generalized external and Coriolis forces F . The soft body is typically discretized into computational points for numerical spatial integration. For this numerical integration, the continuous domain of X is discretized into n _p computational points, and the integral of a distributed quantity $\overline{\mathit{Q}}$ is approximated as a weighted sum of the integrands.

${\int }_0^{L_i}{\overline{\mathit{Q}}}_{i}dX=\sum _{k=1}^{n_p}{w}_k{{\overline{\mathit{Q}}}_i}_k$

(3)

Finally, the Forward Dynamics (FD) computes $\ddot{\mathit{q}}$ by solving,

$\begin{array}{c}\hfill \mathit{M}(\mathit{q})\ddot{\mathit{q}}=\mathit{\tau }(\mathit{q},\dot{\mathit{q}},\mathit{u})+\mathit{F}(\mathit{q},\dot{\mathit{q}})\end{array}$

(4)

where $\mathit{u}\in {\mathbb{R}}^{n_a}$ is the vector of applied actuation forces.

Figure 2 presents a graphical summary of the model implementation. The dynamic simulation computes the temporal evolution of the robot’s state by time-integrating ${[{\dot{\mathit{q}}}^T,{\ddot{\mathit{q}}}^T]}^T$ , while static simulation involves the computation of q for which the following equation is satisfied.

$\begin{array}{c}\hfill \mathit{\tau }(\mathit{q},\mathbf{0},\mathit{u})+\mathit{F}(\mathit{q},\mathbf{0})=0\end{array}$

(5)

Figure 2.

Graphical summary of the GVS model: (a) Schematic of the implemented recursive scheme and (b) block diagram showing the summary of the GVS FD algorithm.

For further details on the model and its computational implementation, readers are encouraged to refer to Mathew et al. (2024).

3. Derivatives for dynamics and statics

Analytical derivatives of governing equations, (4) and (5), are crucial for efficiently solving dynamic and static problems. Here, we explain how the derivatives can be utilized to ensure accurate and fast computations.

3.1. Implicit Euler integration using inverse dynamics

The temporal evolution of the robot state $\mathit{x}={[{\mathit{q}}^T,{\dot{\mathit{q}}}^T]}^T$ can be computed numerically by integrating $\dot{\mathit{x}}$ using an explicit or implicit scheme (Butcher, 2016).

The explicit Euler method is given by

${\mathit{x}}_{n+1}={\mathit{x}}_n+h\dot{\mathit{x}}(t_n,{\mathit{x}}_n)$

(6)

where t is the time, h is the step size, and the subscripts n and n + 1 indicate current and next time steps, respectively.

For an explicit integrator, $\dot{\mathit{x}}$ is a function of the current robot state. In contrast, the implicit Euler method is

${\mathit{x}}_{n+1}={\mathit{x}}_n+h\dot{\mathit{x}}(t_{n+1},{\mathit{x}}_{n+1})$

(7)

Since x _n+1 appears on both sides of the equation, it necessitates a solution through iteration. The equation can be solved for the unknown x _n+1 by making it a residual:

${\mathit{R}}_{\mathit{dyn}}({\mathit{x}}_{n+1})={\mathit{x}}_{n+1}-{\mathit{x}}_n-h\dot{\mathit{x}}(t_{n+1},{\mathit{x}}_{n+1})$

(8)

The Jacobian of this equation is given by

${\mathit{J}}_{\mathit{dyn}}({\mathit{x}}_{n+1})=\mathit{I}-h{\left.\frac{\partial \dot{\mathit{x}}}{\partial \mathit{x}}\right|}_{n+1}$

(9)

With an initial guess of ${\mathit{x}}_{{n+1}_{\mathit{guess}}}={\mathit{x}}_n$ , an iterative solver such as the Newton-Raphson method uses the residual and the Jacobian to march towards the solution. We used the ode15s function in MATLAB, a variable-step, variable-order ODE solver that uses an advanced form of the implicit Euler method. Similar to the Euler scheme, ode15s utilizes the state derivative $(\dot{\mathit{x}})$ and its Jacobian for time integration. For a Lagrangian system governed by (4),

$\begin{array}{ll}\hfill \dot{\mathit{x}}& =\left[\begin{array}{c}\hfill \dot{\mathit{q}}\hfill \\ \hfill FD\hfill \end{array}\right]\hfill \end{array}$

(10a)

$\begin{array}{ll}\hfill \frac{\partial \dot{\mathit{x}}}{\partial \mathit{x}}& =\left[\begin{array}{cc}\hfill {\mathbf{0}}_{n_{\mathit{dof}}\times n_{\mathit{dof}}}\hfill & \hfill {\mathit{I}}_{n_{\mathit{dof}}}\hfill \\ \hfill \frac{\partial FD}{\partial \mathit{q}}\hfill & \hfill \frac{\partial FD}{\partial \dot{\mathit{q}}}\hfill \end{array}\right]\hfill \end{array}$

(10b)

While the Jacobian can be computed numerically or analytically, the analytical computation can provide faster and more accurate results. To obtain the derivatives of FD for the computation of the Jacobian, we take a partial derivative of equation (4) with respect to q :

$\frac{\partial \mathit{M}}{\partial \mathit{q}}\ddot{\mathit{q}}+\mathit{M}\frac{\partial FD}{\partial \mathit{q}}=\frac{\partial \mathit{\tau }}{\partial \mathit{q}}+\frac{\partial \mathit{F}}{\partial \mathit{q}}$

By rearranging terms, we get:

$\frac{\partial FD}{\partial \mathit{q}}={\mathit{M}}^{-1}\left(\frac{\partial \mathit{\tau }}{\partial \mathit{q}}-\frac{\partial ID}{\partial \mathit{q}}\right)$

(11)

where ID is the inverse dynamics given by

$ID(\mathit{q},\dot{\mathit{q}},\ddot{\mathit{q}})=\mathit{M}(\mathit{q})\ddot{\mathit{q}}-\mathit{F}(\mathit{q},\dot{\mathit{q}})$

(12)

with $\ddot{\mathit{q}}$ treated as an independent variable.

Similarly, we get

$\frac{\partial FD}{\partial \dot{\mathit{q}}}={\mathit{M}}^{-1}\left(\frac{\partial \mathit{\tau }}{\partial \dot{\mathit{q}}}-\frac{\partial ID}{\partial \dot{\mathit{q}}}\right)$

(13)

3.2. Newmark-β method

The Newmark β method is another advanced implicit scheme, widely used in structural mechanics where it has been improved over the years thanks to the HHT or the Generalized-α-method among others (Cardona and Géradin, 1994; Chung and Hulbert, 1993). Recently, it has been shown that the Newmark scheme is symplectic and variational (Kane et al., 2000), which may explain the reasons for its success in the FEM community. In the Newmark scheme, ${\ddot{\mathit{q}}}_{n+1}$ , ${\dot{\mathit{q}}}_{n+1}$ , and q _n+1 are solved using equation (4) together with:

$\begin{array}{c}{\dot{\mathit{q}}}_{n+1}=\frac{\gamma }{\beta h}({\mathit{q}}_{n+1}-{\mathit{q}}_n)+\left(1-\frac{\gamma }{\beta }\right){\dot{\mathit{q}}}_n+h\left(1-\frac{\gamma }{2\beta }\right){\ddot{\mathit{q}}}_n\hfill \\ {\ddot{\mathit{q}}}_{n+1}=\frac{1}{\beta h^2}({\mathit{q}}_{n+1}-{\mathit{q}}_n)-\frac{1}{\beta h}{\dot{\mathit{q}}}_n+\left(1-\frac{1}{2\beta }\right){\ddot{\mathit{q}}}_n\hfill \end{array}$

(14)

where β and γ are parameters that can be adjusted to control the stability and accuracy of the method. For instance, (β, γ) = (1/4,1/2) ensures second-order accuracy with no damping. Using equation (14), we can convert (4) into a residue of q _n+1:

${\mathit{R}}_{\mathit{dyn}}({\mathit{q}}_{n+1})=\mathit{\tau }({\mathit{q}}_{n+1})-ID({\mathit{q}}_{n+1})$

(15)

where the Jacobian is given by

$\begin{array}{c}{\mathit{J}}_{\mathit{dyn}}({\mathit{q}}_{n+1})=\frac{\partial \mathit{\tau }}{\partial \mathit{q}}+\frac{\gamma }{\beta h}\frac{\partial \mathit{\tau }}{\partial \dot{\mathit{q}}}\hfill \\ -\left(\frac{\partial ID}{\partial \mathit{q}}+\frac{\gamma }{\beta h}\frac{\partial ID}{\partial \dot{\mathit{q}}}+\frac{1}{\beta h^2}\frac{\partial ID}{\partial \ddot{\mathit{q}}}\right)\hfill \end{array}$

(16)

The solution for q _n+1 can be obtained iteratively, starting with an initial guess ${\mathit{q}}_{{n+1}_{\mathit{guess}}}={\mathit{q}}_n$ and using a time step h. The process can be computationally efficient if the analytical Jacobian is provided. The Newmark-β method can be efficient, particularly for index-3 Differential Algebraic Equations (DAEs) with a large number of kinematic constraints. We implemented from scratch a custom MATLAB code based on dynamic equations (15) and (16). Unlike ode15s, the Newmark scheme works with a fixed time step, which has been chosen for each example, so that the position offset between the custom code output and ode15s, is of the order of millimeters.

3.3. Computation of static equilibrium

The role of the Jacobian in the statics simulation is straightforward. The residual and Jacobian for solving the static equilibrium can be derived from equations (5) and (12) with $\dot{\mathit{q}}=\mathbf{0}$ and $\ddot{\mathit{q}}=\mathbf{0}$ .

$\begin{array}{l}\hfill {\mathit{R}}_{\mathit{sta}}(\mathit{q})=\mathit{\tau }-ID\end{array}$

(17a)

$\begin{array}{l}\hfill {\mathit{J}}_{\mathit{sta}}(\mathit{q})=\frac{\partial \mathit{\tau }}{\partial \mathit{q}}-\frac{\partial ID}{\partial \mathit{q}}\end{array}$

(17b)

Numerical solvers such as fsolve in MATLAB, use (17a) with an initial guess q _guess and march towards a solution using the Jacobian (17b). If the analytical Jacobian is not provided, the solver uses numerical techniques, such as finite differences, to compute it. Therefore, with the analytical derivative of the residue, the problem can be solved quickly and efficiently.

4. Derivatives of soft body dynamics

We begin by deriving the derivative for a single soft body and then extend this approach to a hybrid multi-body system. For simplicity, we omit the subscript i in this section. By virtue of the Magnus expansion, the soft body is computationally equivalent to n _p − 1 rigid joints governed by the same q . The computation of the partial derivative of FD requires determining the partial derivative of ID, according to equations (11) and (13). The most efficient algorithm for the computation of ID is the RNEA (Featherstone, 2008). It is a two-pass algorithm with a forward pass that calculates link kinematics and a backward pass that determines the joint wrench needed to produce this motion. The schematic of RNEA for a soft body is shown in Figure 3. According to RNEA, the ID of the discretized soft body is given by:

$\begin{array}{c}\hfill ID=\sum _{\alpha =1}^{n_p-1}I{D}_{\alpha }=\sum _{\alpha =1}^{n_p-1}{\mathit{S}}_{\alpha }^T{\mathcal{F}}_{\alpha }^C\end{array}$

(18)

where ${\mathcal{F}}_{\alpha }^C$ is the resultant of all the dynamic (inertial and Coriolis) and external wrenches (such as gravity) acting on all points k > α, transformed to the frame of α (Figure 3).

${\mathcal{F}}_{\alpha }^C=\sum _{k=\alpha +1}^{n_p}{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha k}}^{*}{\mathcal{F}}_k$

(19)

where ${\mathrm{A}\mathrm{d}}_{(\cdot )}^{*}\in {\mathbb{R}}^{6\times 6}$ is coadjoint map of SE (3) and ${\mathcal{F}}_k$ is the resultant point wrench in the local frame of k, given by the sum of the inertial and Coriolis forces minus the external force. For a distributed wrench $({\overline{\mathcal{F}}}_k)$ , an equivalent point wrench, given by ${\mathcal{F}}_k=w_k{\overline{\mathcal{F}}}_k$ , where w _k is the quadrature weight for integration, is considered. For convenience, we drop the overbar above all the distributed quantities using this rule. We have

${\mathcal{F}}_k={\mathcal{M}}_k{\dot{\mathit{\eta }}}_k+{\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_k}^{*}{\mathcal{M}}_k{\mathit{\eta }}_k-{\mathcal{M}}_k{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_k}^{-1}\mathcal{G}$

(20)

where, ${\mathcal{M}}_k\in {\mathbb{R}}^{6\times 6}$ is the screw inertia matrix, η _k and ${\dot{\mathit{\eta }}}_k$ and the local velocity and acceleration twists, $\mathcal{G}={[{\mathbf{0}}^T{\mathit{a}}_g^T]}^T$ , where a _g is the acceleration due to gravity in the global frame, and ${\mathrm{a}\mathrm{d}}_{(\cdot )}^{*}$ is the coadjoint operator (Appendix A).

Figure 3.

Schematic of the RNEA for a single soft body showing the forward pass for kinematics and the backward pass for joint wrench computation. The joint motion subspace ( S _α ) projects the resultant wrench acting on the joint into the generalized coordinate space.

With this we proceed to compute the partial derivative of ID with respect to the states of the robot. A summary of the results is presented below. For detailed derivations, readers are encouraged to refer to Appendixes C and D.

4.1. Partial derivative of ID with respect to generalized coordinates

Substituting equations (19) in (18), we can see that the partial derivative of ID _α with respect to q is given by

$\begin{array}{c}\hfill \frac{\partial I{D}_{\alpha }}{\partial \mathit{q}}=\frac{\partial {\mathit{S}}_{\alpha }^T}{\partial \mathit{q}}{\mathcal{F}}_{\alpha }^C+{\mathit{S}}_{\alpha }^T\sum _{k=\alpha +1}^{n_p}\frac{\partial {\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha k}}^{*}}{\partial \mathit{q}}{\mathcal{F}}_k\\ +{\mathit{S}}_{\alpha }^T\sum _{k=\alpha +1}^{n_p}{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha k}}^{*}\frac{\partial {\mathcal{F}}_k}{\partial \mathit{q}}\hfill \end{array}$

(21)

The analytical formula for the first term is provided in the Appendix C. The second and third terms involve sum over k from α + 1 to n _p . The derivations of their summands are given in the Appendix D. We get

$\frac{\partial {\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha k}}^{*}}{\partial \mathit{q}}{\mathcal{F}}_k=\sum _{\beta =\alpha }^{k-1}{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha \beta }}^{*}{\overline{\mathrm{a}\mathrm{d}}}_{{\text{Ad}}_{{\mathit{g}}_{\beta k}}^{*}{\mathcal{F}}_k}^{*}{\mathit{S}}_{\beta }$

(22)

and

$\begin{array}{c}\hfill \frac{\partial {\mathcal{F}}_k}{\partial \mathit{q}}={\mathcal{N}}_k\sum _{\beta =1}^{k-1}{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\beta k}}^{-1}{\mathit{R}}_{\beta }+{\mathcal{M}}_k\sum _{\beta =1}^{k-1}{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\beta k}}^{-1}{\mathit{Q}}_{\beta }\end{array}$

(23)

where,

$\begin{array}{l}\hfill {\mathcal{N}}_k={\overline{\mathrm{a}\mathrm{d}}}_{{\mathcal{M}}_k{\mathit{\eta }}_k}^{*}+{\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_k}^{*}{\mathcal{M}}_k-{\mathcal{M}}_k{\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_k}\end{array}$

(24a)

$\begin{array}{l}\hfill {\mathit{R}}_{\beta }={\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_{\beta }^{+}}{\mathit{S}}_{\beta }+\frac{\partial {\mathit{S}}_{\beta }}{\partial \mathit{q}}\dot{\mathit{q}}\end{array}$

(24b)

$\begin{array}{l}{\mathit{Q}}_{\beta }={\mathrm{a}\mathrm{d}}_{{\dot{\mathit{\eta }}}_{\beta }^{+}}{\mathit{S}}_{\beta }+{\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_{\beta }^{+}}{\mathit{R}}_{\beta }\hfill \end{array}$

(24c)

$\begin{array}{ll}\hfill & +{\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_{\beta }}\frac{\partial {\mathit{S}}_{\beta }}{\partial \mathit{q}}\dot{\mathit{q}}+\frac{\partial {\dot{\mathit{S}}}_{\beta }}{\partial \mathit{q}}\dot{\mathit{q}}+\frac{\partial {\mathit{S}}_{\beta }}{\partial \mathit{q}}\ddot{\mathit{q}}-{\mathrm{a}\mathrm{d}}_{{\text{Ad}}_{{\mathit{g}}_{\beta }}^{-1}\mathcal{G}}{\mathit{S}}_{\beta }\hfill \end{array}$

The definitions of the adjoint operators and the analytical formulas of $\left(\partial {\mathit{S}}_{\beta }/\partial \mathit{q}\right)\dot{\mathit{q}}$ , $\left(\partial {\dot{\mathit{S}}}_{\beta }/\partial \mathit{q}\right)\dot{\mathit{q}}$ , and $\left(\partial {\mathit{S}}_{\beta }/\partial \mathit{q}\right)\ddot{\mathit{q}}$ are provided in the Appendixes A and C. ${\mathit{\eta }}_{\beta }^{+}$ , ${\dot{\mathit{\eta }}}_{\beta }^{+}$ are the velocity and acceleration twists of β + 1 expressed in the frame of β. ${\mathcal{N}}_k$ has the same dimensions as ${\mathcal{M}}_k$ $({\mathbb{R}}^{6\times 6})$ , while R _β and ${\mathit{Q}}_{\beta }\in {\mathbb{R}}^{6\times n_{\mathit{dof}}}$ , similar to S _β . Now we proceed to take the sum over k from α + 1 to n _p . Substituting equations (22) and (23) into (21) we get:

$\begin{array}{c}\frac{\partial I{D}_{\alpha }}{\partial \mathit{q}}=\frac{\partial {\mathit{S}}_{\alpha }^T}{\partial \mathit{q}}{\mathcal{F}}_{\alpha }^C\hfill \\ +{\mathit{S}}_{\alpha }^T\left({\mathcal{N}}_{\alpha }^C{\mathit{R}}_{\alpha }^B+{\mathcal{M}}_{\alpha }^C{\mathit{Q}}_{\alpha }^B+{\mathit{U}}_{\alpha }^S+{\mathit{P}}_{\alpha }^S\right)\hfill \end{array}$

(25)

where, ${\mathit{R}}_{\alpha }^B$ , ${\mathit{Q}}_{\alpha }^B$ , ${\mathcal{F}}_{\alpha }^C$ , ${\mathcal{N}}_{\alpha }^C$ , ${\mathcal{M}}_{\alpha }^C$ , ${\mathit{U}}_{\alpha }^S$ , and ${\mathit{P}}_{\alpha }^S$ are computed efficiently using recursive formulas according to:

$\begin{array}{l}\hfill {\mathit{R}}_{\alpha }^B={\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha -1\alpha }}^{-1}({\mathit{R}}_{\alpha -1}+{\mathit{R}}_{\alpha -1}^B)\end{array}$

(26a)

$\begin{array}{l}\hfill {\mathit{Q}}_{\alpha }^B={\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha -1\alpha }}^{-1}({\mathit{Q}}_{\alpha -1}+{\mathit{Q}}_{\alpha -1}^B)\end{array}$

(26b)

$\begin{array}{l}\hfill {\mathcal{F}}_{\alpha }^C={\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha \alpha +1}}^{*}({\mathcal{F}}_{\alpha +1}+{\mathcal{F}}_{\alpha +1}^C)\end{array}$

(26c)

$\begin{array}{l}\hfill {\mathcal{N}}_{\alpha }^C={\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha \alpha +1}}^{*}({\mathcal{N}}_{\alpha +1}+{\mathcal{N}}_{\alpha +1}^C){\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha \alpha +1}}^{-1}\end{array}$

(26d)

$\begin{array}{l}\hfill {\mathcal{M}}_{\alpha }^C={\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha \alpha +1}}^{*}({\mathcal{M}}_{\alpha +1}+{\mathcal{M}}_{\alpha +1}^C){\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha \alpha +1}}^{-1}\end{array}$

(26e)

$\begin{array}{l}\hfill {\mathit{U}}_{\alpha }^S={\mathcal{N}}_{\alpha }^C{\mathit{R}}_{\alpha }+{\mathcal{M}}_{\alpha }^C{\mathit{Q}}_{\alpha }+{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha \alpha +1}}^{*}{\mathit{U}}_{\alpha +1}^S\end{array}$

(26f)

$\begin{array}{l}\hfill {\mathit{P}}_{\alpha }^S={\overline{\mathrm{a}\mathrm{d}}}_{{\mathcal{F}}_{\alpha }^C}^{*}{\mathit{S}}_{\alpha }+{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha \alpha +1}}^{*}{\mathit{P}}_{\alpha +1}^S\end{array}$

(26g)

Finally, we obtain:

$\frac{\partial ID}{\partial \mathit{q}}=\sum _{\alpha =1}^{n_p-1}\frac{\partial I{D}_{\alpha }}{\partial \mathit{q}}$

(27)

Note that ${\mathit{R}}_{\alpha }^B$ is the partial derivative of η _α : $\partial {\mathit{\eta }}_{\alpha }/\partial \mathit{q}={\mathit{R}}_{\alpha }^B$ , while ${\mathit{Q}}_{\alpha }^B$ (excluding the gravity term) is associated with the partial derivative of ${\dot{\mathit{\eta }}}_{\alpha }$ : $\partial {\dot{\mathit{\eta }}}_{\alpha }/\partial \mathit{q}={\mathit{Q}}_{\alpha }^{*B}-{\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_{\alpha }}{\mathit{R}}_{\alpha }^B$ . Quantities with the superscript B are kinematic quantities determined during the forward pass, while those with superscripts C or S are evaluated during the backward pass. Similar to ${\mathcal{F}}_{\alpha }^C$ , ${\mathcal{N}}_{\alpha }^C$ and ${\mathcal{M}}_{\alpha }^C$ are the resultant ${\mathcal{N}}_k$ and ${\mathcal{M}}_k$ acting on all points k > α, transformed to the point of α. All quantities with superscript B and S has a dimension of ${\mathbb{R}}^{6\times n_{\mathit{dof}}}$ . Hence, the S _α projection in equation (25) returns quantities with dimension ${\mathbb{R}}^{ndof\times n_{\mathit{dof}}}$ . The same rules apply for analytical derivatives with respect to $\dot{\mathit{q}}$ and $\ddot{\mathit{q}}$ , discussed in the next sections.

4.2. Partial derivative of ID with respect to generalized velocities

The partial derivative of ID _α wrt $\dot{\mathit{q}}$ is given by

$\frac{\partial I{D}_{\alpha }}{\partial \dot{\mathit{q}}}={\mathit{S}}_{\alpha }^T\sum _{k=\alpha +1}^{n_p}{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha k}}^{*}\frac{\partial {\mathcal{F}}_k}{\partial \dot{\mathit{q}}}$

(28)

Only the Coriolis force is a function of $\dot{\mathit{q}}$ . We derive (details in Appendix D)

$\begin{array}{c}\hfill \frac{\partial {\mathcal{F}}_k}{\partial \dot{\mathit{q}}}={\mathcal{N}}_k\sum _{\beta =1}^{k-1}{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\beta k}}^{-1}{\mathit{S}}_{\beta }+{\mathcal{M}}_k\sum _{\beta =1}^{k-1}{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\beta k}}^{-1}{\mathit{Y}}_{\beta }\end{array}$

(29)

where

${\mathit{Y}}_{\beta }={\mathit{R}}_{\beta }+{\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_{\beta }}{\mathit{S}}_{\beta }+{\dot{\mathit{S}}}_{\beta }$

(30)

Substituting equations (29) into (28) and taking the sum over k from α + 1 to n _p , we get

$\frac{\partial I{D}_{\alpha }}{\partial \dot{\mathit{q}}}={\mathit{S}}_{\alpha }^T\left({\mathcal{N}}_{\alpha }^C{\mathit{S}}_{\alpha }^B+{\mathcal{M}}_{\alpha }^C{\mathit{Y}}_{\alpha }^B+{\mathit{V}}_{\alpha }^S\right)$

(31)

where, ${\mathit{S}}_{\alpha }^B$ , ${\mathit{Y}}_{\alpha }^B$ , and ${\mathit{V}}_{\alpha }^S$ are recursively computed according to:

$\begin{array}{l}\hfill {\mathit{S}}_{\alpha }^B={\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha -1\alpha }}^{-1}({\mathit{S}}_{\alpha -1}+{\mathit{S}}_{\alpha -1}^B)\end{array}$

(32a)

$\begin{array}{l}\hfill {\mathit{Y}}_{\alpha }^B={\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha -1\alpha }}^{-1}({\mathit{Y}}_{\alpha -1}+{\mathit{Y}}_{\alpha -1}^B)\end{array}$

(32b)

$\begin{array}{l}\hfill {\mathit{V}}_{\alpha }^S={\mathcal{N}}_{\alpha }^C{\mathit{S}}_{\alpha }+{\mathcal{M}}_{\alpha }^C{\mathit{Y}}_{\alpha }+{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha \alpha +1}}^{*}{\mathit{V}}_{\alpha +1}^S\end{array}$

(32c)

Note that ${\mathit{S}}_{\alpha }^B$ is the partial derivative of η _α : $\partial {\mathit{\eta }}_{\alpha }/\partial \dot{\mathit{q}}={\mathit{S}}_{\alpha }^B$ , while ${\mathit{Y}}_{\alpha }^B$ is associated with the partial derivative of ${\dot{\mathit{\eta }}}_{\alpha }$ : $\partial {\dot{\mathit{\eta }}}_{\alpha }/\partial \dot{\mathit{q}}={\mathit{Y}}_{\alpha }^B-{\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_{\alpha }}{\mathit{S}}_{\alpha }^B$ . We have, ${\mathit{J}}_{\alpha }={\mathit{S}}_{\alpha }^B$ and ${\dot{\mathit{J}}}_{\alpha }={\mathit{Y}}_{\alpha }^B-{\mathit{R}}_{\alpha }^B$ .

Summing equation (31) over α from 1 to n _p − 1, we have:

$\frac{\partial ID}{\partial \dot{\mathit{q}}}=\sum _{\alpha =1}^{n_p-1}\frac{\partial I{D}_{\alpha }}{\partial \dot{\mathit{q}}}$

(33)

4.3. Partial derivative of ID with respect to generalized accelerations

The partial derivative of ID _α with respect to $\ddot{\mathit{q}}$ is given by:

$\frac{\partial I{D}_{\alpha }}{\partial \ddot{\mathit{q}}}={\mathit{S}}_{\alpha }^T\sum _{k=\alpha +1}^{n_p}{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha k}}^{*}\frac{\partial {\mathcal{F}}_k}{\partial \ddot{\mathit{q}}}$

(34)

Only the inertial force is a function of $\ddot{\mathit{q}}$ . From equation (20), we get

$\frac{\partial {\mathcal{F}}_k}{\partial \ddot{\mathit{q}}}={\mathcal{M}}_k\sum _{\beta =1}^{k-1}{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\beta k}}^{-1}{\mathit{S}}_{\beta }$

(35)

Substituting this into equation (34), we get

$\frac{\partial I{D}_{\alpha }}{\partial \ddot{\mathit{q}}}={\mathit{S}}_{\alpha }^T\left({\mathcal{M}}_{\alpha }^C{\mathit{S}}_{\alpha }^B+{\mathit{W}}_{\alpha }^S\right)$

(36)

where, ${\mathit{W}}_{\alpha }^S$ is computed recursively according to

${\mathit{W}}_{\alpha }^S={\mathcal{M}}_{\alpha }^C{\mathit{S}}_{\alpha }+{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha \alpha +1}}^{*}{\mathit{W}}_{\alpha +1}^S$

(37)

Finally summing over all α, we get

$\frac{\partial ID}{\partial \ddot{\mathit{q}}}=\sum _{\alpha =1}^{n_p-1}\frac{\partial I{D}_{\alpha }}{\partial \ddot{\mathit{q}}}=\mathit{M}(\mathit{q})$

(38)

Note that equation (38), incidentally, represents a novel way of computing the generalized mass matrix of a soft robot.

4.4. Partial derivatives of internal forces

The internal forces include the elastic, damping, and actuation forces. It is given by

$\mathit{\tau }=\mathit{B}\mathit{u}-\mathit{K}\mathit{q}-\mathit{D}\dot{\mathit{q}}$

(39)

where $\mathit{D}\in {\mathbb{R}}^{n_{\mathit{dof}}\times n_{\mathit{dof}}}$ is the generalized damping matrix, $\mathit{K}\in {\mathbb{R}}^{n_{\mathit{dof}}\times n_{\mathit{dof}}}$ is the generalized stiffness matrix, $\mathit{B}(\mathit{q})\in {\mathbb{R}}^{n_{\mathit{dof}}\times n_a}$ (n _a being the total number of actuators) is the generalized actuation matrix.

The actuation matrix ( B ) for tendon-like actuators was derived in Renda et al. (2020; 2024), while a simple Hooke-like linear elastic and damping model is used for computing K and D . The expressions for B , K , and D can be found in the Appendix D.

The partial derivative of τ with respect to q and $\dot{\mathit{q}}$ are given by:

$\begin{array}{l}\hfill \frac{\partial \mathit{\tau }}{\partial \mathit{q}}=\frac{\partial \mathit{B}}{\partial \mathit{q}}\mathit{u}-\mathit{K}\end{array}$

(40a)

$\begin{array}{l}\hfill \frac{\partial \mathit{\tau }}{\partial \dot{\mathit{q}}}=-\mathit{D}\end{array}$

(40b)

For tendon-like actuators, the partial derivative of Bu with respect to q is provided in the Appendix D.

4.5. Cable-driven soft manipulator

Based on the results so far, we set up the first simulation example. We simulate a cable-driven soft manipulator (CDM) shown in Figure 4(a). The manipulator radius varies linearly from base to tip according to r(X) = r _b + (r _t − r _b )X/L. The material properties used in the simulation are as follows: Young’s modulus of 1 MPa, Poisson’s ratio of 0.5, density of 1000 kg/m³, and a material damping coefficient of 10⁴ Pa ⋅s. The soft manipulator has five tendon-like actuators, as shown in Figure 4(a). Their routing, local coordinates for a given X, are provided in Table 2. Gravity acts in the negative z-axis direction. We used a Legendre polynomial basis with 4th-order angular strains (torsion about x, bending about y and z) and 2nd-order linear strains (elongation about x, shear in y and z). Hence, the total DoF of the system is 24. For the numerical integration, we used 5 Gauss quadrature points. Including the computational points at the base and the tip, the total number of points n _p = 7.

Figure 4.

(a) Schematics of cable-driven soft manipulator with cable routing. L = 50 cm, base radius r _b = 3 cm, and tip radius r _t = 1.5 cm. RGB arrows indicate x, y, and z axes of the global frame. (b) Actuation input used for the simulation.

Table 2.

Cable Routing for the Cable-Driven Manipulator. r(X) is the Manipulator Radius and r _t = r(L).

Cable number	y-Coordinate	z-Coordinate
1	0	r(X)
2	$-\left(\sqrt{3}/2\right)r\left(X\right)$	−(1/2)r(X)
3	$\left(\sqrt{3}/2\right)r\left(X\right)$	−(1/2)r(X)
4	$r_{t}sin\left(\left(4\pi /L\right)X\right)$	$r_{t}cos\left(\left(4\pi /L\right)X\right)$
5	$r_{t}sin\left(\left(4\pi /L\right)X\right)$	$-r_{t}cos\left(\left(4\pi /L\right)X\right)$

The manipulator is actuated by arbitrary time-varying inputs for 10 s, as shown in Figure 4(b). These inputs were selected to assess the robustness of the approach to different types of actuation. Specifically, the inputs for cables 1 and 3 are continuous sinusoidal functions, the input for cable 2 varies linearly, and the inputs for cables 4 and 5 consist of discontinuous step functions. We used ode15s of MATLAB as the ODE integrator, as it demonstrated the fastest computational speed for this simulation. The $\dot{\mathit{x}}$ for the ODE is computed using equation (4), while its Jacobian is computed using equations (10b), (11), (13) and analytical derivatives equations (27), (33), (38), (40a) and (40b). To validate and compare the impact of analytical derivatives, the ode15s solver was executed both without (method 1) and with (method 2) their inclusion. For the Newmark-β scheme (method 3), a time step h of 0.002 s is used in this example. The time step h for all examples in this paper was chosen as the largest value that ensures convergence of the solution while maintaining a positional mismatch at the millimeter level throughout the entire integration period. Throughout all simulations, the default tolerances in MATLAB were used for ode15s and fsolve: “RelTol” of 10⁻³, and “AbsTol” of 10⁻⁶. The PC specifications for all computations presented here are as follows: 13th Gen Intel(R) Core(TM) i9-13900HX, 2.20 GHz, 64.0 GB RAM. The MATLAB version used is R2024a.

Figure 5 shows the simulation results for three cases. The dynamic response is very close for all three methods. Figure 5(a) plots the states of the robot and the tip trajectory. The tip position mismatch computed using ‖Δ r _tip ‖ is plotted in Figure 5(b). The maximum error is less than 30 μm between methods 1 and 2, while it is less than 1 mm between methods 2 and 3. Similarly, the state mismatch between the first two methods is on the order of 10⁻⁴, while the mismatch between the second and third methods is higher, as shown in Figure 5(c). The following metric is used to calculate the mismatch between the states of the body, obtained from two different methods, at a given time instant:

$e_{\mathit{x}}=\frac{\Vert \mathrm{\Delta }\mathit{x}\Vert }{\Vert \text{avg}(\mathit{x})\Vert }$

(41)

Figure 5.

Dynamic response of the CDM: (a) Snapshots of the manipulator at different times and the tip trajectory with and without using analytical derivatives (AD) of FD. (b) The mismatch between the tip positions ( r _tip ). (c) State mismatch. Mismatch between numerical and analytical derivatives of FD (d) with respect to q and (e) with respect to $\dot{\mathit{q}}$

Without analytical derivatives, ode15s took 5.40 s, whereas, with analytical derivatives, it was completed in 1.25 s. This indicates that using analytical derivatives makes the computation more than 4 times faster. Simulation using the Newmark-β method was completed in 24.43 s, which can be explained by the fact that this scheme has a fixed time-step in contrast to ode15s whose time-step is adaptive.

The analytical derivatives of the forward dynamics (FD) are validated as follows. Using the finite difference method, we calculated the numerical derivatives of $FD(\mathit{q},\dot{\mathit{q}},\mathit{u})$ with respect to q and $\dot{\mathit{q}}$ . The i^th column of the numerical Jacobians is computed using the forward finite difference method:

${\left(\frac{\partial FD}{\partial q_i}\right)}_{\text{num}}=\frac{FD(\mathit{q}+ϵ{\mathit{n}}_i,\dot{\mathit{q}},\mathit{u})-FD(\mathit{q},\dot{\mathit{q}},\mathit{u})}{ϵ}$

(42a)

${\left(\frac{\partial FD}{\partial \dot{q_i}}\right)}_{\text{num}}=\frac{FD(\mathit{q},\dot{\mathit{q}}+ϵ{\mathit{n}}_i,\mathit{u})-FD(\mathit{q},\dot{\mathit{q}},\mathit{u})}{ϵ}$

(42b)

where n _i is the unit vector in the direction of the i-th component, and ϵ is a perturbation value set to 10⁻⁶. We used the forward finite difference method as it requires the least function evaluations to compute numerical derivatives, though this may result in lower accuracy compared to other methods.

Using a similar metric like equation (41), we computed the mismatch between analytical and numerical Jacobian of FD with respect to q and $\dot{\mathit{q}}$ at every time-step.

$e_{\frac{\partial FD}{\partial \mathit{q}}}=\frac{‖{\left(\frac{\partial FD}{\partial \mathit{q}}\right)}_{\text{ana}}-{\left(\frac{\partial FD}{\partial \mathit{q}}\right)}_{\text{num}}‖}{‖\text{avg}\left(\frac{\partial FD}{\partial \mathit{q}}\right)‖}$

(43)

A similar equation is used for $\partial FD/\partial \dot{\mathit{q}}$ . Equation (43) provides a quantitative assessment of the accuracy of the analytically computed Jacobians. The results are plotted in Figure 5(d) and (e). Numerical values on the order of 10⁻⁶ and 10⁻⁸ indicate that their differences are insignificant. Computing the Jacobians numerically took, on average, 10.5 ms, while the analytical Jacobian required only 1.3 ms, making the analytical method more than 8 times faster.

For the static equilibrium simulation, the residue is computed using equation (17a), while the analytical Jacobian is obtained from equations (17b), (27), and (40a) with $\dot{\mathit{q}}=\mathbf{0}$ and $\ddot{\mathit{q}}=\mathbf{0}$ . We performed 1000 static equilibrium simulations on the same system. In each simulation, we used random values of cable tensions in the range of 0 to 100 N. Figure 6(a) illustrates 10 equilibrium shapes from these 1000 simulations. Both methods (with and without the analytical Jacobian) yielded identical results. The mismatch in tip positions and static equilibrium solutions (e _q ) using the two approaches are shown in Figure 6(b) and (c). Without analytical derivatives, the static simulations took an average of 28.2 ms, whereas, with analytical derivatives, it took 3.3 ms, making the computation more than 8 times faster.

Figure 6.

Static simulation results: (a) 10 arbitrary equilibrium shapes. (b) The mismatch between tip positions. (c) The mismatch static equilibrium solutions.

5. Extension to hybrid multi-body system

A hybrid multi-body system may incorporate rigid joints (up to 6 DoF), rigid links, soft links, branched chains, and closed-chain joints (Figure 7). The forward pass of RNEA goes through the whole multi-body (i = 1 to N) and the backward pass from i = N to 1. We propose the following rules to simplify the analysis of hybrid multi-body systems:

Figure 7.

Schematics of a generic hybrid multi-body system featuring soft and rigid links, rigid joints, branched chains, and closed-chain (CC) joints.

(1) Each link is defined by a rigid joint and a body (rigid or soft). Accordingly, every rigid link has two computational points: one at X = 0 and another at X = 1 of the joint. These points are defined in the body’s reference frame, typically located at the center of mass (CM), where the inertia matrix is computed. Soft links have n _p + 1 computational points: at X = 0 of the rigid joint and n _p along the soft body. X = 1 of the joint coincides with X = 0 of the soft body.

(2) For a rigid body, the inertial and gravitational forces act on the computational frame on the joint at X = 1. At X = 0 of a rigid joint, ${\mathcal{F}}_k=\mathbf{0}$ , ${\mathcal{M}}_k=\mathbf{0}$ and ${\mathcal{N}}_k=\mathbf{0}$ .

(3) Let g _f denote a fixed transformation between two adjacent frames (Figure 7). The recursive transformation rule, as outlined in equations (26), (32), and (37), applies to both the forward and backward transformations between these two frames when g _α−1α or g _αα+1 is replaced by g _f .

(4) In a branched chain system, the kinematic term (•) ^B denotes the contribution of all joints from the global frame to the computational point α, following a serial chain. Meanwhile, (•) ^C and (•) ^S encompass all the children’s joints, including all branches ahead of point α. The dashed arrows in Figure 7 illustrate these points.

(5) Computation of (•) ^B in the forward pass follows the same rule of the geometric Jacobian in equation (2b):

$\begin{array}{c}{(•)}_{i,\alpha }^B={\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha \alpha -1}}\left({(•)}_{i,\alpha -1}^B\left.+\left[{\mathbf{0}}_{6\times n_i^{-}}{(•)}_{i,\alpha -1}{\mathbf{0}}_{6\times n_i^{+}}\right]\right)\right.\\ \end{array}$

(44)

Hence, while the joint quantities, S _α , R _α , Q _α , and Y _α have dimensions ${\mathbb{R}}^{6\times {n_{\mathit{dof}}}_i}$ , their (•) ^B counterparts have dimensions ${\mathbb{R}}^{6\times n_{\mathit{dof}}}$ .

(6) Computation of (•) ^S in the backward pass follows a similar rule:

${(•)}_{i,\alpha }^S=\left[{\mathbf{0}}_{6\times n_i^{-}}{(•)}_{i,\alpha }{\mathbf{0}}_{6\times n_i^{+}}\right]+{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\alpha \alpha +1}}^{*}{(•)}_{i,\alpha +1}^S$

(45)

Hence, (•) ^S also has dimensions ${\mathbb{R}}^{6\times n_{\mathit{dof}}}$ .

(7) For the i^th Cosserat rod, equations (25), (31), and (36) are generalized for the multi-body as follows:

$\begin{array}{ll}\hfill & \frac{\partial I{D}_{i,\alpha }}{\partial \mathit{q}}=\left[{\mathbf{0}}_{{n_{\mathit{dof}}}_i\times n_i^{-}}\frac{\partial {\mathit{S}}_{i,\alpha }^T}{\partial {\mathit{q}}_i}{\mathcal{F}}_{i,\alpha }^C{\mathbf{0}}_{{n_{\mathit{dof}}}_i\times n_i^{+}}\right]\hfill \end{array}$

(46a)

$\begin{array}{ll}\hfill & +{\mathit{S}}_{i,\alpha }^T\left({\mathcal{N}}_{i,\alpha }^C{\mathit{R}}_{i,\alpha }^B+{\mathcal{M}}_{i,\alpha }^C{\mathit{Q}}_{i,\alpha }^B+{\mathit{U}}_{i,\alpha }^S+{\mathit{P}}_{i,\alpha }^S\right)\hfill \end{array}$

$\begin{array}{ll}\hfill & \frac{\partial I{D}_{i,\alpha }}{\partial \dot{\mathit{q}}}={\mathit{S}}_{i,\alpha }^T\left({\mathcal{N}}_{i,\alpha }^C{\mathit{S}}_{i,\alpha }^B+{\mathcal{M}}_{i,\alpha }^C{\mathit{Y}}_{i,\alpha }^B+{\mathit{V}}_{i,\alpha }^S\right)\hfill \end{array}$

(46b)

$\begin{array}{ll}\hfill & \frac{\partial I{D}_{i,\alpha }}{\partial \ddot{\mathit{q}}}={\mathit{S}}_{i,\alpha }^T\left({\mathcal{M}}_{i,\alpha }^C{\mathit{S}}_{i,\alpha }^B+{\mathit{W}}_{i,\alpha }^S\right)\hfill \end{array}$

(46c)

(8) Finally, the partial derivatives of ID and τ are computed by vertically concatenating row-blocks of each Cosserat rod according to:

$\begin{array}{ll}\hfill & {\frac{\partial ID}{\partial \mathit{q}}=‖}_{i=1}^N\sum _{\alpha =1}^{n_p-1}\frac{\partial I{D}_{i,\alpha }}{\partial \mathit{q}}\hfill \end{array}$

(47a)

$\begin{array}{ll}\hfill & {\frac{\partial ID}{\partial \dot{\mathit{q}}}=‖}_{i=1}^N\sum _{\alpha =1}^{n_p-1}\frac{\partial I{D}_{i,\alpha }}{\partial \dot{\mathit{q}}}\hfill \end{array}$

(47b)

$\begin{array}{ll}\hfill & {\frac{\partial ID}{\partial \ddot{\mathit{q}}}=‖}_{i=1}^N\sum _{\alpha =1}^{n_p-1}\frac{\partial I{D}_{i,\alpha }}{\partial \ddot{\mathit{q}}}=\mathit{M}\hfill \end{array}$

(47c)

$\begin{array}{ll}\hfill & {\frac{\partial \mathit{\tau }}{\partial \mathit{q}}=‖}_{i=1}^N\frac{\partial {\mathit{\tau }}_i}{\partial \mathit{q}}\hfill \end{array}$

(47d)

$\begin{array}{ll}\hfill & {\frac{\partial \mathit{\tau }}{\partial \dot{\mathit{q}}}=‖}_{i=1}^N\frac{\partial {\mathit{\tau }}_i}{\partial \dot{\mathit{q}}}\hfill \end{array}$

(47e)

where ‖ is the vertical concatenation operator. In the backward pass from N to 1, the corresponding derivatives of each Cosserat rod (as block matrices of size ${\mathbb{R}}^{{n_{\mathit{dof}}}_i\times n_{\mathit{dof}}}$ ) are concatenated from bottom to top.

Analytical derivatives of arbitrary hybrid multi-body systems can be evaluated following these rules. In addition to these, the dynamics of a multi-body system is often subject to equality constraints that can be expressed as $\mathit{c}(\mathit{q},\dot{\mathit{q}})=\mathbf{0}$ . These constraints introduce an equal number of unknown Lagrange multipliers to enforce the equality condition. As a result, the system’s ODE is transformed into a DAE. We explore two of the most commonly used types of constraints in robotic systems: joint coordinate controlled rigid joints and closed-chain systems.

5.1. Joint coordinate controlled rigid joints

Rigid joints can be actuated by specifying joint wrench u or providing joint coordinates. The latter can be expressed as an equality constraint, with the Lagrange multiplier being u . Similar to the soft body actuation equation (39), the generalized actuation force of a rigid joint is given by Bu . The expression of the generalized actuation matrix ( B ) and the derivative of Bu with respect to q are provided in the Appendix D. For 1 DoF joints, B is independent of q , and therefore, its derivative is 0.

The computation of FD and its derivatives remains the same if the joint is actuated by specifying u . However, if some of the joints are actuated by specifying joint coordinates, the FD computation is different. One approach is to split $\ddot{\mathit{q}}$ into ${\ddot{\mathit{q}}}_u$ and ${\ddot{\mathit{q}}}_k$ and accordingly, u into u _k and u _u . The subscripts k and u stand for known and unknown quantities. Note that ${\ddot{\mathit{q}}}_k$ is the second time derivative of the specified joint coordinates q _k = f (t), making it an index-1 DAE. Bringing the unknown terms to the LHS and known terms to the RHS, the generalized dynamics equation can be rewritten as

$[{\mathit{M}}_u-{\mathit{B}}_u]\left[\begin{array}{c}\hfill {\ddot{\mathit{q}}}_u\hfill \\ \hfill {\mathit{u}}_u\hfill \end{array}\right]=[{\mathit{B}}_k-{\mathit{M}}_k]\left[\begin{array}{c}\hfill {\mathit{u}}_k\hfill \\ \hfill {\ddot{\mathit{q}}}_k\hfill \end{array}\right]+(•)$

(48)

where, the subscripts of M and B indicate corresponding columns of known and unknown quantities.

Equation (48) is used to solve ${[{\ddot{\mathit{q}}}_u^T{\mathit{u}}_u^T]}^T$ and FD is found by combining ${\ddot{\mathit{q}}}_u$ and ${\ddot{\mathit{q}}}_k$ . To ensure numerical stability, during each iteration of the FD, the known joint angles and their derivatives are replaced with the specified q _k and its time derivative ${\dot{\mathit{q}}}_k$ . The derivative of equation (48) with respect to q gives

$\left[\begin{array}{c}\hfill \frac{\partial {\ddot{\mathit{q}}}_u}{\partial {\mathit{q}}_u}\hfill \\ \hfill \frac{\partial {\mathit{u}}_u}{\partial {\mathit{q}}_u}\hfill \end{array}\right]={[{\mathit{M}}_u-{\mathit{B}}_u]}^{-1}\left(\frac{\partial \mathit{\tau }}{\partial {\mathit{q}}_u}-\frac{\partial ID}{\partial {\mathit{q}}_u}\right)$

(49)

Finally, ∂FD/∂ q is found by combining $\partial {\ddot{\mathit{q}}}_u/\partial {\mathit{q}}_u$ with $\partial {\ddot{\mathit{q}}}_k/\partial \mathit{q}={\mathbf{0}}^{n_k\times n_{\mathit{dof}}}$ and $\partial {\ddot{\mathit{q}}}_u/\partial {\mathit{q}}_k={\mathbf{0}}^{n_u\times n_k}$ . The partial derivative of FD with respect to $\dot{\mathit{q}}$ is computed similarly.

The same problem can be solved using index-3 DAE formulation using the Newmark-β scheme. In this case, we have q _k = f (t) and the residue equation (15) and Jacobian equation (16) are modified to include the unknown u _u,n+1 and partial derivative with respect to q _u,n+1 and u _u,n+1.

${\mathit{R}}_{\mathit{dyn}}=\mathit{\tau }({\mathit{q}}_{n+1},{\mathit{u}}_{n+1})-ID({\mathit{q}}_{n+1})$

(50a)

${\mathit{J}}_{\mathit{dyn}}=\left[\begin{array}{cc}\hfill \frac{\partial \mathit{\tau }}{\partial {\mathit{q}}_u}-\frac{\partial ID}{\partial {\mathit{q}}_u}\hfill & \hfill {\mathit{B}}_u\hfill \end{array}\right]$

(50b)

The statics problem involves finding the unknown generalized coordinates and joint forces ( q _u and u _u ). It can be seen that the residue and Jacobian of the static equilibrium are identical to equations (50a) and (50b) with $\dot{\mathit{q}}$ and $\ddot{\mathit{q}}$ set to zero.

5.1.1. Hybrid serial robot

We modeled a hybrid serial robot by combining a rigid serial robotic arm with a soft manipulator, as shown in Figure 8(a). The robot consists of 7 revolute joints that are actuated by arbitrary joint angles according to Figure 8(b). The actuation inputs for the joint coordinate controlled rigid joints were selected as C²-continuous functions since the joint angle, velocity, and acceleration are required for the computation of dynamics using index-1 DAE. Additionally, all joints start with an initial value of zero at t = 0. For the chosen arbitrary input, joints 1, 3, and 6 follow sinusoidal functions, joints 2 and 4 are governed by fourth-order polynomials, and joints 5 remain fixed throughout the simulation. The soft link has a fixed joint, a length of 50 cm, and a radius linearly varying from 1.5 cm to 1 cm. The mechanical properties of the soft body are identical to that of the first example. The soft body is modeled as a Kirchhoff rod (no shear deformation) with a fourth-order strain basis. Hence, in total, the robot has 27 DoF.

Figure 8.

(a) Hybrid serial robot consisting of 7 rigid links and 1 soft link with a fixed joint. The rigid links shown in red have revolute joints rotating about their local x-axis, while those in blue rotate about the y-axis. (b) Arbitrary joint angles as a function of time.

The dynamic response of the robot is computed over 10 seconds using three methods: ode15s without providing analytical derivatives, ode15s with analytical derivatives, and the Newmark-β method. For the Newmark-β approach, a time step of 0.001 s was used, as the solution failed to converge for larger time steps. Snapshots of the simulation results for the first 5 seconds are displayed in Figure 9(a). Without analytical derivatives, the ode15s took 7.08 s, while with analytical derivatives, it was completed in 2.49 s, making it approximately 3 times faster. While for our custom Newmark-β code, the simulation time was 3 min and 51 s. Mismatch metrics, similar to those in the first example, are shown in Figure 9(b)–(e). The position and state mismatch between the first two methods is in the order of 0.1 mm and 10⁻³, respectively. The differences between the numerical and analytical derivatives of FD indicate that their discrepancies are insignificant. Numerically computing the Jacobians took an average of 32.1 ms, whereas the analytical Jacobian required only 2.5 ms, making the analytical approach nearly 13 times faster. The mismatch metrics in Figure 9 appear noisier than those in Figure 5. This could be due to the more rapid dynamic motion of the serial robot compared to the cable-driven manipulator.

Figure 9.

Dynamic response of the serial robot: (a) Superimposed snapshots of the robot at different times and the tip trajectory with (blue) and without (red) using analytical derivatives. (b) Tip position mismatch. (c) State mismatch. The mismatch between numerical and analytical derivatives of FD (d) with respect to q and (e) with respect to $\dot{\mathit{q}}$ .

Static equilibrium simulation of the robot involves solving q _u of the soft body and u _u of the rigid joints. Similar to the first example, we performed 1000 static equilibrium simulations on the same system where we used random values of joint angles in the range of −π/4 to π/4. Figure 10(a) illustrates 5 equilibrium shapes from these 1000 simulations. Both methods (with and without the analytical Jacobian) yielded very close results. The mismatch in tip positions and static equilibrium solutions using the two approaches are shown in Figure 10(b) and (c). Without analytical derivatives, the static simulations took an average of 49.9 ms, whereas, with analytical derivatives, it took 6.3 ms, making the computation approximately 8 times faster.

Figure 10.

Static simulation results: (a) Five arbitrary equilibrium shapes, (b) the mismatch between tip positions, and (c) The mismatch static equilibrium solutions ( q _u and u _u ).

5.2. Closed-chain systems

Robots can have a closed-chain (CC) structure, as shown in Figure 7. Closed-chain systems can be modeled using kinematic constraints ( e ( q ) = 0) expressed in the Pfaffian form (velocity level): $\mathit{A}(\mathit{q})\dot{\mathit{q}}=\mathbf{0}$ . Accordingly, their generalized dynamic equation is given by Armanini et al. (2022a):

$\begin{array}{ll}\hfill & \mathit{M}\ddot{\mathit{q}}=\mathit{\tau }+\mathit{F}+{\mathit{A}}^T\mathit{\lambda }\hfill \end{array}$

(51a)

$\begin{array}{ll}\hfill & \mathit{A}\ddot{\mathit{q}}+\dot{\mathit{A}}\dot{\mathit{q}}+\frac{2}{T_B}\mathit{A}\dot{\mathit{q}}+\frac{1}{T_B^2}\mathit{e}=0\hfill \end{array}$

(51b)

where $\mathit{\lambda }={[{\mathit{\lambda }}_1^T,{\mathit{\lambda }}_2^T\dots {\mathit{\lambda }}_{n_{CL}}^T]}^T$ represents the unknown constraint wrenches, where n _CL is the number of closed-chain joints and T _B is the Baumgart stabilization constant. A , $\dot{\mathit{A}}$ , and e are given by:

$\begin{array}{ll}\hfill & {\mathit{A}=‖}_{k=1}^{n_{CL}}{\mathbf{\Phi }}_{\perp k}^T({\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\mathit{kBA}}}{\mathit{J}}_{kA}-{\mathit{J}}_{kB})\hfill \end{array}$

(52a)

$\begin{array}{ll}\hfill & {\dot{\mathit{A}}=‖}_{k=1}^{n_{CL}}{\mathbf{\Phi }}_{\perp k}^T({\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\mathit{kBA}}}{\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_{\mathit{kBA}}}{\mathit{J}}_{kA}+{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\mathit{kBA}}}{\dot{\mathit{J}}}_{kA}-{\dot{\mathit{J}}}_{kB})\hfill \end{array}$

(52b)

$\begin{array}{ll}\hfill & {\mathit{e}=‖}_{k=1}^{n_{CL}}{\mathbf{\Phi }}_{\perp k}^T{\left(\text{log}({\mathit{g}}_{\mathit{kBA}})\right)}^{\vee }\hfill \end{array}$

(52c)

where ${\mathit{g}}_{\mathit{kBA}}={\mathit{g}}_{kB}^{-1}{\mathit{g}}_{kA}$ , Φ_⊥k is the basis for the constrained degrees of freedom and J _kA and J _kB are geometric Jacobians at points A and B where the closed-loop joint is defined.

Combining equations (51a) and (51b) we get:

${\mathit{M}}_A\left[\begin{array}{c}\hfill \ddot{\mathit{q}}\hfill \\ \hfill \mathit{\lambda }\hfill \end{array}\right]=\left[\begin{array}{c}\hfill \mathit{\tau }+\mathit{F}\hfill \\ \hfill -\dot{\mathit{A}}\dot{\mathit{q}}-\frac{2}{T_B}\mathit{A}\dot{\mathit{q}}-\frac{1}{T_B^2}\mathit{e}\hfill \end{array}\right]$

(53)

where,

${\mathit{M}}_A=\left[\begin{array}{cc}\hfill \mathit{M}\hfill & \hfill -{\mathit{A}}^T\hfill \\ \hfill \mathit{A}\hfill & \hfill 0\hfill \end{array}\right]$

(54)

Equation (53) is used to solve for $\ddot{\mathit{q}}$ and λ , and λ is ignored for the time integration. The partial derivative of equation (53) with respect to q gives

$\left[\begin{array}{c}\hfill \frac{\partial FD}{\partial \mathit{q}}\hfill \\ \hfill \frac{\partial \mathit{\lambda }}{\partial \mathit{q}}\hfill \end{array}\right]={\mathit{M}}_A^{-1}\left[\begin{array}{c}\hfill \frac{\partial \mathit{\tau }}{\partial \mathit{q}}-\frac{\partial ID(\mathit{q},\dot{\mathit{q}},\ddot{\mathit{q}},\mathit{\lambda })}{\partial \mathit{q}}\hfill \\ \hfill -\frac{\partial \mathit{A}}{\partial \mathit{q}}\ddot{\mathit{q}}-\frac{\partial \dot{\mathit{A}}}{\partial \mathit{q}}\dot{\mathit{q}}-\frac{2}{T_B}\frac{\partial \mathit{A}}{\partial \mathit{q}}\dot{\mathit{q}}-\frac{1}{T_B^2}\frac{\partial \mathit{e}}{\partial \mathit{q}}\hfill \end{array}\right]$

(55)

where ID includes the contributions of the constraint force: a local wrench of ${\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\mathit{kBA}}}^{-\ast }{\mathbf{\Phi }}_{\perp k}{\mathit{\lambda }}_k$ on point A and −Φ_⊥k λ _k on point B of the closed-loop joint k. ID treats λ as an independent variable.

Similarly, the derivative of equation (53) with respect to $\dot{\mathit{q}}$ gives:

$\left[\begin{array}{c}\hfill \frac{\partial FD}{\partial \dot{\mathit{q}}}\hfill \\ \hfill \frac{\partial \mathit{\lambda }}{\partial \dot{\mathit{q}}}\hfill \end{array}\right]={\mathit{M}}_A^{-1}\left[\begin{array}{c}\hfill \frac{\partial \mathit{\tau }}{\partial \dot{\mathit{q}}}-\frac{\partial ID(\mathit{q},\dot{\mathit{q}},\ddot{\mathit{q}},\mathit{\lambda })}{\partial \dot{\mathit{q}}}\hfill \\ \hfill -\frac{\partial \dot{\mathit{A}}}{\partial \dot{\mathit{q}}}\dot{\mathit{q}}-\dot{\mathit{A}}-\frac{2}{T_B}\mathit{A}\hfill \end{array}\right]$

(56)

For the index-3 DAE formulation of the problem, we have additional unknowns λ and additional equations given by the kinematic constraint e ( q ) = 0. Hence, the residue equation (15) and Jacobian equation (16) of the Newmark-β scheme are modified as follows.

${\mathit{R}}_{\mathit{dyn}}({\mathit{q}}_{n+1},{\mathit{\lambda }}_{n+1})=\left[\begin{array}{c}\hfill \mathit{\tau }({\mathit{q}}_{n+1})-ID({\mathit{q}}_{n+1},{\mathit{\lambda }}_{n+1})\hfill \\ \hfill \mathit{e}({\mathit{q}}_{n+1})\hfill \end{array}\right]$

(57a)

$\begin{array}{cc}\hfill {\mathit{J}}_{\mathit{dyn}}({\mathit{q}}_{n+1})=& \left[\begin{array}{cc}\hfill \frac{\partial \mathit{\tau }}{\partial \mathit{q}}-\frac{\partial ID}{\partial \mathit{q}}\hfill & \hfill {\mathit{A}}^T\hfill \\ \hfill \frac{\partial \mathit{e}}{\partial \mathit{q}}\hfill & \hfill \mathbf{0}\hfill \end{array}\right]\hfill \end{array}$

(57b)

The static equilibrium simulation involves solving q and λ where, $\dot{\mathit{q}}$ and $\ddot{\mathit{q}}$ are zero. In this case, the residue and Jacobian are identical to equations (57a) and (57b). Analytical derivatives of all quantities in equations (55), (56), and (57b) can be found in the Appendix D.

5.2.1. Fin-ray finger

Figure 11 shows a fin-ray finger, a multi-body system with 17 soft links, revolute joints, a prismatic joint, and 6 closed-chain joints. The finger is actuated by a displacement of the prismatic joint, as shown in Figure 11(b). The ribs (horizontal links) are connected to the sidewalls via revolute joints. Each link has a rectangular cross-section with a width of 1 mm. The sidewalls have a height of 2.5 cm and a length of 3 cm, while the ribs have a height of 1 cm. All soft links are modeled using linear bending, constant elongation, and shear strains, resulting in a planar multi-body with 74 DoF. The material used is Acrylonitrile-Butadiene-Styrene (ABS), with a Young’s modulus of 2 GPa and a Poisson’s ratio of 0.35, commonly used in 3D printing. Five of the six closed-chain joints are revolute, and the top joint is fixed. The problem includes both the joint coordinate constraint and the closed-chain constraint.

Figure 11.

(a) A fin-ray finger with 17 soft links and 6 closed-chain joints. The robot is actuated by a prismatic joint along the global z-axis at its base. (b) Displacement input of the prismatic joint in the range −2.5 to 2.5 cm.

In this example, when the analytical Jacobian was not provided, the dynamics simulation using ode15s was practically stuck at 0.04 s. Other MATLAB ODE integrators, such as ode45 and ode113, progressed extremely slowly, completing only $\sim 0.1$ s in 12 hours. However, when ode15s was supplied with the analytical derivatives of FD, the simulation was completed in 16 min and 34 s. Meanwhile, the Newmark-β integrator (with h = 0.01 s) completed the same simulation in just 1 min and 9 s, making it the fastest DAE solver for this example. Figure 12(a) illustrates the dynamic response of the finger, which exhibits periodic behavior as expected. Figure 12(b) and (c) compare the tip position and the robot states, respectively. The tip position has a mismatch in the order of mm, while the state mismatch is higher. Using a lower value of time step can reduce this mismatch at the cost of increased computational effort. Figure 12(d) and (e) indicates that the discrepancies between the numerical and analytical derivatives of FD are insignificant. The numerical derivatives took an average of 498 ms to compute, while the analytical derivatives took 23.74 ms, making the analytical approach more than 20 times faster.

Figure 12.

Dynamic response of the fin-ray finger: (a) Snapshots of the finger at different times with the trails of the tip position, (b) average values of mismatch between tip positions, and (c) state mismatch. The mismatch between numerical and analytical derivatives of FD (d) with respect to q and (e) with respect to $\dot{\mathit{q}}$ .

6. Examples of common external forces

In this section, we derive the analytical derivatives of various external loads to which robots are commonly subjected: point wrenches, contact forces, and hydrodynamic forces.

6.1. Point wrench

The point wrench can be expressed in the local frame (as a follower load) or the global frame. In the former case, the partial derivative of ${\mathcal{F}}_k$ with respect to q is 0. In our formulation, an external wrench expressed in the global frame ${\mathcal{F}}_{gk}$ must be transformed into the local frame for the projection into the space of generalized coordinates. The transformed wrench in the local frame is given by:

${\mathcal{F}}_k={\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\theta k}}^{-*}{\mathcal{F}}_{gk}$

(58)

where, g _θk is the rotational component of g _k .

The derivative of this term with respect to q is given by

$\frac{\partial {\mathcal{F}}_k}{\partial \mathit{q}}=-{\overline{\mathrm{a}\mathrm{d}}}_{{\mathcal{F}}_k}^{*}{\mathit{I}}_{\theta }{\mathit{S}}_k^B$

(59)

where, I _θ = diag ([1 1 1 0 0 0]).

Accordingly, the partial derivative of ID with respect to q includes an additional term at every computational point k:

$\frac{\partial I{D}_k}{\partial \mathit{q}}=(•)+{\mathit{S}}_k^T\left({\frac{\partial {\mathcal{F}}_k}{\partial \mathit{q}}}^C\right)$

(60)

where, $\partial {\mathcal{F}}_{\mathit{k}}/{\partial \mathit{q}}^C$ is computed similarly to ${\mathcal{F}}_k^C$ :

${\frac{\partial {\mathcal{F}}_k}{\partial \mathit{q}}}^C={\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{kk+1}}^{*}\left(\frac{\partial {\mathcal{F}}_{k+1}}{\partial \mathit{q}}+{\frac{\partial {\mathcal{F}}_{k+1}}{\partial \mathit{q}}}^C\right)$

(61)

6.1.1. Hybrid parallel robot

Figure 13(a) shows an example of a hybrid parallel robot with three soft pillars and a rigid platform. The properties of the soft body are kept identical to those of CDM. The soft body is modeled as a cuboid with dimensions 1.5 cm × 3 cm × 15 cm. The top rigid platform is an equilateral triangle with a side length of 20 cm and a height of 1 cm. The soft pillars are connected to the platform via spherical joints and are parameterized using cubic angular strains and first-order linear strains. Hence, the total DoF of the robot is 63. The dynamic simulation is solved using FD computed from equation (53), with analytic Jacobians according to equations (55) and (56). The platform is subjected to an external point wrench (force and moment) in the global frame according to Figure 13(b), and the robot’s dynamic response is calculated over 10 seconds.

Figure 13.

(a) Hybrid parallel robot consisting of three soft pillars, a rigid platform with three spherical joints, and two closed-chain revolute joints subject to a point wrench (force f and moment m ). (b) Applied external wrenches.

Figure 14(a) shows snapshots of the dynamic response. In this example, when the analytical Jacobian was not provided, the simulation using ode15s advanced at an impractically slow pace. Consequently, we switched to MATLAB’s ode45 and ode113 (method 1) integrators. The simulation took 1 hr 46 mins with ode45 and 1 hr 15 mins with ode113. In contrast, ode15s with the analytical Jacobian completed the simulation in just 2.46 s, making it about 1800 times faster than ode113. For the Newmark-β scheme, the computational time was 17.62 s for a time step of 0.01 s. Figure 14(b) and (c) demonstrate how closely the dynamic simulation results of all three methods align. The position and state mismatch between the first two methods is in the order of 10⁻⁵ m and 10⁻⁴, respectively. The difference between the analytical and numerical derivatives of FD, displayed in Figure 14(d) and (e), validates the analytical derivatives. Numerically computing the derivatives took an average of 82.9 ms, whereas the analytical derivatives required only 5.1 ms, making the analytical approach nearly 16 times faster. Thus, it is not just the speed of computation; its accuracy also contributed to the significant improvement in the overall computational efficiency of dynamic simulations.

Figure 14.

Dynamic response of the hybrid parallel robot: (a) Snapshots of the robot at different times with the trails of the corner positions of the rigid platform. Solid and dotted arrows indicate the applied point forces and moments, respectively. (b) Average values of mismatch between corner positions. (c) State mismatch. The mismatch between numerical and analytical derivatives of FD (d) with respect to q and (e) with respect to $\dot{\mathit{q}}$ .

For the static equilibrium simulations, the system is subjected to 1000 randomly applied point wrenches: forces in the range −0.5 to 0.5 N and moments in the range −0.05 to 0.05 Nm. The equilibrium values of q and λ are computed by solving equation (57a). When the analytical derivatives equation (57b) were not provided, the simulation took an average of 166.39 ms. In contrast, the simulation was completed in just 8.97 ms with the analytical derivatives, making it approximately 18 times faster. Figure 15(a) presents four static equilibrium shapes from the simulation. The average corner position mismatch is depicted in Figure 15(b), while the solution mismatch, including the equilibrium values of q and λ , is shown in Figure 15(c). These figures suggest that, with and without analytical derivatives, the solutions obtained are identical, except in cases where multiple solutions are present.

Figure 15.

Static simulation results of the hybrid parallel robot: (a) Four arbitrary equilibrium shapes. (b) The mismatch between tip positions. (c) The mismatch static equilibrium solutions ( q and λ ). Vertical lines in (b) and (c) indicate cases of multiple static solutions obtained from both methods.

6.2. Contact force

Accurate modeling of contact mechanics is paramount in robotics and has been addressed through various models and algorithms (Lidec et al., 2024). Here, we examine a simplified internal contact scenario where a cable-driven soft manipulator, such as the one depicted in Figure 4, is actuated inside a hollow cylinder, with the cylinder’s centerline aligned to the global z-axis. The following assumptions are made to simplify the contact force:

(1) To compute the contact force, the manipulator is discretized into spheres at each computational point (n _p = 12 in this example). The radius of each sphere corresponds to the manipulator’s radius at that point k.

(2) Only normal forces are considered; tangential (frictional) forces are assumed to be zero. Hence, the contact force does not cause any local moments.

(3) Penetration into the cylinder is allowed, and the contact force is assumed to be a function of the penetration δ.

(4) The direction of the contact force is normal to the surface of the cylindrical wall, taking the form ${[n_x,n_y,0]}^T$ .

To model the contact force, we used the Hertz model (Flores, 2022), a two-parameter nonlinear contact model given by:

${\mathcal{F}}_{gk}=\left\{\begin{array}{cc}\left(\begin{array}{c}\hfill \mathbf{0}\hfill \\ \hfill k_c{\delta }_k^p{\mathit{u}}_{\perp k}\hfill \end{array}\right)\hfill & \text{if\hspace{0.17em}}\delta >0\hfill \\ \mathbf{0}\hfill & \text{otherwise}\hfill \end{array}\right.$

(62)

where k _c represents the contact stiffness coefficient and p is the force exponent, typically dependent on the material properties and geometry of the contact. u _⊥ is the unit normal from the center of the sphere to the wall of the cylinder. The penetration δ _k is given by

${\delta }_k=\Vert {\mathit{n}}_{\perp k}\Vert +r_k-r_{\mathit{cyl}}$

(63)

where n _⊥k = C ₁ r _gk , C ₁ = diag ([1 1 0]) and r _gk is the vector from the global frame to point k. Note that u _⊥k = n _⊥k/‖ n _⊥k‖. Since ${\mathcal{F}}_{gk}$ is in the global frame, we need to transform it into the local frame according to equation (58). The derivative of the local force with respect to q is given by

$\frac{\partial {\mathcal{F}}_k}{\partial \mathit{q}}=-{\overline{\mathrm{a}\mathrm{d}}}_{{\mathcal{F}}_k}^{*}{\mathit{I}}_{\theta }{\mathit{S}}_k^B+{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\theta k}}^{-1}\frac{\partial {\mathcal{F}}_{gk}}{\partial \mathit{q}}$

(64)

The partial derivative of the contact force in the global frame is given by

$\frac{\partial {\mathcal{F}}_{gk}}{\partial \mathit{q}}=\left(\begin{array}{cc}\hfill \mathbf{0}\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill {\mathit{k}}^{*}{\mathit{C}}_1\hfill \end{array}\right){\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\theta k}}{\mathit{S}}_k^B$

(65)

where k * is given by

${\mathit{k}}^{*}=k_{c}p{\delta }_k^{p-1}{\mathit{u}}_{\perp k}{\mathit{u}}_{\perp k}^T+\frac{k_c{\delta }_k^p}{\Vert {\mathit{n}}_{\perp k}\Vert }({\mathit{I}}_3-{\mathit{u}}_{\perp k}{\mathit{u}}_{\perp k}^T)$

(66)

Similar to the case of point force, the derivative of the local force equation (64) is projected backward, and their contribution is accounted into the partial derivative of ID. Note that the penetration condition of the contact force equation (62) also applies to its derivative. Readers interested in detailed derivation may refer to Appendix D.

6.2.1. Contact dynamics

For the dynamic simulation, we used the same manipulator and actuation inputs as shown in Figure 4. We used an inner wall radius of r _cyl = 15 cm, k _c = 10⁵ N/m, and p = 1.5. The simulation results are displayed in Figure 16(a). Simulation using ode15s without providing analytical derivatives was completed in 30.61 s, while with analytical Jacobian, it was completed in 7.80 s, making it nearly 4 times faster. Integration using the Newmark-β approach with a time step of 0.002 s was completed in 46.64 s. The validation between the methods in terms of tip position and robot state are shown in Figure 16(b) and (c), respectively. Between methods 1 and 2, the tip position mismatch is less than 40 μm and the state mismatch is in the order of 10⁻³. Figure 16(d) and (e) compare the derivatives of FD obtained using numerical and analytical methods. While the discrepancies are generally small, the relatively larger mismatches in the partial derivatives of FD with respect to q can be attributed to the penetration condition. The average time to compute the numerical derivatives was 23.7 ms, while for the analytical derivatives, it was 2.3 ms, making the analytical method approximately 10 times faster.

Figure 16.

Contact simulation results: (a) Snapshots of dynamics of CDM inside a hollow cylinder with tip trajectories. Inset showing a top view. (b) Mismatch between tip positions. (c) State mismatch. Mismatch between numerical and analytical derivatives of FD: (d) with respect to q and (e) with respect to $\dot{\mathit{q}}$ .

6.3. Hydrodynamic force

Several hybrid soft robots are designed for underwater exploration. In addition to gravity, they are subject to external forces due to buoyancy, drag, lift, and added mass (fluid displacement). Buoyancy acts as an acceleration opposing gravity, while the added mass force can be simplified as additional inertia that the body experiences. To model the effect of buoyancy, we can use the modified acceleration ${\mathit{a}}_G^{\prime }=(1-{\rho }_w/{\rho }_b){\mathit{a}}_G$ , where ρ _w is the density of the surrounding water and ρ _b is the density of the robot’s body. To tackle the hydrodynamic forces along rods, we follow Boyer et al. (2006) and use a model that combines a simplified version of the reactive theory of Lighthill (1970), with the resistive empirical model of Morison et al. (1950). In this model, the rod inertia matrices are replaced by the modified inertia matrix ${\mathcal{M}}_k^{\prime }={\mathcal{M}}_k+{\mathcal{M}}_{Ak}$ , where ${\mathcal{M}}_{Ak}$ represents the added inertia matrix on the kth computational point on the robot. The effects of viscosity are modeled by a field of drag-lift forces applied along the flagellum, and given by Armanini et al. (2021):

${\mathcal{F}}_{Dk}={\mathcal{D}}_k\Vert {\mathit{v}}_k\Vert {\mathit{\eta }}_k$

(67)

where, ${\mathcal{D}}_k$ is the drag-lift matrix and $\Vert {\mathit{v}}_k\Vert =\sqrt{{\mathit{\eta }}_k^T{\mathit{I}}_v{\mathit{\eta }}_k}$ , where I _v = diag ([0 0 0 1 1 1]). Hence, we have

$\frac{\partial {\mathcal{F}}_{Dk}}{\partial \mathit{q}}={\mathcal{D}}_k^{*}\frac{\partial {\mathit{\eta }}_k}{\partial \mathit{q}}$

(68)

where ${\mathcal{D}}_k^{*}={\mathcal{D}}_k\left(1/\Vert {\mathit{v}}_k\Vert {\mathit{\eta }}_k{\mathit{\eta }}_k^T{\mathit{I}}_v+\Vert {\mathit{v}}_k\Vert {\mathit{I}}_6\right)$ .

Substituting the partial derivative of the velocity twist (Appendix D), we get the derivative of the drag-lift force:

$\frac{\partial {\mathcal{F}}_{Dk}}{\partial \mathit{q}}={\mathcal{D}}_k^{*}\sum _{\beta <k}{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\beta k}}^{-1}{\mathit{R}}_{\beta }$

(69)

Similarly, we can write the partial derivative of the drag-lift force with respect to $\dot{\mathit{q}}$ as follows:

$\frac{\partial {\mathcal{F}}_{Dk}}{\partial \dot{\mathit{q}}}={\mathcal{D}}_k^{*}\sum _{\beta <k}{\mathrm{A}\mathrm{d}}_{{\mathit{g}}_{\beta k}}^{-1}{\mathit{S}}_{\beta }$

(70)

With the form of equations (69) and (70), it is easy to see that the contribution of drag-lift force can be accounted for by including an additional term in equation (24a):

${\mathcal{N}}_k={\overline{\mathrm{a}\mathrm{d}}}_{{\mathcal{M}}_k{\mathit{\eta }}_k}^{*}+{\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_k}^{*}{\mathcal{M}}_k-{\mathcal{M}}_k{\mathrm{a}\mathrm{d}}_{{\mathit{\eta }}_k}+{\mathcal{D}}_k^{*}$

(71)

6.3.1. Underwater flagellated vehicle

Using this, we simulated the dynamics of an underwater (UW) flagellated vehicle. The schematic of the robot is shown in Figure 17. The robot is a hybrid-branched chain system with a mobile (6 DoF) body, two shafts (rigid bodies with revolute joints), six soft bodies (flagella), and six hooks that connect the flagella with the shafts. The three front flagella are 35 cm long, while the rear ones are 50 cm long. Each flagellum has a base radius of 12.5 cm, tapering smoothly to a point at the tips. The material properties of the soft body are kept identical to previous examples. The robot is actuated by joint torques while subject to external hydrodynamic forces. In this example, we assume that all the bodies are neutrally buoyant (ρ _w = ρ _b ). Each flagellum is modeled as an inextensible Kirchhoff rod with a cubic strain field, resulting in 80 DoFs for the robot.

Figure 17.

Schematic of the underwater hybrid mobile robot. The robot is actuated by joint torque and is subject to drag-lift and added mass forces.

We applied a joint torque of 0.25 Nm on the first shaft and −0.25 Nm on the second to cancel out the rotation of the robot body. The dynamics of the robot are computed for 10 s. Similar to the case of the hybrid parallel robot, the simulation using ode15s without providing the Jacobian progressed very slowly. As a result, we used ode45 and ode113 (method 1), obtaining simulation times of 11 min 58 s and 8 min 16 s, respectively. In contrast, ode15s with the analytical Jacobian completed the simulation in just 1.17 s, making it more than 400 times faster. Using the Newmark-β method, the simulation was finished in 60.91s. The dynamic response of the robot is displayed in Figure 18(a). The mismatch metrics comparing the outputs of all three methods are provided in Figure 18(b) and (c). Between methods 1 and 2, the state mismatch is in the order of 10⁻⁴. The validation of the analytical derivatives of FD is provided in Figure 18(d) and (e). The numerical method took 216.4 ms on average, while the average time for the analytical approach was 11.9 ms, making it nearly 18 times faster.

Figure 18.

Dynamic response of the underwater mobile robot: (a) Snapshots of the robot at different times with the trajectory of two flagella. (b) Average values of mismatch between the tips of all six flagella. (c) State mismatch. The mismatch between numerical and analytical derivatives of FD (d) with respect to q and (e) with respect to $\dot{\mathit{q}}$ .

7. Discussions and conclusions

In this work, we used the RNEA algorithm to derive the analytical derivatives of the GVS model for the mechanical analysis of hybrid soft-rigid robots. The “pseudo rigid joint” formulation of the GVS model, facilitated by the Magnus expansion of soft rods, allows for the extension and generalization of analytical derivatives of rigid-body dynamics algorithms (Carpentier and Mansard, 2018; Singh et al., 2022). These derivatives were applied to the dynamic and static simulations of various hybrid multi-body systems. We presented six dynamic and three static simulations, demonstrating significant computational improvements. For dynamic simulations, three integration methods were used: method 1 employed either ode15s or ode113 (when ode15s was slower or stalled), method 2 utilized ode15s with analytical derivatives, and method 3 applied the Newmark-β approach. Method 2 provided exceptional speed-up, particularly in multi-body systems with constraints, where speed-up factors of over 3 orders of magnitude $(>10^3)$ were observed. The Newmark-β approach was observed to be faster for the highly constrained fin-ray finger simulation, while the solution using method 1 was impractically slow for the same case. The results for dynamic simulations summarizing the increase in simulation speed and the mismatch between the analytical and numerical Jacobian of FD are shown in Table 3. The results emphasize the effectiveness of analytical derivatives and the accuracy of its computation. For static simulations, fsolve with analytical Jacobian was at least 8 times faster in all cases due to the improved convergence achieved with the accurate derivative information and efficient recursive implementation.

Table 3.

Summary of Computational Times and the Maximum Jacobian Mismatch for 10 s Dynamic Simulations. Computational Time is Denoted as t _c , and the Speed-Up Factor is S _f . Method 1 Does Not Use Analytical Derivatives, While Method 2 Refers to ode15s With Analytical Derivatives, and Method 3 Represents Neumark-β With Analytical Derivatives and a Fixed Time Step h.

Example	N	DoF	n p	Method 1		Method 2		Method 3		max (e∂FD/∂ q )	max $(e_{\partial FD/\partial \dot{\mathit{q}}})$
				t c	Solver	t c	S f	h (s)	t c
CDM	1	24	7	5.4 s	ode15s	1.25 s	4.32	0.002	24.43 s	2.7 × 10−6	2.4 × 10−8
Serial robot	8	27	22	7.08 s	ode15s	2.49 s	2.84	0.001	3 min 51 s	8.5 × 10−7	6.4 × 10−8
Fin-ray finger	23	74	136	Very slow	N/A	16 min 34 s	Very high	0.01	1 min 9 s	1.2 × 10−5	9.2 × 10−8
Parallel robot	6	63	26	1 hr 15 min	ode113	2.46 s	1829.27	0.01	17.62 s	1.1 × 10−5	6.7 × 10−8
Contact	1	24	12	30.61 s	ode15s	7.8 s	3.92	0.002	46.64 s	8.9 × 10−4	6.7 × 10−6
UW vehicle	19	80	80	8 min 16 s	ode113	1.17 s	424.27	0.01	1 min 9 s	1.1 × 10−6	1.1 × 10−5

A limitation of the ode15s solver in MATLAB is that it does not allow passing $\dot{\mathit{x}}$ to the Jacobian function. Since the partial derivative of FD equation (11) requires $\ddot{\mathit{q}}$ , we need to compute $\ddot{\mathit{q}}$ twice, slowing down the simulation. Adding the capability to directly pass $\dot{\mathit{x}}$ could improve simulation speed. Even though the Newmark-β approach is not a built-in MATLAB function, and our implementation is currently far from optimized, it offers several advantages. Its high stability allows large time steps without sacrificing accuracy, especially in handling DAE index-3 problems, as they are commonly considered in nonlinear structural dynamics of hybrid soft-rigid systems, where this formulation is generally preferred to the index-1 formulation of the Baumgart algorithm. In addition, the Newmark-β scheme works particularly well for highly constrained systems. Moreover, preserving the symplectic structure of mechanical systems while maintaining simulation stability and second-order accuracy, it is ideal for long-duration simulations. Here, we used a fixed time step that resulted in a positional mismatch at the millimeter level compared to method 2, however, developing a variable time-step version of the method could further enhance its efficiency and performance. Similarly, replacing the nominal version of the Newmark scheme with the generalized HHT or α schemes should improve its performance, while requiring only minor modifications to the scheme.

We have implemented the GVS model in a GUI-based MATLAB toolbox called SoRoSim Mathew et al. (2022a). The toolbox allows users from various backgrounds to easily simulate hybrid multi-body systems without requiring deep expertise in underlying algorithms. Implementation of analytical derivatives for the analysis of arbitrary robotic systems is a complex task, particularly for new researchers. To further simplify this process, we updated SoRoSim into a fully differentiable simulator, using the theory presented in this work. The latest version of the toolbox can be obtained from MathWorks or GitHub Mathew et al. (2025b). The readers are encouraged to explore the toolbox and leverage its differential capabilities for various robotic analyses.

The applications of analytical derivatives extend far beyond the algorithms presented for static and dynamic analysis. They are foundational for a wide range of advanced control and optimization techniques. For example, in design optimization, they enable efficient tuning of robot geometry and material properties to achieve desired performance. In trajectory optimization, they provide efficient gradient information for motion planning, improving computational efficiency and accuracy. They are also crucial for Model Predictive Control (MPC), where real-time system predictions are needed to maintain optimal performance. Moreover, their use can significantly improve methods like Differential Dynamic Programming (DDP), especially in handling nonlinear constraints and complex robot configurations. Analytical derivatives facilitate accurate local model linearization of nonlinear system dynamics, which is essential for control design and stability analysis. They also enable sensitivity analysis by capturing system behavior changes due to parameter variations, a key aspect of robust design and uncertainty quantification. Furthermore, they enhance system identification by improving parameter estimation to minimize the sim-to-real gap. In reinforcement learning (RL), analytical derivatives aid policy optimization, accelerating convergence and improving training stability. Analytical derivatives can make several rigid-body algorithms accessible for soft robotic systems, allowing researchers to simulate and control complex hybrid systems using existing algorithms. The techniques presented in this paper offer broad applicability across the robotics field for enhancing the efficiency and precision of computations. We believe that this work will play a pivotal role in addressing challenges in soft and hybrid soft-rigid robots, driving innovation, and enabling real-world applications.

Supplemental Material