Recursive inverse dynamics sensitivity analysis of open-tree-type multibody systems

Abstract

We present a first-order recursive approach to sensitivity analysis based on the application of the direct differentiation method to the inverse Lagrangian dynamics of rigid multibody systems. Our method is simple and efficient and is characterized by the following features. Firstly, it describes the kinematics of multibody systems using branch connectivity graphs and joint-branch connectivity matrices. For most mechanical systems with an open-tree kinematic structure, this method turns out to be more efficient compared to other kinematic descriptions employing joint or link connectivity graphs. Secondly, a recursive sensitivity analysis is presented for a dynamic system with an open-tree kinematic structure and inverse dynamic equations described in terms of the Lagrangian formalism. Thirdly, known approaches to recursive inverse dynamic and sensitivity analyses are modified to include dynamic systems with external forces and torques acting simultaneously at all joints. Finally, the proposed method for sensitivity analysis is easy to implement and computationally efficient. It can be utilized to evaluate the derivatives of the dynamic equations of multibody systems in gradient-based optimization algorithms. It also allows less experienced users to perform sensitivity analyses using the power of high-level programming languages such as MATLAB. To illustrate the method, simulation results for a human body model are discussed. The shortcomings of the method and possible directions for future work are outlined.

1 Introduction

Recent advancements in the analysis of nonlinear dynamics of mechanical systems has been significantly facilitated by the progress in numerical methods, optimal control, nonlinear control, optimal design, system identification, and related fields. Combination of new efficient techniques with a rapidly growing computational power enables researchers to tackle problems deemed unattainable a decade ago. The problem of optimal control of a nonlinear dynamic system such as human body is one such example. Our main motivation in this paper is the implementation of the dynamic and sensitivity analysis of the human body.

Equations of motion for multibody systems can be rough-ly divided into two large classes in terms of their mathematical appearance or form: closed-form and recursive equations [24] .¹ In the closed form, also called a bulk form, dynamic equations of motion are written as a single vector differential equation containing all generalized coordinates. In the recursive form, all terms in the dynamic equations for generalized coordinates of one joint are presented as functions of generalized coordinates of neighboring joint. The main advantage of the closed form is that is it easier to handle and analyze with analytical tools, primarily for the design of controllers in the state space. However, in this case equations of motion become complicated and highly nonlinear, especially as the degrees of freedom (DOF) of the system increase. The attractive advantages of the recursive formulation are its computational efficiency and the ease of coding. The main disadvantage is the “distortion” of the structure of the dynamic model to the form that excludes the possibility of gaining the insight into the dynamics of the system and exploiting it efficiently [24]. However, some recursive formulations do not have this shortcoming and still allow to gain insight into dynamics [33, 48].

Multibody dynamics can be also classified as forward dynamics (kinematic variables are not known) and inverse dynamics (force/torque variables are not known) [12, 26, 51]. However, in applied problems neither positions and velocities nor input forces and torques are known in advance; these variables should be determined during simulation. The problems where both kinematic and dynamic variables are not known fall into the third category; they are formulated as optimization problems and solved using optimization tools. In various engineering fields this class of problems is called trajectory optimization, motion synthesis, motion control, or predictive dynamics [1]. Efficient solution of optimization problems with gradient-based methods depends largely on the gradients of the objective function and constraints. Calculation of gradient matrices and vectors is called sensitivity analysis. In essence, sensitivity analysis is the inverse of the optimization problem. In the optimization problem, the task is to minimize the objective function. This is equivalent to the following problem: given the value for the gradient of the objective function, find the value of the independent variable which satisfies it. In the sensitivity analysis the problem is the opposite: given the value for the independent variable, evaluate the gradient of the objective function. Sensitivity analysis can be performed with respect to states (generalized coordinates, velocities, and accelerations) [1, 57]. Sensitivity analysis with respect to physical system parameters (mass, inertia terms, lengths, etc.) or any other design parameters (such as initial conditions) is termed design sensitivity [27, 53, 55]. Sensitivity analysis is important for problems involving design optimization, parameter estimation, and model correlation. It is also crucial for implicit numerical integration of differential-algebraic systems and kinematic workspace analysis of multibody systems [49].

Sensitivity analysis can be performed using analytical, semi-analytical, and numerical methods [10]. Methods for numerical sensitivity analysis can be further classified as finite difference and automatic differentiation methods. Analytical methods include direct differentiation and adjoint variable method [5]. We draw the reader’s attention to the fact that the above classification is not standardized and accurate, because there are hybrid methods that combine the aforementioned techniques. For example, there might be direct and/or adjoint variable method combined with numerical and/or automatic differentiation [9, 10, 37]. The above classification is provided only for information purposes. The finite difference method is commonly employed for evaluating sensitivity matrices. It is simple and easy to implement, but its accuracy depends on the magnitude of the perturbation and is affected by truncation and rounding errors. Therefore, finite difference method may lead to ill-conditioned problems and unreliable results; its computational costs are prohibitively high for large DOF systems [19]. The automatic differentiation method is based on the application of the symbolic chain rule of differentiation; its accuracy and speed are superior to the finite difference method. Open-source software based on automatic differentiation method and written in C++ (DRAKE [52], MBSlib [56], and RobCoGen [45]) can be used to perform sensitivity analysis of dynamic equations with respect to generalized coordinates, velocities, and parameters. The advantages of the automatic differentiation method include possibilities to simulate forward, inverse, and hybrid dynamics, parameter estimations, and trajectory optimization. The main limitation is the need to couple automatic differentiation software with that employed by the user; this might require additional learning. For instance, it is known that the wrong application of the chain rule of differentiation can lead to erroneous results [16]. Furthermore, computation of implicit derivatives often requires costly time integration. The direct differentiation method is based on a straightforward application of differentiation rules in the equations of motion [17, 49]. Its advantages include easiness of implementation, accuracy, and higher numerical stability [10]; the method is especially efficient when the number of sensitivity variables is small. Examples of the application of the direct differentiation method for sensitivity analysis of multibody mechanical systems can be found in [6, 15, 47]. In the adjoint variable method, the objective function is modified with dynamic equations and constraints and additional adjoint conditions that simplify the computation of the Jacobian of the objective function are obtained [28, 44]. This method is accurate and numerically efficient, but can become complex and lengthy since it requires backward integration in time [7, 46]; it is efficient when the number of objective functions is small. Relevant examples of the application of the adjoint variable method for sensitivity analysis of multibody mechanical systems can be found in [4, 7, 16, 29, 44]. One can also find in the literature approaches based on the combination of existing methods. For instance, a semi-analytical sensitivity analysis replaces symbolic derivatives with respect to design variables in analytic expressions by finite differences combining the simplicity of the finite difference method with the accuracy of the adjoint variable and direct differentiation methods in [46].

