1 Introduction

Task space trajectory tracking appears in different contexts [1–4], ranging from fixed-base industrial robots to legged robots or even aerial robots. The goal is to follow a prescribed task velocity \(\mathbf{V}_{t,c}\) respectively acceleration \(\dot{\mathbf{V}}_{t,c}\). For redundant systems, such as legged robotics [5–8], the trajectory tracking problem is often formulated as an optimization problem of the form

$$\begin{aligned} \min _{\ddot{\mathbf{q}},\mathbf{u}} \ &\frac{1}{2} \left \lVert \mathbf{J}_{t}\ddot{\mathbf{q}} + \dot{\mathbf{J}}_{t} \dot{\mathbf{q}} - \dot{\mathbf{V}}_{t,c} \right \rVert ^{2} \end{aligned}$$
(1a)
$$\begin{aligned} &\text{s.t. } \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}( \mathbf{q},\dot{\mathbf{q}}) = \mathbf{B}(\mathbf{q})\mathbf{u} , \end{aligned}$$
(1b)

which has to be solved for the generalized acceleration \(\ddot{\mathbf{q}}\) and input \(\mathbf{u}\) at the current state \((\mathbf{q},\dot{\mathbf{q}})\). Here, \(\mathbf{J}_{t}\) is the task Jacobian relating task velocity and generalized velocity as \(\mathbf{V}_{t}~=~\mathbf{J}_{t} \dot{\mathbf{q}}\), and (1b) are the equations of motion (EoM) of the system with generalized mass matrix \(\mathbf{M}\), input matrix \(\mathbf{B}\) and \(\mathbf{h}\), the sum of all remaining generalized forces. A solution of (1a)–(1b) minimizes the deviation from the desired acceleration \(\dot{\mathbf{V}}_{t,c}\) while satisfying the dynamic constraints (1b). The above formulation accounts for general task tracking problem with possibly changing inputs and outputs. The inputs \(\mathbf{u}\) may consist of actuation torques/forces \(\boldsymbol{\tau}\) as well as interaction torques/forces \(\mathbf{F}_{c}\) so that generally \(\mathbf{u}~=~ \begin{pmatrix} \boldsymbol{\tau}^{\top }& \mathbf{F}_{c}^{\top }\end{pmatrix} ^{\top}\). As an example consider the bipedal humanoid robot in Fig. 1. The task trajectories given through \(\mathbf{J}_{t}\) would be the trajectories of the two feet and the trajectory of its centroidal momentum, which is used to guarantee the stability of the robot [9]. Clearly, for this example, the number of prescribed task velocities and contact forces changes during the motion.

Fig. 1
figure 1

Sketch of a bipedal robot showing the specified inputs. The motor torques \(\tau _{1},\ldots ,\tau _{14}\) are shown in blue, the contact forces \(\mathbf{F}_{1},\ldots ,\mathbf{F}_{8}\) between feet and ground in red (Color figure online)

Full size image

It should be emphasized that the optimization problem must be solved at a given instant of time with given state \((\mathbf{q},\dot{\mathbf{q}})\). Moreover, of interest are the input variables \(\mathbf{u}\) only rather than the acceleration \(\ddot{\mathbf{q}}\). The crucial point is that a numerical optimization scheme repeatedly evaluates (1a)–(1b) for fixed state. That is, all matrices in (1b) and (1a) are constant during the optimization.

The above problem (1a)–(1b) can be transformed to an unconstrained optimization problem [9, 10]. To this end, (1b) is solved for the accelerations, also known as the forward dynamics (FD) solution,

$$ \ddot{\mathbf{q}} = -\mathbf{M}^{-1}\mathbf{h} + \mathbf{M}^{-1} \mathbf{B}\mathbf{u} = \ddot{\mathbf{q}}_{0} + \ddot{\mathbf{q}}_{u} \,, $$
(2)

where \(\ddot{\mathbf{q}}_{0} = -\mathbf{M}^{-1}\mathbf{h}\) is the acceleration that depends on the current system state only, and \(\ddot{\mathbf{q}}_{u}\) are the accelerations due to the inputs, \(\ddot{\mathbf{q}}_{u} = \hat{\mathbf{B}} \mathbf{u}\). The task acceleration is thus

$$\begin{aligned} \dot{\mathbf{V}}_{t} = \mathbf{J}_{t} \ddot{\mathbf{q}} + \dot{\mathbf{J}}_{t} \dot{\mathbf{q}} = \mathbf{J}_{t}( \ddot{\mathbf{q}}_{u} + \ddot{\mathbf{q}}_{0}) + \dot{\mathbf{J}}_{t} \dot{\mathbf{q}} = \mathbf{J}_{t} \hat{\mathbf{B}} \mathbf{u} + \dot{\mathbf{V}}_{t,0}, \end{aligned}$$
(3)

where \(\dot{\mathbf{V}}_{t,0} = \mathbf{J}_{t} \ddot{\mathbf{q}}_{0} + \dot{\mathbf{J}}_{t} \dot{\mathbf{q}}\) are the task accelerations occurring inherently from the system dynamics, which can therefore be assumed constant for the optimization since they are purely state dependent. This simplifies problem (1a) to the unconstrained optimization

$$\begin{aligned} \min _{\mathbf{u}}\ &\frac{1}{2} \left \lVert \mathbf{J}_{t} \hat{\mathbf{B}} \mathbf{u} + \dot{\mathbf{V}}_{t,0} - \dot{\mathbf{V}}_{t,c} \right \rVert ^{2}. \end{aligned}$$
(4)

For solving problem (4), the EoM only need to be evaluated once for every optimization run (in which \((\mathbf{q}, \dot{\mathbf{q}})\) is const.) since except for the optimization variables \(\mathbf{u}\), all other occurring quantities only depend on known values. This means that the FD only needs to be solved once to initialize the optimization. Additionally, the set of optimization variables gets smaller since the accelerations \(\ddot{\mathbf{q}}\) are eliminated.