Most research on the sensitivity analysis uses a closed-form dynamic formulation where all equations are written using a single vector–matrix form [6, 7, 10, 15,16,17,18, 29, 44, 46, 47, 49]. To the best of our knowledge, only a few papers on the recursive sensitivity analysis of mechanical systems with open-tree topology structure are available, although it is often computationally more efficient compared to “bulk” formulations for large multibody systems [5].² Closed-form expressions for the sensitivity of multibody system dynamics equations using the spatial notation provided in [25] are abstract and their correct implementation requires substantial user effort. A useful framework for generating motion sequences in musculoskeletal systems was designed by combining the computational tools of MATLAB with the modeling capabilities of OpenSim [39]. However, the sensitivity analysis is not presented there; it was performed numerically using forward finite differences. As expected, the authors noted that the fmincon solver was very slow. Sensitivity analysis for simple open-chain multibody structures using inverse recursive Newton-Euler dynamic formulation and spatial notation (as in [21]) was performed in [20]. Using the aggregate force and momentum expressions (changing the order in which terms are computed), linear time analytical first derivatives of the objective function were obtained. However, this method provides true values not for all joint momenta and torques but only for the base joint. We would like to emphasize that the direct differentiation sensitivity analysis using the inverse recursive Lagrangian formulation [1, 57], forward recursive Kane’s notation [3], and transfer matrix method [54] are valid for simple open-chain structures.

We use Lagrange’s formulation of the system dynamics for improving the sensitivity analysis method suggested in [1, 57] for open-tree topological structures. Our work complements the recursive sensitivity analysis techniques based on the Kane’s method [8, 31, 43] and on the Newton-Euler method with the spatial notation [33, 50]. Contrary to the sensitivity analysis derived in [8, 31, 43] for forward recursive dynamics, we perform it for inverse recursive dynamics. In comparison with the forward dynamics formulation, the inverse dynamics does not require time integration and increases the efficiency of the sensitivity analysis. The main differences between our proposed method to describe system topology and the method used in [33] are the following. Firstly, we propose a simple method to describe system topology using the branch connectivity graph and the joint-branch connectivity matrix. In contrast, the square adjacency matrix and the block-weighted adjacency matrix are used in [33]. The adjacency matrix carries the same information as the branch connectivity graph and the joint-branch connectivity matrix but requires more space and so is less efficient. The block-weighted adjacency matrix has a larger size than the adjacency matrix. Secondly, in this work, a homogeneous transformation matrix is used rather than the spatial operator algebra notation (6-D vector formulation) as in [33, 50]. Our work extends the possibilities of recursive dynamics and sensitivity analysis presented in [1, 57] by allowing the inclusion of external forces and torques acting simultaneously at all joints and/or links. This is necessary, for example, for the modeling of two reaction forces acting on both feet during the double support phase of walking. We expect that the proposed sensitivity analysis will reduce the time to evaluate sensitivity information compared to traditional numerical methods. This in turn might reduce the time required by the conventional fmincon solver for computations required in optimization problems for nonlinear dynamic systems with many DOF, such as the human body. Since using MATLAB for testing and debugging is easier than, for instance, employing the interior point optimizer IPOPT [39], our contribution should positively impact research on multibody dynamics.

In summary, we present a first-order recursive sensitivity analysis based on the application of the direct differentiation method to the inverse Lagrangian dynamics of rigid multibody systems which is not affected by the perturbations of design variables. Major contributions in this paper are:

- a new method for describing the topology of mechanical systems with an open-tree structure is proposed;

- known recursive dynamic and sensitivity analyses are modified for the use with dynamic systems having an open-tree structure where external forces and torques act simultaneously on all joints;

- the proposed algorithm can be easily implemented in MATLAB thus allowing the use of high-level programming capabilities for the human motion synthesis.

The paper is organized as follows. Section 2 introduces kinematics of the multibody mechanical system. Dynamic equations of motion are defined in Sect. 3, followed by the sensitivity analysis in Sect. 4. Simulation model is presented in Sect. 5 and simulation results are discussed in Sect. 6.

2 Kinematics

2.1 Joint connectivity graph

A mechanical system can be represented as a finite number of connected links and joints. A graph, on the other hand, is a collection of nodes and edges [32]. To correctly represent the kinematics of a mechanical system, the connectivity of the joints should be described. To this end, we utilize the joint connectivity graph, an undirected graph similar to the one used in [22, 23] where a node represents a link and an arc represents a joint. Since every type of joint (prismatic, revolute, spherical, universal joint, etc.) has to be defined separately, this approach is less efficient in practice. Similarly, each joint type requires its own kinematic description in [33]. Our modification of the connectivity graph employs nodes (vertices) for representing joints, while the arcs (edges) represent connections between neighboring joints. If a mechanical system has an open-tree structure (that is, it does not have closed-loop chains), then its joint connectivity graph is a joint topological tree. On a joint connectivity graph, all joints are numbered so that a given joint $i\in \mathbb {N}$ has a lower number than any of its children, and a higher number than any of its parents, see Fig. 1a. In other words, if a number $\lambda _{j}(i)$ denotes a parent and $\nu _{j}(i)$ denotes a child of a joint i, then

$$\begin{aligned} 1\le \lambda _{j}(i)<i<\nu _{j}(i). \end{aligned}$$

(1)

This indexing is opposite to (or inverse of) the canonical tree notation used in [33, 34]. It follows from (1) that the root node (joint) has the number 1, while in [22, 23] the root node (link) starts with number 0. For a joint topological tree, a joint has only a single parent but can have multiple children. Let the number of children of the node i be denoted by $\kappa _{j}(i)$. We call a node that has at most one child a simple node and the one that has more than one child a complex node (also called junction node in [32] ). A simple node that does not have children nodes will be called an external node. For example, nodes 3 and 5 in Fig. 1a are complex, nodes 6, 7, 9 and 11 are external, and all remaining nodes are simple. For a root node (joint) $\lambda _{j}(i)=0,$ and for an external node $\nu _{j}(i)=0$. A joint numbering system is not unique, and a given joint topological tree can have multiple correct ways of doing this. Ideally, the knowledge of $\lambda _{j}(i)$ for each i furnishes complete information about the topological structure of the system which can be expressed, for example, in the form of the so-called joint parent matrix $\varUpsilon \in \mathbb {R}^{n_{j}\times 2}$, where $n_{j}$ is the total number of joints. The first column $\varUpsilon (i,1)$ would specify the current joint number i, and the second column would specify the number of its parent joint $\varUpsilon (i,2)=\lambda _{j}(i)$. Similarly, complete information is provided if a set of $\nu _{j}(i)$ is specified for each joint i. Compared to other descriptions of the system topology in terms of adjacency and incidence matrices³ [32, 34, 35, 42], the joint parent matrix is already compact. However, the description of the information in the joint parent matrix form can be further compressed and made more efficient.

Fig. 1

Topological tree of a mechanical system with 11 joints and 6 branches: a joint connectivity graph with node numbers representing joints, b branches of the joint connectivity graph, c branch connectivity graph with node numbers representing branches

2.2 Branch connectivity graph and joint-branch connectivity matrix

In this section, we suggest a more efficient (compact) way to describe the topological structure of a mechanical system. Analyzing joint topological trees, we conclude that most nodes (joints) are simple and their description within the joint connectivity graph is rather straightforward, whereas difficulties usually arise in the description of complex nodes. For this purpose, a branch of the joint connectivity graph is defined as a set of its nodes that have simple linear connections, Fig. 1b. One might notice that any branch starts either with a root node or with a child of a complex node and ends either with an external node or with a complex node. Definitions of nodes and branches are analogous to those used in electric circuit theory. A branch connectivity graph is defined similarly to the joint connectivity graph, but the difference is that each node represents a single branch, Fig. 1c, that is, a node in the former graph represents a single graph branch of the latter one. If a mechanical system has an open-tree structure, its branch connectivity graph is a branch topological tree. Similarly to the process of joint numbering, nodes of a branch connectivity graph (open-tree branches of a mechanical system) are numbered so that a given tree branch $i\in \mathbb {N}$ has a lower number than any of its child branches and a higher number than any of its parent branches. For a branch k with a parent $\lambda _{b}(k)$ and a child $\nu _{b}(k)$, the identity similar to (1) holds,

$$\begin{aligned} 1\le \lambda _{b}(k)<k<\nu _{b}(k). \end{aligned}$$

(2)

For a root node (branch) one has $\lambda _{b}(k)=0$ and for an external node $\nu _{b}(k)=0$. Simply speaking, a branch connectivity graph is a “lighter” version of a joint connectivity graph where all simple nodes are removed. Therefore, for any node in a branch connectivity graph, one has $\nu _{b}(k)\ne 1$, which also implies that the branch connectivity graph cannot be further simplified by a similar procedure. In other words, a branch connectivity graph is the simplest representation of the joint connectivity graph which preserves all information about complex nodes. Unless stated otherwise, in what follows the index i refers to the joint number and the index k denotes the branch number. Let $\kappa _{b}(k)$ denote the number of children of a node in a branch connectivity tree. From the definition of a branch, $\kappa _{b}(k)=\kappa _{j}(i)$ where i is the last node of the branch k in the joint connectivity graph, that is, the number of children of the branch k is equal to the number of children of its last joint. In general, $i\ne k,$ because the number of joints is not equal to the number of branches. Since our focus is on a branch connectivity graph and not on simple joint nodes, for the sake of simplicity, we slightly abuse the notation denoting by $\kappa (i)$ the number of children of a node. Complete information about the branch topological tree of a given system is provided if for each branch either $\lambda _{b}(k)$ or $\nu _{b}(k)$ is specified. This can be achieved by specifying a branch parent matrix $\varPhi \in \mathbb {R}^{n_{b}\times 2}$, where $n_{b}$ is the total number of branches in the branch topological tree for the given system. The first column of the matrix shows that the current branch number $\varPhi (k,1)=k$, and the second column specifies its parent branch number $\varPhi (k,2)=\lambda _{b}(k)$. The branch numbering is not unique and can be performed in multiple ways.

When the branch connectivity graph is specified, one cannot construct the kinematic tree of the dynamic system yet because the knowledge about the joints that compose each branch is missing. This information can be provided in a joint-branch connectivity matrix $\varPsi \in \mathbb {R}^{n_{b}\times 2}$. Under the assumption that the joints in a given branch are numbered consecutively, the first column in $\varPsi (k,1)$ specifies the joint number where the branch k starts, whereas the second column $\varPsi (k,2)$ specifies the joint number where it terminates. There are two alternative ways to describe system’s kinematics structure, using either the joint connectivity graph (this requires a single matrix $\varUpsilon$) or the branch connectivity graph in conjunction with the joint-branch connectivity matrix (this requires two matrices $\varPhi$ and $\varPsi$). If a mechanical system possesses many joints and a few branches, using the latter approach is simpler and more efficient whereas for a system with many branches and joints the former description is preferable. For example, for a system with one hundred DOF and only four branches, the branch topological tree has four nodes, and the joint-branch connectivity matrix is a $4\times 2$ matrix. Recall that a node represents a joint in a joint connectivity graph and a branch in a branch connectivity graph. Therefore, for a system whose kinematic structure is represented by a full binary tree, the joint connectivity graph requires less information. Our method to describe the system’s topology is also different (simpler) than in [33]. However, this simplicity is only related to the description of the system’s topology, and it does not affect the sensitivity analysis. In other words, the sensitivity analysis does not depend much on the method to describe the topology of the system.

2.3 Denavit–Hartenberg convention

The Denavit-Hartenberg (D-H) convention is used to formulate the kinematics of the model. It specifies four parameters for each DOF, $\theta _{i}$, $d_{i}$, $a_{i}$, and $\alpha _{i}$, where the variables $\theta _{i}$ and $d_{i}$ are associated with the cases where the joint is revolute or prismatic, respectively. We note that there are multiple variations of the D-H convention. In this paper, the distal version of the D-H convention is utilized. This means that the i-th local frame is attached to the distal end of the link i [24], and a joint i is located at the proximal end of the same link. Alternatively, the same local frame can be attached to the proximal end of the link [12]. Even though all D-H conventions describe the kinematics of arbitrary mechanical systems equally well, care should be taken when working with the D-H convention because of the aforementioned notational differences.

3 Dynamics

Assume that we have a mechanical $n_{q}$-DOF system, then the number of joints $n_{q}=n_{j}$. For a fully actuated system, the number of input (driving) torques is equal to the number of DOF. If the number of input torques is less than the number of joints, then one has an under-actuated system; otherwise the system is over-actuated. A human body is an over-actuated system, which is a beneficial feature in terms of redundancy, because a single muscle can actuate multiple joints and a single joint can be actuated by multiple muscles. However, this redundancy is difficult to model, especially when our goal is to generate a dynamically feasible human body motion. To achieve this, it suffices to consider a fully actuated human body model which determines our main focus on fully actuated multibody systems. The detailed derivation of the dynamic equations of motion is outside the scope of this paper. Our work is based on the Lagrangian formulation, so a reader interested in more details about the derivation (such as Lagrangian function), can consult [1]. Dynamic equations of motion for a mechanical $n_{q}$-DOF system are often represented in the form

$$\begin{aligned} M(q)\ddot{q}+K(q,{\dot{q}})+W(q)=Q, \end{aligned}$$

(3)

where $q=[q_{1}~q_{2}~\ldots q_{n_{q}}]^{T}\in \mathbb {R}^{n_{q}}$ and $\dot{q}=[{\dot{q}}_{1}~{\dot{q}}_{2}~\ldots ~{\dot{q}}_{n_{q}}]^{T}\in \mathbb {R}^{n_{q}}$ are the vectors of generalized coordinates and generalized velocities, respectively, $M(q)\in \mathbb {R}^{n_{q}\times n_{q}}$ is the inertia matrix, $K(q,{\dot{q}})\in \mathbb {R}^{n_{q}}$ is the vector of Coriolis and normal inertial forces, $W(q)\in \mathbb {R}^{n_{q}}$ is the vector of gravitational forces, and $Q=[Q_{1}~Q_{2}~\ldots Q_{n_{q}}]^{T}\in \mathbb {R}^{n_{q}}$ is the vector of generalized forces. The superscript $(\cdot )^{T}$ denotes the transpose operator. These equations can be referred to as “bulk” form, because all DOF are included in the equation, and are therefore computed simultaneously [24]. However, the direct computation of this set of equations is burdensome, especially for high-DOF systems with many joints and links. Instead, there are efficient “recursive” techniques of calculation where each DOF is computed recursively from the previous DOF [11, 30, 41]. We use the recursive Lagrangian formulation to derive the dynamic equations of motion based on the extension of ideas reported in [1, 57, 58] to the case of multiple external loads and topological tree structures.

3.1 Recursive kinematics

For each DOF, the corresponding homogeneous transformation matrix $A_{i} \in \mathbb {R}^{4\times 4}$ is defined as