To efficiently evaluate the FD of multibody systems with many degrees of freedom (DOF), \(O(n)\) methods were developed, such as the articulated-body algorithm (ABA) [11] or earlier algorithms in [12, 13], as well as a similar method presented in [14], which is used in this paper. All these methods are, however, unable to decouple the accelerations from the inputs, which is necessary for solving (4). Consequently, the IWIM \(\hat{\mathbf{B}}\) should be derived separately. Explicitly evaluating and inverting the generalized mass matrix of the system to compute the IWIM as \(\hat{\mathbf{B}} = \mathbf{M}^{-1}\mathbf{B}\) is computationally expensive. The performance could be improved by directly calculating the inverse of the mass matrix with a recursive method [15]. Another way to calculate the IWIM might be found through the inverse operational space inertia matrix (OSIM) \(\boldsymbol{\Lambda}^{-1} = \mathbf{J}_{t}\mathbf{M}^{-1}\mathbf{J}_{t}^{ \top}\), for which there are recursive algorithms [16–20]. By partitioning the input \(\mathbf{u}\) into motor torques \(\boldsymbol{\tau}\) and contact forces \(\mathbf{F}_{c}\), the relationship of \(\boldsymbol{\Lambda}^{-1}\) and \(\hat{\mathbf{B}}\) can be established. Partitioning the input matrix \(\mathbf{B}\) into \(\mathbf{B}~=~ \begin{pmatrix} \mathbf{J}_{a}^{\top }& \mathbf{J}_{t}^{\top}\end{pmatrix} \), with the actuation Jacobian \(\mathbf{J}_{a}\) and the task Jacobian \(\mathbf{J}_{t}\), yields \(\mathbf{J}_{t} \hat{\mathbf{B}} = \begin{bmatrix} \mathbf{J}_{t} \mathbf{M}^{-1} \mathbf{J}_{a}^{\top }& \boldsymbol{\Lambda}^{-1}\end{bmatrix} \). That is, the IWIM premultiplied by the task Jacobian results in a matrix that contains the inverse OSIM. Thus, evaluating the OSIM does not help to compute the IWIM.

This paper presents the implicit inversion method (IIM), an algorithm which enables the calculation of the IWIM for systems with chain or tree topology using intermediate results obtained by solving an \(O(n)\) method. Section 2 covers how the EoM are derived as well as the \(O(n)\) algorithm that is used to solve the FD problem, showing the deficiencies inherent to this method regarding the desired application. Then, Sect. 3 presents the extended unit force method, a method to calculate the IWIM, which augments an algorithm that is originally used to evaluate the inverse OSIM. In Sect. 4 the main contribution of this work, the IIM, is presented. Building on this, simulation results of several models and runtime comparisons are shown in Sect. 5. Finally, Sect. 6 concludes this paper.

2 Subsystem formulation

To efficiently evaluate the FD of more complex systems, recursive algorithms were developed, ideally with a computational complexity of \(O(n)\). In the following, an \(O(n)\)-method [14], which is based on the subsystem formulation introduced in [14, 21], and [22], is presented. This method is equivalent to the ABA described in [11] if subsystems only consist of single bodies.

The subsystem formulation as introduced in [14, 21] is a modeling approach where the overall system is considered an assembly of smaller subsystems that are attached to one another at so-called coupling points, usually the joints connecting them. Such a subsystem can consist of a single rigid or flexible body or a compound of such. In industrial robots for example, it often occurs that a link of the robot arm is combined as a unit with the corresponding motor and gearbox. Therefore, it makes sense to combine these components into a subsystem, and thus to simplify the modeling of the robot. The kinematic topology of a subsystem can be arbitrary, so it can also contain closed kinematic loops.

2.1 Recursive kinematics

Instead of the generalized coordinates \(\dot{\mathbf{q}}\), subsystems use the subsystem-specific velocity coordinates

$$ \boldsymbol{V}_{\!i} = \begin{pmatrix} \mathbf{v}_{o,i} \\ \boldsymbol{\omega}_{o,i} \\ \dot{\mathbf{q}}_{i} \end{pmatrix} \, . $$
(5)

These consist of the guidance velocities \(\mathbf{v}_{o,i}\) and \(\boldsymbol{\omega}_{o,i}\) as well as the internal velocities \(\dot{\mathbf{q}}_{i}\). The internal velocities describe the movement caused by components of the subsystem itself, while the guidance velocities introduce the motion of the system the current subsystem is coupled with. The first two components, \(\mathbf{v}_{o,i}\) and \(\boldsymbol{\omega}_{o,i}\), are the velocities of the preceding subsystem at the coupling point \(i\), expressed in frame \(i\), while the last one usually consists of joint velocities or velocities of flexible bodies as shown in Fig. 2.

Fig. 2
figure 2

Visualization of the subsystem velocity components. Both subsystems \(p\) and \(i\) are instances of the same subsystem, albeit with different parameters

Full size image

The subsystem velocities can be calculated recursively through

$$ \boldsymbol{V}_{\!i} = \mathbf{T}_{ip}\, \boldsymbol{V}_{\!\!p} + \mathbf{F}_{i}\, \dot{\mathbf{q}}_{i}, $$
(6)

with the index \(p\) indicating the parent subsystem of \(i\), the velocity transformation matrix \(\mathbf{T}_{ip}\) from \(p\) to \(i\) and \(\mathbf{F}_{i}\) the matrix that maps the generalized velocities belonging to subsystem \(i\) to the internal velocities. Therefore, \(\mathbf{F}_{i}\) usually is constant, like

$$ \mathbf{F}_{i} = \begin{pmatrix} \mathbf{0} \\ \mathbf{0} \\ \mathbf{I} \end{pmatrix} . $$
(7)

Then the matrix \(\mathbf{T}_{ip}\) transforms the velocities \(\boldsymbol{V}_{\!\!p}\) of the preceding subsystem into the guidance velocity components \(\mathbf{v}_{o,i}\) and \(\boldsymbol{\omega}_{o,i}\), therefore it takes the shape

$$ \mathbf{T}_{ip} = \begin{pmatrix} \mathbf{R}_{ip} & \mathbf{R}_{ip} \tilde{\mathbf{r}}^{\top}_{pi} & \frac{\partial \,\mathbf{v}_{pi}}{\partial \,\dot{\mathbf{q}}_{p}} \\ \mathbf{0} & \mathbf{R}_{ip} & \frac{\partial \,\boldsymbol{\omega}_{pi}}{\partial \,\dot{\mathbf{q}}_{p}} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} \end{pmatrix} \, , $$
(8)

with \(\mathbf{v}_{pi}\) and \(\boldsymbol{\omega}_{pi}\) being the relative velocities between the coupling points \(p\) and \(i\). The upper left 2x2-submatrix is a spatial transform as described in [23], Chap. 2.2. This matrix transforms the point at which the velocities \(\mathbf{v}_{o,p}\) and \(\boldsymbol{\omega}_{o,p}\) are defined from coupling point \(p\) to \(i\) while also changing the frame in which the velocities are described from \(p\) to \(i\) through the rotation matrix \(\mathbf{R}_{ip}\). The (∼)-operator is called the cross-product operator and turns a vector into a so-called cross-product matrix, so that \(\mathbf{a} \times \mathbf{b} = \tilde{\mathbf{a}} \mathbf{b}\). The vector \(\mathbf{r}_{pi}\) is the position vector connecting the coupling points of the subsystems \(p\) and \(i\). The other component of \(\mathbf{T}_{ip}\) is the Jacobian of the relative velocities \(\mathbf{v}_{pi}\) and \(\boldsymbol{\omega}_{pi}\) w.r.t. the internal velocities of \(p\), represented in frame \(i\).

This relationship between preceding and current subsystem velocities is called the kinematic chain [14] due to the chain-like structure of relationship (6). After evaluating this formula for all subsystems, the result can be rewritten as a global relationship