$$\begin{aligned} A_{i}\!\!=\!\! \begin{bmatrix} \cos {\theta _{i}} &{} -\cos {\alpha _{i}}\sin {\theta _{i}} &{} \sin {\alpha _{i}} \sin {\theta _{i}} &{} a_{i}\cos {\theta _{i}}\\ \sin {\theta _{i}} &{} \cos {\alpha _{i}}\cos {\theta _{i}} &{} -\sin {\alpha _{i}} \cos {\theta _{i}} &{} a_{i}\sin {\theta _{i}}\\ 0 &{} \sin {\alpha _{i}} &{} \cos {\alpha _{i}} &{} d_{i}\\ 0 &{} 0 &{} 0 &{} 1. \end{bmatrix}\!,\nonumber \\ \end{aligned}$$

(4)

where $i=1,\ldots ,n_{q}$ [1]. The matrix transforms homogeneous coordinates from the frame i to its parent frame $i-1$. The first, second and third-order derivatives of $A_{i}$ with respect to $q_{i}$ are defined accordingly as $B_{i}=dA_{i}/dq_{i}$, $C_{i}=dB_{i}/dq_{i}$ and $D_{i}=dC_{i}/dq_{i}$. The matrices $A_{i},$ $B_{i}$, $C_{i}$, and $D_{i}$ are related by the equations $B_{i}=ZA_{i}$, $C_{i}=ZB_{i}$, $D_{i}=ZC_{i}$ [24] where the matrix $Z\in \mathbb {R}^{4\times 4}$ has all zero entries except for the following: $Z(1,2)=-1$ and $Z(2,1)=1$ for a revolute joint, and $Z(3,4)=1$ for a prismatic joint. The global homogeneous transformation matrix is introduced using the local transformation matrices as $T_{i}=A_{1}\cdot \ldots \cdot A_{i}\in \mathbb {R}^{4\times 4}$. The first and second-order time derivatives of the global transformation matrix are $S_{i}=dT_{i}/dt={\dot{T}}_{i}\in \mathbb {R}^{4\times 4}$ and $R_{i} =dS_{i}/dt={\dot{S}}_{i}\in \mathbb {R}^{4\times 4}$. These matrices can be calculated in recursive form starting from the first, root joint, until the last, end-effector joint, in the kinematic chain:

$$\begin{aligned} \begin{aligned}&T_i = T_{p} A_i, \\ {}&S_i = S_{p} A_i + T_{p} B_i {\dot{q}}_i, ~~~ i=1,\ldots ,n_q \\ {}&R_i = R_{p} A_i + 2 S_{p} B_i {\dot{q}}_i + T_{p} C_i {\dot{q}}^2_i + T_{p} B_i \ddot{q}_i. \end{aligned} \end{aligned}$$

(5)

where $\ddot{q}_{i}=d{\dot{q}}_{i}/dt$. For a system which has only simple nodes in the joint topological tree or for a single branch within the joint topological tree, the subscript $p=i-1$. For a node that has a complex node parent, if the joint connectivity graph is used, the subscript $p=\lambda _{j}(i)=\varUpsilon (i,2)$ and if the branch connectivity graph is used, $p=\varPsi (\lambda _{b}(k),2)=\varPsi (\varPhi (k,2),2)$, where k is the index of the branch to which the joint i belongs. For the first joint ($i=1$), one sets $p=0$. In summary, if $i=\varPsi (k,1)\wedge \lambda _{b}(k)\ne 0,$ then $p=\varPsi (\varPhi (k,2),2)$, if $i=\varPsi (k,1)\wedge \lambda _{b}(k)=0,$ then $p=0$, otherwise, $p=i-1$. The initial values of the global transformation matrix $T_{i}$ and its time derivatives are $T_{0}=I$, $S_{0}=0$, and $R_{0}=0$, where $I\in \mathbb {R}^{4\times 4}$ and $0\in \mathbb {R}^{4\times 4}$ are, correspondingly, the identity and zero matrices. With the help of homogeneous matrices, the position $r_{g}$, velocity $v_{g}$, and acceleration $a_{g}$ of any point of the mechanical system with respect to the global frame can be specified as

$$\begin{aligned} r_{g}=T_{i}{r}_{i},\quad v_{g}=S_{i}{r}_{i},\quad a_{g}=R_{i}{r}_{i}, \end{aligned}$$

(6)

where ${r}_{i}$ is the position of the point in the local coordinate system i. The set of equations in (5) defines what is called the recursive forward kinematics.

3.2 Recursive dynamics

After the forward kinematics matrices are computed, dynamic matrices can be obtained. Firstly, we define the mass $m_{i}\in \mathbb {R}$ and inertia matrix $J_{i}\in \mathbb {R}^{4\times 4}$ of the link associated with a joint i and expressed in the local frame i. The inertia matrix is defined as

(7)

where $J^*_{xx} = \frac{1}{2}(-J_{xx}+J_{yy}+J_{zz})$, $J^*_{yy} = \frac{1}{2}(J_{xx}-J_{yy}+J_{zz})$, $J^*_{zz} = \frac{1}{2}(J_{xx}+J_{yy}-J_{zz})$. $J_{ij}$, $i,j \in {x,y,z}$, are the moments and products of inertia of link i in its local frame of reference, while , and denote the location of the center of mass of link i in the coordinate system i. Secondly, the inertia $H_{i}\in \mathbb {R} ^{4\times 4}$ and the external force $F_{i}\in \mathbb {R}^{4\times n_{q}}$ matrices, the gravity $E_{i}\in \mathbb {R}^{4}$ and the external torque $G_{i}\in \mathbb {R}^{4}$ vectors are defined. For simplicity, we refer to these matrices as dynamic transformation matrices, because they transform dynamic quantities from a given joint to the next one. These matrices are also defined recursively, but this time from the last joint (end-effector) to the first joint (root)

(8)

If a system does not have complex nodes in its joint topological tree or within a branch $\kappa (i)=1,$ the subscript $p=i+1$. For a complex node in the joint topological tree $p=\nu _{j}(i),$ and in the branch topological tree $p=\varPsi (\nu _{b}(k),1)=\varPsi (\varPhi (j(k),1),1)$, where, as above, k is the index of the branch to which the joint i belongs. The index notation j(k) is used for the children of the branch k found through the branch parent matrix by using the identity $k=\varPhi (j,2)$. For the last joint in the branch without child joints, $\kappa (i)=0;$ we can also assume that the initial values of the dynamic matrices and vectors are $D_{n_{q}+1}=0$ and $E_{n_{q}+1}=F_{n_{q} +1}=G_{n_{q}+1}=0$. The same initial values are used for any external node in the joint connectivity graph. In summary, if $i=\varPsi (k,2)\wedge \nu _{b} (k)\ne 0,$ then $p=\varPsi (\varPhi (j(k),1),1)$, if $i=\varPsi (k,2)\wedge \nu _{b}(k)=0$, then $\kappa (i)=0$, and if $i\ne \varPsi (k,2)$, then $p=i+1$ and $\kappa (i)=1$. Furthermore, is the location of the center of mass of link i with respect to frame i, where the matrix has all zero entries except for those in the only non-zero column i,  . This is the location of the point of application of an external point force $f_{i}$ acting on the link i expressed in the local frame i. The net external force $f_{i}=[f_{i,x} ~f_{i,y}~f_{i,z}~0]^{T}$ and torque $h_{i}=[h_{i,x}~h_{i,y}~h_{i,z}~0]^{T}$ acting on the link i are expressed in the global frame.⁴ The net external forces and torques acting on all links can be grouped into matrices $V=[f_{1}~f_{2}\cdots f_{n_{q}}]\in \mathbb {R}^{4\times n_{q}}$ and $U=[h_{1}~h_{2}\cdots h_{n_{q} }]\in \mathbb {R}^{4\times n_{q}}$.