$$ \boldsymbol{V} = \begin{pmatrix} \boldsymbol{V}_{\!1} \\ \boldsymbol{V}_{\!2} \\ \vdots \\ \boldsymbol{V}_{\!N} \end{pmatrix} = \begin{pmatrix} \mathbf{F}_{1} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{T}_{21} \mathbf{F}_{1} & \mathbf{F}_{2} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{T}_{N1} \mathbf{F}_{1} & \mathbf{T}_{N2} \mathbf{F}_{2} & \cdots & \mathbf{F}_{N} \end{pmatrix} \begin{pmatrix} \dot{\mathbf{q}}_{1} \\ \dot{\mathbf{q}}_{2} \\ \vdots \\ \dot{\mathbf{q}}_{N} \end{pmatrix} = \mathbf{F} \dot{\mathbf{q}} $$
(9)

of the set of all subsystem velocities \(\boldsymbol{V}_{\!i}\) and the generalized velocities \(\dot{\mathbf{q}}\). Velocity transformation matrices over multiple subsystems, like \(\mathbf{T}_{N1}\), are obtained by multiplication of neighboring transformation matrices, e.g., \(\mathbf{T}_{ac} = \mathbf{T}_{ab} \mathbf{T}_{bc}\). For systems with a topological tree structure, \(\mathbf{F}\) exhibits a lower triangular block structure, which will be exploited later on in the IIM.

2.2 Subsystem-based EoM

The dynamic equations of the subsystems (see Sect. A) give rise to the EoM of the multibody system

$$ \sum _{i=1}^{N} \left ( \frac{\partial \boldsymbol{V}_{\!i}}{\partial \dot{\mathbf{q}}} \right )^{\top } \left (\overline{\mathbf{M}}_{i} \dot{\boldsymbol{V}}_{\!i} + \overline{\mathbf{h}}_{i}(\boldsymbol{V}_{\!i}, \mathbf{q},\mathbf{u}_{i}) \right ) = \mathbf{0}\ . $$
(10)

The velocity vector \(\boldsymbol{V}_{\!i}\) of subsystem \(i\) can be fully described. The matrix \(\overline{\mathbf{M}}_{i}\) is the mass matrix of subsystem \(i\) and \(\overline{\mathbf{h}}_{i}\) includes all the generalized subsystem forces apart from the inertia reaction forces. The transpose of the Jacobian \(\frac{\partial \boldsymbol{V}_{i}}{\partial \dot{\mathbf{q}}}\), which is the row of \(\mathbf{F}\), can be interpreted as the transformation from subsystem forces into the generalized forces. Further information on the composition of these subsystem matrices is compiled in appendix A.

Finally, the relation between the EoM in subsystem form (10) and the EoM in generalized coordinates (1b) is given obtained by substituting the relationships \(\boldsymbol{V}~=~\mathbf{F}\dot{\mathbf{q}}\) and \(\dot{\boldsymbol{V}}~=~\mathbf{F}\ddot{\mathbf{q}}~+~\dot{\mathbf{F}} \dot{\mathbf{q}}\), which results in

$$\begin{aligned} \mathbf{M}(\mathbf{q}) &= \sum _{i=1}^{N} \left ( \frac{\partial \boldsymbol{V}_{\!i}}{\partial \dot{\mathbf{q}}} \right )^{\top } \overline{\mathbf{M}}_{i} \frac{\partial \boldsymbol{V}_{\!i}}{\partial \dot{\mathbf{q}}} \end{aligned}$$
(11)
$$\begin{aligned} \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) &= \sum _{i=1}^{N} \left ( \frac{\partial \boldsymbol{V}_{\!i}}{\partial \dot{\mathbf{q}}} \right )^{\top } \left [ \overline{\mathbf{M}}_{i} \frac{\mathrm{d}\ }{\mathrm{d}\,t} \left (\frac{\partial \boldsymbol{V}_{\!i}}{\partial \dot{\mathbf{q}}}\right ) \dot{\mathbf{q}} + \overline{\mathbf{h}}_{i} \right ] \end{aligned}$$
(12)
$$\begin{aligned} \mathbf{B}(\mathbf{q}) &= \sum _{i=1}^{N} \left ( \frac{\partial \boldsymbol{V}_{\!i}}{\partial \dot{\mathbf{q}}} \right )^{\top } \overline{\mathbf{B}}_{i} \end{aligned}$$
(13)

with \(\overline{\mathbf{B}}_{i} = - \frac{\partial \, \overline{\mathbf{h}}_{i}}{\partial \,\mathbf{u}}\). These global matrices now allow to solve the FD according to (2), i.e., the mass matrix \(\mathbf{M}\) is inverted to obtain the system accelerations \(\ddot{\mathbf{q}}\).

2.3 Subsystem \(O(n)\) algorithm

The solution of the forward dynamics as shown in (2), however, has a computational complexity of \(O(n^{3})\) due to the necessary inversion of the mass matrix. For more efficient evaluation, various recursive algorithms with complexity \(O(n)\) were proposed, e.g., [11–14]. Due to increased overhead, however, they only become faster than the direct solution of the global form for \(n > 9\).

The \(O(n)\)-methods consist of three recursions. In the first recursion of the subsystem \(O(n)\) algorithm [14], the kinematics is evaluated from the base subsystem outwards as in (6). Then, in the following inward recursion, the articulated-body inertia \(\overline{\mathbf{M}}_{i}^{*}\) and bias forces \(\overline{\mathbf{h}}_{i}^{*}\), as they are named in the ABA, are computed as

$$\begin{aligned} \overline{\mathbf{M}}_{p}^{*} &= \overline{\mathbf{M}}_{p} + \sum _{i \in \{s(p)\}} \mathbf{T}^{*\top}_{ip} \overline{\mathbf{M}}_{i}^{*} \mathbf{T}_{ip} \qquad \qquad \end{aligned}$$
(14)
$$\begin{aligned} \overline{\mathbf{h}}_{p}^{*} &= \overline{\mathbf{h}}_{p} + \sum _{i \in \{s(p)\}} \mathbf{T}^{*\top}_{ip}\left ( \overline{\mathbf{M}}_{i}^{*} \dot{\mathbf{T}}_{ip}\boldsymbol{V}_{\!\!p} + \overline{\mathbf{h}}_{i}^{*} \right ), \end{aligned}$$
(15)
$$\begin{aligned} &\text{with} \quad \mathbf{T}^{*}_{ip} = (\mathbf{I} - \overline{\mathbf{M}}_{i}^{*} \mathbf{S}_{i})^{\top }\mathbf{T}_{ip}, \end{aligned}$$
(16)
$$\begin{aligned} &\quad \mathbf{S}_{i} = \mathbf{F}_{i} \overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top }\quad \text{and} \quad \overline{\mathbf{M}}_{Ri} = \mathbf{F}_{i}^{\top }\overline{\mathbf{M}}_{i}^{*} \mathbf{F}_{i}. \end{aligned}$$
(17)