The generalized force $Q_{i}$ acting on the link i (i-th component of the vector Q from (3)) can be found by a similar recursive process starting from the last link to the first link,

$$\begin{aligned} Q_{i}= & {} \text {tr}\left[ \frac{\partial {T_{i}}}{\partial {q_{i}}} H_{i}\right] -g^{T}\frac{\partial {T_{i}}}{\partial {q_{i}}}E_{i} -\text {tr}\left[ V^{T}\frac{\partial {T_{i}}}{\partial {q_{i}}} F_{i}\right] _{i:n_{q}}\nonumber \\{} & {} -G_{i}^{T}T_{p}w_{0}, \end{aligned}$$

(9)

for $i=1,\ldots ,n_{q}$, $\text {tr}[\cdot ]$ is the trace operator, and $\text {tr}[\cdot ]_{i:n_{q}}$ denotes the sum of diagonal matrix elements from i until $n_{q}$, $g=[g_{x}~g_{y}~g_{z}~0]^{T}$ is the gravity vector expressed in the global frame of reference,⁵ the vector $w_{0}$ is defined as $w_{0} =[0~0~1~0]^{T}$ for a revolute joint and as $w_{0}=0\in \mathbb {R}^{4}$ for a prismatic joint. The subscript p in (9) is defined as in (5): if $i=\varPsi (k,1)\wedge \lambda _{b}(k)\ne 0,$ then $p=\varPsi (\varPhi (k,2),2)$, if $i=\varPsi (k,1)\wedge \lambda _{b}(k)=0,$ then $p=0$, otherwise $p=i-1$. The external force $f_{i}$ and the torque $h_{i}$ are both expressed in the global coordinate system. The process of calculating the torque and the terms defined in (8) is called the recursive backward dynamics.

4 Sensitivity analysis

In gradient-based optimization algorithms, convergence to the local minimum is highly dependent on the gradient information. It is known that the computation of the derivatives of the objective function and constraints in some cases consumes over 90% of the CPU time required for each iteration of the algorithm [2]. The computation time depends on the selection of the set of generalized coordinates, constraints, constraints enforcement schemes, the set of sensitivity parameters, differentiation methods, and other factors. Thus, for solving an NLP problem efficiently, it is desirable to provide gradients of the object function and constrains with respect to optimization variables. Furthermore, the gradient and Jacobian information enable a coherent and easy extension of algorithms to more complex models making algorithms well scalable. To this end, a sensitivity analysis is performed, i.e., partial derivatives of the desired expressions with respect to the generalized coordinates, velocities, and accelerations are determined. There are two reasons for the utilization of the direct differentiation method for sensitivity analysis in this paper. The first is that the adjoint variable method requires integration of the adjoint sensitivity equations. However, we use the inverse dynamics formulation to avoid the integration of dynamic equations of motion. Thus, the adjoint variable method would neutralize our effort to avoid the time integration. Secondly, as already mentioned, our work is extension of the work in [1], and so we continue to use the direct differentiation methods as they did.

4.1 Kinematic sensitivity analysis

The sensitivity of homogeneous transformation matrices with respect to the generalized coordinates, velocities and accelerations is written as

$$\begin{aligned} \frac{\partial {T_{i}}}{\partial {q_{m}}},~\frac{\partial {S_{i}}}{\partial {q_{m}}},~\frac{\partial {S_{i}}}{\partial {{\dot{q}}_{m}}},~\frac{\partial {R_{i} }}{\partial {q_{m}}},~\frac{\partial {R_{i}}}{\partial {{\dot{q}}_{m}}},~\frac{\partial {R_{i}}}{\partial {\ddot{q}_{m}}},~\frac{\partial ^{2}{T_{i}} }{\partial {q_{m}}\partial {q_{i}}}\in \mathbb {R}^{4\times 4}, \end{aligned}$$

where $i=1,\ldots ,n_{q}$, $m=1,\ldots ,n_{q}$. Since $T_{i}$ depends only on the generalized coordinates, its partial derivatives with respect to generalized velocities and accelerations are zeros. Similarly, partial derivatives of $S_{i}$ with respect to generalized accelerations are zeros. The homogeneous transformation matrices can be found from the following relations. For $m<i,$

$$\begin{aligned}{} & {} \begin{aligned}&\frac{\partial {T_{i}}}{\partial {q_{m}}}=\frac{\partial {T_{p}}}{\partial {q_{m}}}A_{i},\\&\frac{\partial {S_{i}}}{\partial {q_{m}}}=\frac{\partial {S_{p}}}{\partial {q_{m}}}A_{i}+\frac{\partial {T_{p}}}{\partial {q_{m}}}B_{i}{\dot{q}} _{i},\\&\frac{\partial {S_{i}}}{\partial {{\dot{q}}_{m}}}=\frac{\partial {S_{p}} }{\partial {{\dot{q}}_{m}}}A_{i},\\&\frac{\partial {R_{i}}}{\partial {q_{m}}}=\frac{\partial {R_{p}}}{\partial {q_{m}}}A_{i}+2\frac{\partial {S_{p}}}{\partial {q_{m}}}B_{i}\dot{q}_{i}+\frac{\partial {T_{p}}}{\partial {q_{m}}}C_{i}{\dot{q}}_{i}^{2} +\frac{\partial {T_{p}}}{\partial {q_{m}}}B_{i}\ddot{q}_{i},\\&\frac{\partial {R_{i}}}{\partial {{\dot{q}}_{m}}}=\frac{\partial {R_{p}} }{\partial {{\dot{q}}_{m}}}A_{i}+2\frac{\partial {S_{p}}}{\partial {{\dot{q}}_{m}} }B_{i}{\dot{q}}_{i},\\&\frac{\partial {R_{i}}}{\partial {\ddot{q}_{m}}}=\frac{\partial {R_{p}} }{\partial {\ddot{q}_{m}}}A_{i},\\&\frac{\partial ^{2}{T_{i}}}{\partial {q_{m}}\partial {q_{i}}}=\frac{\partial {T_{p}}}{\partial {q_{m}}}B_{i}. \end{aligned} \end{aligned}$$

(10)

$$\begin{aligned}{} & {} \begin{aligned}&\text {For the case}~~(m=i)\text {:}\\&\frac{\partial {T_{i}}}{\partial {q_{m}}}=T_{p}B_{i},\\&\frac{\partial {S_{i}}}{\partial {q_{m}}}=S_{p}B_{i}+T_{p}C_{i}{\dot{q}}_{i},\\&\frac{\partial {S_{i}}}{\partial {{\dot{q}}_{m}}}=T_{p}B_{i},\\&\frac{\partial {R_{i}}}{\partial {q_{m}}}=R_{p}B_{i}+2S_{p}C_{i}{\dot{q}} _{i}+T_{p}D_{i}{\dot{q}}_{i}^{2}+T_{p}C_{i}\ddot{q}_{i},\\&\frac{\partial {R_{i}}}{\partial {{\dot{q}}_{m}}}=2S_{p}B_{i}+2T_{p}C_{i} {\dot{q}}_{i},\\&\frac{\partial {R_{i}}}{\partial {\ddot{q}_{m}}}=T_{p}B_{i},\\&\frac{\partial ^{2}{T_{i}}}{\partial {q_{m}}\partial {q_{i}}}=T_{p}C_{i}. \end{aligned} \end{aligned}$$