The set \(\{s(p)\}\) consists of all subsystems directly attached to \(p\), i.e., that have \(p\) as their common parent. Finally, the accelerations are calculated through the final outward recursion

$$\begin{aligned} \ddot{\mathbf{q}}_{i} &= -\overline{\mathbf{M}}_{Ri}^{-1}\mathbf{F}_{i}^{ \top }\left [ \overline{\mathbf{M}}_{i}^{*}(\mathbf{T}_{ip} \dot{\boldsymbol{V}}_{\!\!p} + \dot{\mathbf{T}}_{ip}\boldsymbol{V}_{\!\!p}) + \overline{\mathbf{h}}_{i}^{*} \right ] \end{aligned}$$
(18)
$$\begin{aligned} \dot{\boldsymbol{V}}_{\!i} &= \mathbf{T}_{ip}\dot{\boldsymbol{V}}_{\!\!p} + \dot{\mathbf{T}}_{ip}\boldsymbol{V}_{\!\!p} + \mathbf{F}_{i}\ddot{\mathbf{q}}_{i} + \dot{\mathbf{F}}_{i}\dot{\mathbf{q}}_{i}. \end{aligned}$$
(19)

As can be seen from (17) and (18), the only matrix that needs to be inverted here is \(\overline{\mathbf{M}}_{Ri}\), whose dimension is that of the internal velocities of the respective subsystem. The downside of this method is, however, that it does not allow the splitting of the accelerations as in (2). Therefore, a different method is needed to calculate the IWIM.

3 The extended unit force method (eUFM)

One method to calculate the inverse OSIM that fulfils the criteria mentioned in Sect. 1 is shown in [20]. It builds on the unit force method of [16] but is augmented so that it does not rely on additional assumptions to simplify the system dynamics. Because of this, it will be called the extended unit force method.

To calculate the inverse OSIM \(\boldsymbol{\Lambda}^{-1} = \mathbf{J}_{t}\mathbf{M}^{-1} \mathbf{J}_{t}^{ \top}\), the matrix \(\mathbf{M}^{-1} \mathbf{J}_{t}^{\top}\) is evaluated first since this term can be taken directly from the FD

$$ \ddot{\mathbf{q}} = -\mathbf{M}^{-1}\left (\mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}) - \mathbf{J}_{a}^{\top }\boldsymbol{\tau}\right ) + \mathbf{M}^{-1}\mathbf{J}_{t}^{\top }\mathbf{F}_{c} . $$
(20)

This can also be written as \(\ddot{\mathbf{q}} = \ddot{\mathbf{q}}_{[0]} + \mathbf{M}^{-1} \mathbf{J}_{t}^{\top }\mathbf{F}_{c}\), where \(\ddot{\mathbf{q}}_{[0]}\) are the system accelerations according to the current state and actuator torques, which can be obtained through the \(O(n)\)-method. The matrix \(\mathbf{M}^{-1}\mathbf{J}_{t}^{\top}\) can then be calculated column-wise by choosing a unit vector \(\mathbf{e}_{i}\) (whose \(i\)th entry is 1) as force input. When solving for the accelerations, the result is

$$ \ddot{\mathbf{q}}_{[i]} = \ddot{\mathbf{q}}_{[0]} + \mathbf{M}^{-1} \mathbf{J}_{t}^{\top }\mathbf{e}_{i}, $$
(21)

so \(\ddot{\mathbf{q}}_{[i]}\) are the system accelerations for input \(u_{i} = 1\). Now, the \(i\)th column of \(\mathbf{M}^{-1}\mathbf{J}_{t}^{\top}\) is simply the difference \(\ddot{\mathbf{q}}_{[i]} - \ddot{\mathbf{q}}_{[0]}\). By repeating this process for each element of the contact force vector, the entire matrix is obtained. To get the inverse OSIM now, this result is premultiplied by \(\mathbf{J}_{t}\). This final multiplication can just be left out as mentioned in Sect. 1. Now, the input for the eUFM can be extended to \(\mathbf{u} = \begin{pmatrix} \boldsymbol{\tau}^{\top }& \mathbf{F}_{c}^{\top }\end{pmatrix} ^{\top}\) so that the dynamics can now be expressed as in (2) with \(\mathbf{B} = \begin{bmatrix} \mathbf{J}_{a}^{\top }& \mathbf{J}_{t}^{\top }\end{bmatrix} \). For this case, the eUFM now returns \(\hat{\mathbf{B}} = \begin{bmatrix} \mathbf{M}^{-1}\mathbf{J}_{a}^{\top }& \mathbf{M}^{-1}\mathbf{J}_{t}^{ \top }\end{bmatrix} \) instead of \(\mathbf{M}^{-1}\mathbf{J}_{t}^{\top}\).

The drawback of this method is that the \(O(n)\)-algorithm has to be evaluated \(m+1\) times for \(\mathbf{u} \in \mathbb{R}^{m}\), resulting in a computational complexity of \(O(m\,n)\). This reduces the usability of this method for systems with higher numbers of inputs.

4 The implicit inversion method (IIM)

This section presents the main contribution of this paper, the implicit inversion method. It is a way to evaluate the IWIM without having to explicitly invert the global mass matrix, hence the name. The aim is to derive an algorithm from the subsystem \(O(n)\)-method that is computationally less expensive than those presented in the previous sections. The algorithm should be efficient in particular for systems with multiple inputs while still working with subsystem quantities as far as possible. As mentioned in the introduction, we only consider systems that exhibit a linear relation between inputs and accelerations. Thus the \(\overline{\mathbf{h}}_{i}\) in (10) are linear in the inputs \(\mathbf{u}\) and can be partitioning into

$$ \overline{\mathbf{h}}_{i} = \overline{\mathbf{h}}_{0,i} + \overline{\mathbf{h}}_{u,i} \qquad \text{with} \qquad \overline{\mathbf{h}}_{u,i} = - \overline{\mathbf{B}}_{i}\mathbf{u} \ , $$
(22)

where the matrices \(\overline{\mathbf{B}}_{i}\) determine how the inputs \(\mathbf{u}\) affect the subsystem. The vector \(\overline{\mathbf{h}}_{0,i}\) contains the sum of all subsystem forces that do not depend on the inputs \(\mathbf{u}\). The FD is evaluated using the \(O(n)\)-method presented in Sect. 2.3 so that most of the terms needed for the IIM are already available.

4.1 Basic form of the IIM

4.1.1 Computing the input-based accelerations \(\ddot{\mathbf{q}}_{u}\)

The derivation of the IWIM is done by going through the recursive FD method step by step and picking out all the terms that are related to the inputs. That means that the first step of the \(O(n)\)-method, the evaluation of the kinematic chain can be skipped, as it is not affected by the inputs.

In the second step, the bias forces do depend on the inputs. Substituting (22) in the propagation formula for the bias forces (15) gives