(11)

$$\begin{aligned}{} & {} \begin{aligned}&\text {For the case}~~(m>i)\vee (m<i\wedge i=\varPsi (k,1)\wedge \lambda _{b}(k)=0)\text {:}\\&\frac{\partial {T_{i}}}{\partial {q_{m}}}=\frac{\partial {S_{i}}}{\partial {q_{m}}}=\frac{\partial {S_{i}}}{\partial {{\dot{q}}_{m}}}=\frac{\partial {R_{i}}}{\partial {q_{m}}}\\&=\frac{\partial {R_{i}}}{\partial {{\dot{q}} _{m}}}=\frac{\partial {R_{i}}}{\partial {\ddot{q}_{m}}}= \frac{\partial ^{2}{T_{i}}}{\partial {q_{m}}\partial {q_{i}}}=0 \end{aligned} \end{aligned}$$

(12)

Note that $p=\varPsi (\lambda _{b}(k),2)=\varPsi (\varPhi (k,2),2)$ in (10) and (11) if, in addition to the conditions imposed at the beginning of each equation, $i=\varPsi (k,1)\wedge \lambda _{b}(k)\ne 0$. These additional conditions are used to determine whether a parent branch exists. The index k refers to the branch number associated with the joint number i, e.g., it is understood that the joint i is located within the branch k. If $i=\varPsi (k,1)\wedge \lambda _{b}(k)=0$, then $p=0$ in (11) for $m=i$, but for $m<i$ (12) applies. If $i\ne \varPsi (k,1)$, then $p=i-1$. Alternatively, in the joint connectivity graph notation, $p=\lambda _{j}(i)=\varUpsilon (i,2)$.

4.2 Dynamic sensitivity analysis

Similarly to the kinematics sensitivity analysis, sensitivity of the dynamic transformation matrices with respect to generalized coordinates, velocities and accelerations is written as

$$\begin{aligned} \frac{\partial {H_{i}}}{\partial {q_{m}}},\frac{\partial {H_{i}}}{\partial {{\dot{q}}_{m}}},~\frac{\partial {H_{i}}}{\partial {\ddot{q}_{m}}}\in \mathbb {R}^{4\times 4},\frac{\partial {E_{i}}}{\partial {q_{m}}},\frac{\partial {G_{i}}}{\partial {q_{m}}}\in \mathbb {R}^{4\times 1},\\\frac{\partial {F_{i}}}{\partial {q_{m}}}\in \mathbb {R}^{4\times n_{q}} \end{aligned}$$

where $i=1,\ldots ,n_{q}$, $m=1,\ldots ,n_{q}$. These matrices can be found from the following relations. For $m\le i,$

$$\begin{aligned} \begin{aligned}&\frac{\partial {H_{i}}}{\partial {q_{m}}}=J_{i}\frac{\partial {R_{i}^{T}} }{\partial {q_{m}}}+\sum _{p=1}^{\kappa (i)}A_{p}\frac{\partial {H_{p}}}{\partial {q_{m}}},\\&\frac{\partial {H_{i}}}{\partial {{\dot{q}}_{m}}}=J_{i}\frac{\partial {R_{i} ^{T}}}{\partial {{\dot{q}}_{m}}}+\sum _{p=1}^{\kappa (i)}A_{p}\frac{\partial {H_{p} }}{\partial {{\dot{q}}_{m}}},\\&\frac{\partial {H_{i}}}{\partial {\ddot{q}_{m}}}=J_{i}\frac{\partial {R_{i}^{T}}}{\partial {\ddot{q}_{m}}}+\sum _{p=1}^{\kappa (i)}A_{p}\frac{\partial {H_{p}}}{\partial {\ddot{q}_{m}}},\\&\frac{\partial {E_{i}}}{\partial {q_{m}}}=0,\\&\frac{\partial {F_{i}}}{\partial {q_{m}}}=0,\\&\frac{\partial {G_{i}}}{\partial {q_{m}}}=0. \end{aligned} \end{aligned}$$

(13)

For $m\ge i+1,$

$$\begin{aligned} \begin{aligned}&\frac{\partial {H_{i}}}{\partial {q_{m}}}=\sum _{p=1}^{\kappa (i)}A_{p} \frac{\partial {H_{p}}}{\partial {q_{m}}}+\beta B_{p}H_{p},\\&\frac{\partial {H_{i}}}{\partial {{\dot{q}}_{m}}}=\sum _{p=1}^{\kappa (i)} A_{p}\frac{\partial {H_{p}}}{\partial {{\dot{q}}_{m}}},\\&\frac{\partial {H_{i}}}{\partial {\ddot{q}_{m}}}=\sum _{p=1}^{\kappa (i)} A_{p}\frac{\partial {H_{p}}}{\partial {\ddot{q}_{m}}},\\&\frac{\partial {E_{i}}}{\partial {q_{m}}}=\sum _{p=1}^{\kappa (i)}A_{p} \frac{\partial {E_{p}}}{\partial {q_{m}}}+\beta B_{p}E_{p},\\&\frac{\partial {F_{i}}}{\partial {q_{m}}}=\sum _{p=1}^{\kappa (i)}A_{p} \frac{\partial {F_{p}}}{\partial {q_{m}}}+\beta B_{p}F_{p},\\&\frac{\partial {G_{i}}}{\partial {q_{m}}}=0. \end{aligned} \end{aligned}$$

(14)

For the case $m\ge i+1\wedge \varPsi (k,2)\wedge \nu _{b}(k)=0,$

$$\begin{aligned} \frac{\partial {H_{i}}}{\partial {q_{m}}}= & {} \frac{\partial {H_{i}}}{\partial {{\dot{q}}_{m}}}=\frac{\partial {H_{i}}}{\partial {\ddot{q}_{m}}}\nonumber \\= & {} \frac{\partial {E_{i}}}{\partial {q_{m}}}=\frac{\partial {F_{i}}}{\partial {q_{m}} }=\frac{\partial {G_{i}}}{\partial {q_{m}}}=0 \end{aligned}$$

(15)

where $p=\varPsi (\nu _{b}(k),1)=\varPsi (\varPhi (j(k),1),1)$ provided that, in addition to the conditions imposed at the beginning of each equation, $i=\varPsi (k,2)\wedge \nu _{b}(k)\ne 0$. These conditions verify the existence of children of the branch k and the sum includes all its child branches $\kappa (i)$. If $i=\varPsi (k,2)\wedge \nu _{b}(k)=0$, then $\kappa (i)=0$ in (13) for $m\le i$, but for $m\ge i+1$ (15) applies. In the case $i\ne \varPsi (k,2)$, one has $p=i+1$ and $\kappa (i)=1$. The coefficient $\beta =1$ if $m=p$, which means that $m=\varPsi (\nu _{b}(k),1)$ for the case $i=\varPsi (k,2)$ and $m=i+1$ for the case $i\ne \varPsi (k,2)$, otherwise, $\beta =0$. In the joint connectivity graph notation, the index $p=\nu _{j}(i)$.