$$\begin{aligned} \begin{aligned} \overline{\mathbf{h}}^{*}_{p} &= \overline{\mathbf{h}}_{0,p}^{*} + \overline{\mathbf{h}}_{u,p}^{*} \\ &\text{with} \quad \overline{\mathbf{h}}_{u,p}^{*} = \overline{\mathbf{h}}_{u,p} + \sum _{i \in \{s(p)\}} \mathbf{T}^{* \top}_{ip} \overline{\mathbf{h}}_{u,i}^{*} = -\overline{\mathbf{B}}_{p} \mathbf{u} + \sum _{i \in \{s(p)\}} \mathbf{T}^{*\top}_{ip} \overline{\mathbf{h}}_{u,i}^{*} \,, \end{aligned} \end{aligned}$$
(23)

a recursive formula for the input-dependent component of the bias forces \(\overline{\mathbf{h}}_{u,i}^{*}\). Rewriting this formula to a global form leads to the relationship

$$ \overline{\mathbf{h}}_{u}^{*}= -{\mathbf{T}^{*}}^{\top } \overline{\mathbf{B}} \mathbf{u} $$
(24)

with

$$\begin{aligned} {\mathbf{T}^{*}} = \begin{pmatrix} \mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{T}_{21}^{*} & \mathbf{I} & \cdots & \mathbf{0} \\ \vdots & \vdots &\ddots & \vdots \\ \mathbf{T}_{N1}^{*} & \mathbf{T}_{N2}^{*} & \cdots &\mathbf{I} \end{pmatrix}. \end{aligned}$$
(25)

The matrices \(\mathbf{T}_{ac}^{*}\) propagate as \(\mathbf{T}_{ac}^{*} = \mathbf{T}_{ab}^{*} \mathbf{T}_{bc}^{*}\).

In the third step of the \(O(n)\)-method, the accelerations are obtained through a final outward recursion (18). Here, the variables \(\dot{\boldsymbol{V}}_{\!i}\) and \(\overline{\mathbf{h}}^{*}_{i}\) depend on \(\mathbf{u}\). To separate accelerations from inputs, a change to global variables is necessary for the input-dependent accelerations \(\ddot{\mathbf{q}}_{u}\). So, inserting (2) into \(\dot{\boldsymbol{V}}_{\!i} = \frac{\partial \boldsymbol{V}_{\!i}}{\partial \dot{\mathbf{q}}} \ddot{\mathbf{q}} + \frac{\mathrm{d}\,}{\mathrm{d}\,t} \left ( \frac{\partial \boldsymbol{V}_{\!i}}{\partial \dot{\mathbf{q}}}\right ) \dot{\mathbf{q}}\) leads to

$$ \dot{\boldsymbol{V}}_{\!i} = \dot{\boldsymbol{V}}_{\!0,i} + \left ( \frac{\partial \boldsymbol{V}_{\!i}}{\partial \dot{\mathbf{q}}}\right ) \ddot{\mathbf{q}}_{u} \qquad \text{with} \qquad \dot{\boldsymbol{V}}_{\!0,i} = \frac{\partial \boldsymbol{V}_{\!i}}{\partial \dot{\mathbf{q}}} \ddot{\mathbf{q}}_{0} + \frac{\mathrm{d}\,}{\mathrm{d}\,t} \left ( \frac{\partial \boldsymbol{V}_{\!i}}{\partial \dot{\mathbf{q}}}\right ) \dot{\mathbf{q}} \ . $$
(26)

That way, all state-dependent accelerations can still be calculated as before while extracting the calculation of \(\ddot{\mathbf{q}}_{u,i}\) as

$$ \ddot{\mathbf{q}}_{u,i} = - \overline{\mathbf{M}}_{Ri}^{-1}\mathbf{F}_{i}^{ \top }\left [ \overline{\mathbf{M}}_{i}^{*}\mathbf{T}_{ip} \left ( \frac{\partial \boldsymbol{V}_{\!\!p}}{\partial \dot{\mathbf{q}}} \right ) \ddot{\mathbf{q}}_{u} + \overline{\mathbf{h}}_{u,i}^{*} \right ] . $$
(27)

To avoid mixing variables on subsystem level and global level, namely \(\ddot{\mathbf{q}}_{u,i}\) and \(\ddot{\mathbf{q}}_{u}\), the whole equation is expanded to the global form

$$ \ddot{\mathbf{q}}_{u} = - \text{diag}\left (\overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top }\overline{\mathbf{M}}_{i}^{*}\right ) ( \mathbf{F} - \text{diag}(\mathbf{F}_{i})) \ddot{\mathbf{q}}_{u} - \text{diag}\left (\overline{\mathbf{M}}_{Ri}^{-1}\mathbf{F}_{i}^{\top} \right )\overline{\mathbf{h}}_{u}^{*}\ . $$
(28)

Substituting (24), it can be further simplified to

$$ \left [ \mathbf{I} + \text{diag}\left (\overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top}\overline{\mathbf{M}}_{i}^{*}\right )(\mathbf{F} - \text{diag}(\mathbf{F}_{i}))\right ]\ddot{\mathbf{q}}_{u} = \text{diag}\left (\overline{\mathbf{M}}_{Ri}^{-1}\mathbf{F}_{i}^{\top} \right ) {\mathbf{T}^{*}}^{\top }\overline{\mathbf{B}} \mathbf{u} \,, $$
(29)

and using (17), the expression \(\mathbf{I} - \text{diag} \left (\overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top}\overline{\mathbf{M}}_{i}^{*}\mathbf{F}_{i} \right )\) vanishes and the equation is reduced to

$$ \left [ \text{diag}\left (\overline{\mathbf{M}}_{Ri}^{-1}\mathbf{F}_{i}^{ \top}\overline{\mathbf{M}}_{i}^{*}\right )\mathbf{F} \right ] \ddot{\mathbf{q}}_{u} = \text{diag}\left (\overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top}\right ){\mathbf{T}^{*}}^{\top } \overline{\mathbf{B}} \mathbf{u} \,. $$
(30)

On the right-hand side of this equation, the product \({\mathbf{T}^{*}}\text{diag}\left (\mathbf{F}_{i}\right )\) takes the shape

$$\begin{aligned} {\mathbf{T}^{*}}\text{diag}\left (\mathbf{F}_{i}\right ) = \begin{pmatrix} \mathbf{F}_{1} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{T}_{21}^{*}\mathbf{F}_{1} & \mathbf{F}_{2} & \cdots & \mathbf{0} \\ \vdots & \vdots &\ddots & \vdots \\ \mathbf{T}_{N1}^{*}\mathbf{F}_{1} & \mathbf{T}_{N2}^{*}\mathbf{F}_{2} & \cdots &\mathbf{F}_{N} \end{pmatrix}, \end{aligned}$$
(31)

which looks like \(\mathbf{F}\), the only difference being that all transformations \(\mathbf{T}_{ip}\) are replaced by \(\mathbf{T}_{ip}^{*}\). Therefore, the product is summarized as \(\mathbf{F}^{*} = {\mathbf{T}^{*}}\text{diag}\left (\mathbf{F}_{i} \right )\). (30) also shows an acceleration transformation matrix \(\boldsymbol{\Gamma} = \text{diag}\left (\overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top}\overline{\mathbf{M}}_{i}^{*}\right )\mathbf{F}\) that needs to be inverted to obtain the input-dependent accelerations

$$\begin{aligned} \ddot{\mathbf{q}}_{u} &= \boldsymbol{\Gamma}^{-1}\text{diag}\left ( \overline{\mathbf{M}}_{Ri}^{-1}\right ){\mathbf{F}^{*}}^{\top } \overline{\mathbf{B}}\mathbf{u} \,. \end{aligned}$$
(32)

4.1.2 Inversion of \({\Gamma}\)

The matrix \(\boldsymbol{\Gamma}\) retains the structure of \(\mathbf{F}\), i.e., it still is a lower triangular block matrix. Besides, thanks to \(\overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top } \overline{\mathbf{M}}_{i}^{*} \mathbf{F}_{i} = \mathbf{I}\), all the block matrices on the diagonal of \(\boldsymbol{\Gamma}\) are identity matrices, leading to a lower unitriangular structure, i.e., a lower triangular matrix with only ones as its diagonal entries. This guarantees that \(\boldsymbol{\Gamma}\) will always be invertible.

Using this special structure, the inversion of \(\boldsymbol{\Gamma}\) can be done through the cascaded inversion of its submatrices as shown in the following example for a 3-link chain.

figure c

The inverse of the resulting block matrix form

$$\begin{aligned} \mathbf{A}_{i} = \begin{pmatrix} \mathbf{A}_{p} & \mathbf{0} \\ \mathbf{C}_{i} & \mathbf{I} \end{pmatrix} \end{aligned}$$
(34)

is

$$\begin{aligned} \mathbf{A}_{i}^{-1} = \begin{pmatrix} \mathbf{A}_{p}^{-1} & \mathbf{0} \\ -\mathbf{C}_{i}\mathbf{A}_{p}^{-1} & \mathbf{I} \end{pmatrix} \end{aligned}$$
(35)

for quadratic diagonal block matrices, which is guaranteed for these matrices. By starting this inversion process with the upper left submatrix \(\mathbf{A}_{1} = \mathbf{I}\) and then iteratively expanding until \(\mathbf{A}_{N} = \boldsymbol{\Gamma}\), the inverse of the original matrix is obtained without any explicit matrix inversion, which motivates to call this approach the implicit inversion method (IIM).

4.2 Subsystem-based inversion of \(\Gamma \)

The terms that appear after following the sequential approach of building the inverse can be further simplified once more, leading to a closed form solution for the inverse. This is shown with the help of an example here, but the result can be generalized to all systems with a topological chain or tree structure.

To show the general structure of the solution, a minimum of three consecutive links is necessary. That is why a 3-link pendulum will serve as an example here. The kinematic Jacobian from (9) for this system has the shape of

$$ \mathbf{F} = \begin{pmatrix} \mathbf{F}_{1} & \mathbf{0} & \mathbf{0} \\ \mathbf{T}_{21}\mathbf{F}_{1} & \mathbf{F}_{2} & \mathbf{0} \\ \mathbf{T}_{31}\mathbf{F}_{1} &\mathbf{T}_{32}\mathbf{F}_{2} & \mathbf{F}_{3} \end{pmatrix} \ , $$
(36)

which, when following the notation of (34) and introducing \(\mathbf{C}_{i} = [\mathbf{c}_{ij}]_{j=1..p}\), leads to

$$ \boldsymbol{\Gamma} = \mathrm{diag}(\overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top }\overline{\mathbf{M}}_{i}^{*})\mathbf{F} = \begin{pmatrix} \mathbf{I} & \mathbf{0} & \mathbf{0} \\ \mathbf{c}_{21} & \mathbf{I} & \mathbf{0} \\ \mathbf{c}_{31} & \mathbf{c}_{32} & \mathbf{I} \end{pmatrix} $$
(37)

with

$$\begin{aligned} \mathbf{c}_{21} &= \overline{\mathbf{M}}_{R2}^{-1} \mathbf{F}_{2}^{ \top }\overline{\mathbf{M}}_{2}^{*} \mathbf{T}_{21} \mathbf{F}_{1} , \\ \mathbf{c}_{31} &= \overline{\mathbf{M}}_{R3}^{-1} \mathbf{F}_{3}^{ \top }\overline{\mathbf{M}}_{3}^{*} \mathbf{T}_{31} \mathbf{F}_{1} , \\ \mathbf{c}_{32} &= \overline{\mathbf{M}}_{R3}^{-1} \mathbf{F}_{3}^{ \top }\overline{\mathbf{M}}_{3}^{*} \mathbf{T}_{32} \mathbf{F}_{2} \ . \end{aligned}$$
(38)

Following (34), we get the submatrices

$$\begin{aligned} \mathbf{A}_{1} = \mathbf{I} ,\qquad \mathbf{A}_{2} = \begin{pmatrix} \mathbf{I} & \mathbf{0} \\ \mathbf{c}_{21} & \mathbf{I} \end{pmatrix} , \qquad \mathbf{A}_{3} = \boldsymbol{\Gamma} \ , \end{aligned}$$
(39)

for which (35) gives the inverses

$$\begin{aligned} \mathbf{A}_{1}^{-1} = \mathbf{I} ,\qquad \mathbf{A}_{2}^{-1} = \begin{pmatrix} \mathbf{I} & \mathbf{0} \\ -\mathbf{c}_{21} & \mathbf{I} \end{pmatrix} , \qquad \mathbf{A}_{3}^{-1} = \begin{pmatrix} \mathbf{I} & \mathbf{0} & \mathbf{0} \\ -\mathbf{c}_{21} & \mathbf{I} & \mathbf{0} \\ -\mathbf{c}_{31} + \mathbf{c}_{32}\mathbf{c}_{21} & -\mathbf{c}_{32} & \mathbf{I} \end{pmatrix} \ . \end{aligned}$$
(40)

In the matrix \(\mathbf{A}_{3}^{-1}\), the lower left element is the only entry that is not explicitly known yet. Inserting (38) into (40), we get

$$\begin{aligned} -\mathbf{c}_{31} + \mathbf{c}_{32}\mathbf{c}_{21} = - \overline{\mathbf{M}}_{R3}^{-1} \mathbf{F}_{3}^{\top } \overline{\mathbf{M}}_{3}^{*}(\mathbf{T}_{31} - \mathbf{T}_{32} \mathbf{F}_{2}\overline{\mathbf{M}}_{R2}^{-1} \mathbf{F}_{2}^{\top } \overline{\mathbf{M}}_{2}^{*} \mathbf{T}_{21} ) \mathbf{F}_{1} \, . \end{aligned}$$
(41)