Finally, the torque sensitivity can be found as follows.

$$\begin{aligned}{} & {} \begin{aligned}&\frac{\partial {Q_{i}}}{\partial {q_{m}}}=\text {tr}\left[ \frac{\partial ^{2}{T_{i}}}{\partial {q_{m}}\partial {q_{i}}}H_{i}+\frac{\partial {T_{i}}}{\partial {q_{i}}}\frac{\partial {H_{i}}}{\partial {q_{m}} }\right] -g^{T}\frac{\partial ^{2}{T_{i}}}{\partial {q_{m}}\partial {q_{i}} }E_{i}\\&-\text {tr}\left[ V^{T}\frac{\partial ^{2}{T_{i}}}{\partial {q_{m} }\partial {q_{i}}}F_{i}\right] _{i:n_{q}}-G_{i}^{T}\frac{\partial {T_{p}} }{\partial {q_{m}}}w_{0}, \qquad m\le i, \\&\frac{\partial {Q_{i}}}{\partial {q_{m}}}=\text {tr}\left[ \frac{\partial {T_{i}}}{\partial {q_{i}}}\frac{\partial {H_{i}}}{\partial {q_{m}} }\right] -g^{T}\frac{\partial {T_{i}}}{\partial {q_{i}}}\frac{\partial {E_{i}} }{\partial {q_{m}}}-\text {tr}\left[ V^{T}\frac{\partial {T_{i}} }{\partial {q_{i}}}\frac{\partial {F_{i}}}{\partial {q_{m}}}\right] _{i:n_{q}}, \\&m\ge i+1, \end{aligned} \end{aligned}$$

(16)

$$\begin{aligned}{} & {} \begin{aligned}&\frac{\partial {Q_{i}}}{\partial {{\dot{q}}_{m}}}=\text {tr} \left[ \frac{\partial {T_{i}}}{\partial {q_{i}}}\frac{\partial {H_{i}} }{\partial {{\dot{q}}_{m}}}\right] ,\\&\frac{\partial {Q_{i}}}{\partial {\ddot{q}_{m}}}=\text {tr} \left[ \frac{\partial {T_{i}}}{\partial {q_{i}}}\frac{\partial {H_{i}} }{\partial {\ddot{q}_{m}}}\right] \end{aligned} \end{aligned}$$

(17)

where, as in (9), $p=\varPsi (\varPhi (k,2),2)$ if $i=\varPsi (k,1)\wedge \lambda _{b}(k)\ne 0$ and $p=0$ if $i=\varPsi (k,1)\wedge \lambda _{b}(k)=0,$ otherwise, $p=i-1$. Equations (10)-(17 ) define a recursive kinematics and dynamic sensitivity analysis. The corresponding pseudo-algorithms are summarized in Algorithms 1 -3, which require three loops, the kinematic loop from the root to the external nodes, the dynamic loop from external nodes to the root, and, finally, the torque loop back again from the root to external nodes. It is possible to include computations in Algorithm 3 in Algorithm 2 so that only two loops are required. For the sake of clarity, however, we present the sensitivity analysis in three loops.

5 Simulation

5.1 Human model

A rigid human body model consisting of rigid links and revolute joints is used to test the proposed sensitivity method. A complete human body has about 350 joints [40]. For our purposes, the human model has 43 DOF and 20 links. The number of joints exceeds the number of links due to the presence of virtual links which have zero length and mass. For example, a spherical joint is represented as a combination of three consecutive rotational joints. The 43-DOF human model is shown schematically in Fig. 2 where the green dot indicates the origin of the global coordinate system. The red dot indicates the pelvis point used as the reference point for the human body model. The global coordinate system has axes $x_{0}$-$y_{0}$-$z_{0}$, and the local coordinate system i, associated with the joint i and attached to the distal end of the link i, has the unit vectors $x_{i}$-$y_{i}$-$z_{i}$. The mobility of the human model with respect to the global frame of reference is achieved by the global virtual DOF of the joints connected to the pelvis point. Global virtual DOF consists of three translational and three rotational DOF. All other human links and joints are referenced through the pelvis point. The joint and branch connectivity graphs for the human model are shown in Fig. 3. There are 43 joints and 7 branches. The corresponding joint parent matrix $\varUpsilon \in \mathbb {R}^{43\times 2}$, the branch parent matrix $\varPhi \in \mathbb {R} ^{7\times 2}$, and the joint-branch connectivity matrix $\varPsi \in \mathbb {R} ^{7\times 2}$ are defined by (18)-(20). Even for this modestly-sized system, the branch connectivity graph allows a more compact and efficient description of the system topology in comparison with the joint connectivity graph. The values of D-H parameters for the human model are collected in Table 1. The values of the physical parameters for the 43 DOF human model are provided in Table 2. Note that the neutral position of the human model corresponds to the vertical rest position (the human model stands vertically straight with both arms stretched along the sides, as shown in Fig. 2).

Fig. 2

Schematic diagram of the humanoid model with 43 DOF and ten links. For clarity, only the z-axes of the local coordinate systems are shown

Fig. 3

Joint (a) and branch (b) connectivity graphs for the human model

$$\begin{aligned} \varUpsilon ^{T}&= \begin{aligned}&\left[ \begin{array}{lllllllllllllllllll} 1 &{} 2 &{} 3 &{} 4 &{} 5 &{} 6 &{} 7 &{} 8 &{} 9 &{} 10 &{} 11 &{} 12 &{} 13 &{} 14 &{} 15 &{} 16 &{} 17 &{} 18 &{} 19 \\ 0 &{} 1 &{} 2 &{} 3 &{} 4 &{} 5 &{} 6 &{} 7 &{} 8 &{} 9 &{} 10 &{} 11 &{} 12 &{} 5 &{} 14 &{} 15 &{} 16 &{} 17 &{} 18 \end{array} \right. \\ {}&\left. \begin{array}{lllllllllllllllllll} 20 &{} 21 &{} 22 &{} 23 &{} 24 &{} 25 &{} 26 &{} 27 &{} 28 &{} 29 &{} 30 &{} 31 &{} 32 &{} 33 &{} 34 &{} 35 \\ 19 &{} 20 &{} 5 &{} 22 &{} 23 &{} 24 &{} 25 &{} 26 &{} 27 &{} 28 &{} 29 &{} 30 &{} 22 &{} 32 &{} 33 &{} 34 \end{array}\right. \\ {}&\left. \begin{array}{lllllllll} 35 &{} 36 &{} 37 &{} 38 &{} 39 &{} 40 &{} 41 &{} 42 &{} 43 \\ 34 &{} 35 &{} 36 &{} 37 &{} 38 &{} 39 &{} 22 &{} 41 &{} 42 \end{array}\right] . \end{aligned}\nonumber \\ \end{aligned}$$

(18)

$$\begin{aligned} \varPhi ^{T}&= \begin{bmatrix} 1 &{} 2 &{} 3 &{} 4 &{} 5 &{} 6 &{} 7\\ 0 &{} 1 &{} 1 &{} 1 &{} 4 &{} 4 &{} 4 \end{bmatrix}. \end{aligned}$$