Using the relationships \(\mathbf{T}_{31} = \mathbf{T}_{32}\mathbf{T}_{21} \) and \(\mathbf{S}_{i}~=~{\mathbf{F}_{i}\overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top}}\), the result can be simplified further to

$$\begin{aligned} -\mathbf{c}_{31} + \mathbf{c}_{32}\mathbf{c}_{21} = - \overline{\mathbf{M}}_{R3}^{-1} \mathbf{F}_{3}^{\top } \overline{\mathbf{M}}_{3}^{*}\mathbf{T}_{32}( \mathbf{I} - \mathbf{S}_{2} \overline{\mathbf{M}}_{2}^{*} ) \mathbf{T}_{21}\mathbf{F}_{1} \ , \end{aligned}$$
(42)

which can be further reduced by using the properties \(\mathbf{S}_{i} = \mathbf{S}_{i}^{\top}\) and \(\overline{\mathbf{M}}_{i}^{*} = {\overline{\mathbf{M}}_{i}^{*}}^{ \top}\). These imply \((\mathbf{I} - \mathbf{S}_{i}\overline{\mathbf{M}}_{i}^{*}) = ( \mathbf{I} - \overline{\mathbf{M}}_{i}^{*}\mathbf{S}_{i})^{\top}\), which allows for the substitution (17). This gives us the final result of

$$\begin{aligned} -\mathbf{c}_{31} + \mathbf{c}_{32}\mathbf{c}_{21} = - \overline{\mathbf{M}}_{R3}^{-1} \mathbf{F}_{3}^{\top } \overline{\mathbf{M}}_{3}^{*}\mathbf{T}_{32} \mathbf{T}_{21}^{*} \mathbf{F}_{1} \end{aligned}$$
(43)

for this entry. With that, the matrix

$$\begin{aligned} \mathbf{A}_{3}^{-1} &= \begin{pmatrix} \mathbf{I} & \mathbf{0} & \mathbf{0} \\ -\overline{\mathbf{M}}_{R2}^{-1} \mathbf{F}_{2}^{\top } \overline{\mathbf{M}}_{2}^{*} \mathbf{T}_{21} \mathbf{F}_{1} & \mathbf{I} & \mathbf{0} \\ -\overline{\mathbf{M}}_{R3}^{-1} \mathbf{F}_{3}^{\top } \overline{\mathbf{M}}_{3}^{*}\mathbf{T}_{32} \mathbf{T}_{21}^{*} \mathbf{F}_{1} & -\overline{\mathbf{M}}_{R3}^{-1} \mathbf{F}_{3}^{ \top }\overline{\mathbf{M}}_{3}^{*} \mathbf{T}_{32} \mathbf{F}_{2} & \mathbf{I} \end{pmatrix} \end{aligned}$$
(44)

is the result of the inversion procedure, which can also be written as

$$\begin{aligned} \mathbf{A}_{3}^{-1} &= \mathbf{I} - \mathrm{diag}( \overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top } \overline{\mathbf{M}}_{i}^{*}\mathbf{T}_{ip}) \hat{\mathbf{F}}\ . \end{aligned}$$
(45)

In (45), only a single variable necessary for the calculation of the inverse is new compared to \(\boldsymbol{\Gamma}\), namely

$$\begin{aligned} \hat{\mathbf{F}} &= \begin{pmatrix} \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{F}_{1} & \mathbf{0} & \mathbf{0} \\ \mathbf{T}_{21}^{*}\mathbf{F}_{1} & \mathbf{F}_{2} & \mathbf{0} \end{pmatrix}\ . \end{aligned}$$
(46)

When comparing \(\hat{\mathbf{F}}\) to (31), it can be seen that each row \(i\) in \(\hat{\mathbf{F}}\) corresponds to the row of its predecessor \(p(i)\) in \(\mathbf{F}^{*}\). With this, the general form of the matrix \(\hat{\mathbf{F}}\) can be derived as

$$ \hat{\mathbf{F}} = \left [\ \mathbf{f}_{i}\ \right ]_{i=1..N} \qquad \text{with} \qquad \mathbf{f}_{i} = \left \{ \textstyle\begin{array}{l@{\quad}c@{\quad}l} \mathbf{0} & & p(i) = 0 \\ \mathbf{F}^{*}_{p} & & p(i) > 0 \end{array}\displaystyle \right . $$
(47)

where \(p(i)\) denotes the predecessor of \(i\) and \(\mathbf{F}^{*}_{p}\) the rows of \(\mathbf{F}^{*}\) belonging to subsystem \(p\). From this, a general rule for the calculation of \(\boldsymbol{\Gamma}^{-1}\) can be deduced, namely

$$ \boldsymbol{\Gamma}^{-1} = \mathbf{I} - \text{diag}(\overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top }\overline{\mathbf{M}}_{i}^{*} \mathbf{T}_{ip}) \hat{\mathbf{F}}. $$
(48)

4.3 Recursive computation of the IIM

Inserting the inverse (48) into the previous formula (32) for the input-based accelerations gives the final result

$$ \ddot{\mathbf{q}}_{u} = [ \mathbf{I} - \text{diag}( \overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top } \overline{\mathbf{M}}_{i}^{*} \mathbf{T}_{ip}) \hat{\mathbf{F}} ]\, \text{diag}(\overline{\mathbf{M}}_{Ri}^{-1} )\mathbf{F}^{* \top} \overline{\mathbf{B}}\mathbf{u} ~. $$
(49)

In this formula, the only inverses remaining are those of \(\overline{\mathbf{M}}_{Ri}\), which were already calculated during the execution of the \(O(n)\)-method, so no separate matrix inversions are needed. This yields \(\ddot{\mathbf{q}} = \ddot{\mathbf{q}}_{0} + \hat{\mathbf{B}} \mathbf{u}\) with \(\ddot{\mathbf{q}}_{0}\) obtained by running the \(O(n)\)-method without any inputs as mentioned before. During this evaluation, all variables that are necessary to calculate the global matrices \(\mathbf{F}^{*}\), therefore \(\hat{\mathbf{F}}\) and subsequently \(\hat{\mathbf{B}}\), are computed in advance. Finally, the global form of the IWIM \(\hat{\mathbf{B}}\) is obtained as

$$ \hat{\mathbf{B}} = [ \mathbf{I} - \text{diag}(\overline{\mathbf{M}}_{Ri}^{-1} \mathbf{F}_{i}^{\top }\overline{\mathbf{M}}_{i}^{*}\mathbf{T}_{ip}) \hat{\mathbf{F}} ]\, \text{diag}(\overline{\mathbf{M}}_{Ri}^{-1} ) \mathbf{F}^{* \top}\overline{\mathbf{B}}. $$
(50)

However, the local nature of the resulting matrices also allows for a recursive reformulation. The fact that \(\hat{\mathbf{F}}\) only consists of components from \(\mathbf{F}^{*}\) can be utilized to further reduce the number of variables. The result is summarized in algorithm 1. The bracketed indices refer to all rows/columns corresponding to the respective subsystem. If two indices are separated by a colon, e.g., \([p:i]\), this refers to all columns from \(p\) to \(i\). That way, the sparsity patterns introduced by the topology of the system through \(\mathbf{F}\) are utilized to accelerate the calculation of the IWIM.

Algorithm 1
figure 3

Pseudo-code for the IIM

Full size image

5 Simulation results

To analyze the performance of the proposed method, the effect of different topologies, numbers of DOF, and inputs is studied through simulation. The runtime of the three methods to calculate the IWIM presented in this paper will be compared here. These are the calculation of the IWIM by inversion of the global mass matrix as mentioned in Sect. 1 ,Footnote 1 the eUFM from Sect. 3, and the IIM as shown in algorithm 1. For that, the algorithms are implemented in Matlab 2023b and run on an Apple M2 Pro processor. The runtime is taken as the average evaluation time for the FD of a model over 1000 runs with randomized values for the states and inputs for each run.

5.1 n-link chain

At first, we look at systems with a pure chain structure, i.e., the model is an n-link pendulum with \(n\) ranging from 1 to 50. For this model, all subsystems were assumed to be a single rigid body with identical parameters. In Fig. 3 (a), only the last link is actuated, i.e., the number of inputs remained constant over the changing number of DOF.

Fig. 3
figure 4

Run time comparisons for the forward dynamics evaluation n-link pendulum models with (a) a single input and (b) the fully actuated models

Full size image

In case only one joint is actuated, all the methods perform very similarly up until \(\sim~18\) DOF. Above that, the global inverse form starts to get noticeably slower, and up to at least 50 DOF, the IIM outperforms the eUFM marginally. From this, it can be seen that the execution of algorithm 1 for models of this scale is faster than the evaluation of the \(O(n)\) method, as that is what the eUFM does in this case. Systems with only a single input are a very rare case, however, so the same n-link pendulum is modeled as a fully actuated system next. The results for these simulations can be seen in Fig. 3 (b). When looking at the results for the eUFM now, it becomes apparent why this method should not be used for systems with multiple inputs, since the resulting computational cost becomes prohibitive. Comparing the IIM with the global inverse form, the runtime for these methods does not change much compared to the single input case, the point where the IIM becomes faster is still at around ≈ 18-19 DOF.

To get a better understanding of the influence the number of inputs has on runtime, the 50-link pendulum model was simulated with a varying number of actuated links. The results of which are shown in Fig. 4 (a). Here, we can see a steep linear increase for the eUFM for an increasing number of inputs as expected. The increase in runtime for both the global inverse and the IIM is also linear, albeit much slower. It is necessary to zoom in to Fig. 4 (b), to get a better picture of the increase for these methods.

Fig. 4
figure 5

Runtime comparison for a 50-link pendulum with a varying number of inputs (a) shows the full results and (b) is zoomed in to show the behavior of the IIM and the global inverse

Full size image

When comparing the extreme cases of Fig. 4, the values of which are listed in Table 1, the increase of computational time per additional input is at \(\approx 1\%\) for the IIM and \(\approx 0.33\%\) for the global inverse form, while for the eUFM, it rises by \(\approx 33\%\) per input.

Table 1 Average runtimes for a 50-link pendulum
Full size table

The low impact the number of inputs has on the IIM can be explained when looking at (50). Here, the inputs only directly affect \(\overline{\mathbf{B}}\), which is very sparsely populated, reducing the computational effort. This reasoning also applies to the solution in global inverse form since it uses \(\overline{\mathbf{B}}\) as well.

5.2 n-element binary tree

Opposed to the n-link chain, which only has a single branch in its topological graph, the next test model is an n-link complete binary tree, which has the highest number of branching points possible. Although this structure makes no sense for practical systems, all systems with topological chain or tree structure are somewhere in between those two models. Therefore, the runtime for an arbitrary system should lie somewhere between these two test models. For that, the same tests were then conducted on models with a complete binary tree for their topological graph to check how the different system topology affects the calculation time.

The runtimes for the solution of the global inverse decrease slightly, while the other methods are not really impacted by the change in topology as can be seen in Table 2. The fact that the runtime for the IIM does not change is because the sparsity pattern left by tree structures is not yet exploited in the current implementation of the IIM.

Table 2 Average runtimes for a 50-link binary tree
Full size table

All in all, it seems as if the system topology does not significantly affect the speed of these methods, just the number of DOF and in the case of the eUFM, the inputs.

5.3 Application example: a bipedal robot

Finally, the three methods are tested on a more practical application, a bipedal robot with 20 DOF and 38 inputs as shown in Fig. 1. The system consists of 14 motor-arm subsystems as mentioned in Sect. 2.2 and of one floating base subsystem for the robot torso. The large number of inputs is due to the number of contact points on the robot. There are 8 contact points defined for the robot with 4 per foot, so there are 24 contact forces and 14 motor torques.

For this system, the FD is calculated for a set of 1000 random states and inputs. The results of these simulations are shown in Table 3, and they show the same trends as were observed in the n-link pendulum and tree. Since this system has a fairly large number of inputs, the eUFM is obviously the slowest of the three methods. For the other two methods, the results also make sense since the system has only 20 DOF, the IIM is only slightly faster than the solution of the inverse when compared with the results obtained for the chain and tree models.

Table 3 Average computational times for the bipedal robot
Full size table

6 Conclusions

This contribution introduces the implicit inversion method (IIM), a recursive method that leverages the subsystem formulation to effectively calculate the inertia-weighted input matrix (IWIM) \(\hat{\mathbf{B}} = \mathbf{M}^{-1}\mathbf{B}\), which is the Jacobian of the forward dynamics (FD) w.r.t. the system inputs. The IIM is used in conjunction with an \(O(n)\)-algorithm to evaluate the state-dependent dynamics and makes use of intermediate results coming from the evaluation of the algorithm. Therefore, the IWIM can be calculated without having knowledge of the mass matrix \(\mathbf{M}\) of the system in its generalized coordinate form. Both the accelerations from the O(n)-algorithm and the IWIM can then be used to solve an optimization of the form (4). Using all the byproducts of these methods additionally contributes to the efficiency of this method.

To determine the performance of this method, runtime comparisons were made between the IIM, an extended form of the unit force method, and the direct calculation of the IWIM through the inverse of the joint-space mass matrix using a Cholesky factorization. The IIM generally performs at least at the level of the other methods, and for systems above \(\sim 18\) DOF and multiple inputs outperforms them increasingly.

However, it still needs to be compared against methods that are more efficient in calculating the inverse mass matrix, such as the one mentioned in [15] to get a better understanding of its standing. In theory, they should perform very similarly, since they are structurally very close to one another.

For a fair comparison though, the method should be reimplemented in a different programming language, e.g., in C++ instead of the current implementation, which is done in Matlab.