(19)

$$\begin{aligned} \varPsi ^{T}&= \begin{bmatrix} 1 &{} 6 &{} 14 &{} 22 &{} 23 &{} 32 &{} 41\\ 5 &{} 13 &{} 21 &{} 22 &{} 31 &{} 40 &{} 43 \end{bmatrix}. \end{aligned}$$

(20)

Table 1 D-H table for the 43 DOF humanoid model. Index i refers to the joint (DOF) number, while index k refers to the branch number

Table 2 Values of physical parameters. $m_{j}$ is the link mass [kg], $L_{j}$ is the link length [cm]

5.2 Implementation

We performed the sensitivity analysis using the MATLAB software. Due to the inverse dynamics formulation utilized in this work, the reference trajectories for generalized coordinates, velocities, and accelerations are required for the sensitivity analysis. As reference trajectories for the joints, we simply imposed the sinusoidal function $q_{i}=\sin {2\pi ft}$, where $f=1$ and t are frequency and time, respectively. The generalized velocities and accelerations can be obtained by a simple time differentiation of the generalized coordinates. These reference trajectories were discretized with a sampling time of 10 ms, whereas the duration of the simulation was set to 3 s, resulting in a total of 301 sampling instances. At each sampling instance, both numerical and suggested analytical sensitivity values were evaluated. Numerical sensitivity was estimated from (9) using the jacobianest function of MATLAB, which is a fully adaptive and robust numerical differentiation tool [14]. The analytical sensitivity was obtained from (16)-(17 ). Note that for a system with 43 DOF, the total number of different torque sensitivity values $\frac{\partial {Q_{i}}}{\partial {q_{m}}}$, $i,m=1,\ldots ,43$, is equal to $43^{2}=1849$. Similarly for the torque sensitivity values with respect to velocities and accelerations, but some values would be zero. For example, for torque and joint angles from different branches, such as $i=42$ and $m=18$ in Fig. 3, the torque sensitivity is zero because these branches do not influence each other. The root mean squared (RMS) error was also calculated. The simulations were performed on a ThinkPad notebook with an Intel Core i5-10310U CPU processor and 16 GB RAM. The sensitivity analysis results are provided in the next section.

6 Results

Fig. 4

Results of the sensitivity analysis for $\frac{\partial {Q_{20}} }{\partial {q_{5}}}$, $\frac{\partial {Q_{30}}}{\partial {{\dot{q}}_{25}}}$, and $\frac{\partial {Q_{42}}}{\partial {\ddot{q}_{22}}}$

Fig. 5

Results of the sensitivity analysis for $\frac{\partial {Q_{18}} }{\partial {q_{14}}}$, $\frac{\partial {Q_{18}}}{\partial {{\dot{q}}_{14}}}$, and $\frac{\partial {Q_{18}}}{\partial {\ddot{q}_{14}}}$

The results for randomly selected torque sensitivity values ${\partial {Q_{20}}}/{\partial {q_{5}}}$, ${\partial {Q_{30}}}/{\partial {{\dot{q}}_{25}}}$, and ${\partial {Q_{42}}}/{\partial {\ddot{q}_{22}}}$ are shown in Fig. 4 with the obtained sensitivity trajectories in the left column and the difference between the numerical and analytical methods in the right column. In other words, the error plots on the right show a simple difference between analytical and numerical sensitivities. The results obtained by using the analytical method agree well with the numerical results. The example where the numerical and analytical results differ is shown in Fig. 5. Observe that there are spikes in the numerical torque sensitivity values for ${\partial {Q_{18}}}/{\partial {q_{14}}}$ at $t=0.5$ and $t=1$ s, but similar spikes are not noticeable for ${\partial {Q_{18}}}/{\partial {{\dot{q}}_{14}}}$ and ${\partial {Q_{18}}}/{\partial {\ddot{q}_{14}}}$. This indicates that the observed spike is due to the inaccuracy in the numerical estimation of the sensitivity ${\partial {Q_{18}}}/{\partial {q_{14}}}$. Otherwise, similar spikes should have been also observed in the plots of velocity and acceleration sensitivities. To further analyze the numerical behavior, we plotted the trajectories of the generalized coordinates $q_5$ and $q_{14}$, as well as computed the torques $Q_{20}$ and $Q_{18}$ in Fig. 6. One can see that the numerical errors observed in the sensitivity ${\partial {Q_{18}}}/{\partial {q_{14}}}$ occur exactly when $q_{14}=0$ and $Q_{18}=0$. Thus, numerical errors occur exactly when both the numerator and the denominator vanish. From the trajectories of $q_5$ and $Q_{20}$, we observe that, when the coordinate $q_5=0$, the torque $Q_{20} \ne 0$. Similarly, the trajectories of ${\dot{q}}_{25}$ and $Q_{30}$ demonstrate that at times when the former (angular velocity) is zero, the latter (torque) differs from zero, (see Fig. 6). As expected, the corresponding numerical sensitivity ${\partial {Q_{30}}}/{\partial {{\dot{q}}_{25}}}$, Fig. 4, is computed correctly. Thus, the numerical algorithm copes well with the sensitivity calculation when only the denominator vanishes or is close to zero, but it might give erroneous results when both the numerator and the denominator are close to zero. Therefore, numerical evaluation of the sensitivity values close to a singularity should be exercised with care. The cumulative time for obtaining all of the analytical sensitivity expressions (measured using tic and toc commands at each sampling time and summed up for all 301 sampling times) was 16.6 ms, whereas the cumulative time for computing the numerical sensitivity was $2.62\cdot 10^{5}$ s, amounting roughly to 3 days and 48 min. Thus, the numerical sensitivity analysis takes seven orders of magnitude more time than its analytical counterpart. We expect that for more complex, nonlinear systems with a larger number of DOF the magnitude of errors and the computational time for the numerical sensitivity analysis would be even larger. These results suggest that the numerical sensitivity analysis is error-prone, time-consuming, and computationally expensive. As a result, the numerical sensitivity analysis should be used only as the very last option when other methods are not available for some reason. Our proposed recursive sensitivity analysis is accurate and efficient.

Fig. 6

Time variation of the generalized coordinates $q_5$, $q_{14}$, velocity ${\dot{q}}_{25}$, and torques $Q_{18}$, $Q_{20}$, $Q_{30}$

7 Conclusions

The first-order recursive sensitivity analysis based on the application of the direct differentiation method to the inverse Lagrangian dynamics of rigid multibody systems was presented. We suggested a new description of the system topology based on the branch connectivity graph. The inverse dynamic algorithm based on the Lagrangian formulation was extended to systems with external forces and torques acting at each joint. The advantages of the proposed sensitivity analysis are the following. The method does not suffer from numerical issues associated with the perturbation magnitude in design variables. It has low computational costs and is easy to implement in high-level programming languages such as MATLAB which makes programming and debugging easy. The method is valid for open-chain and open-tree type topological systems. Finally, it is stable, accurate, and numerically efficient. As a result, the proposed algorithm becomes a useful tool in the sensitivity analysis of mechanical multibody systems.

Future work could include the incorporation of the divide-and-conquer algorithm into the sensitivity analysis to speed up computation time through parallelization. An extension of our sensitivity analysis for closed-loop mechanical systems and its application to the human body motion synthesis problem are pending.