# Closed-form time derivatives of the equations of motion of rigid body systems

Andreas Müller, Shivesh Kumar

► To cite this version:

Andreas Müller, Shivesh Kumar. Closed-form time derivatives of the equations of motion of rigid body systems. Multibody System Dynamics, 2021, 53, 53 (3), pp.257-273. 10.1007/s11044-021-09796-8 hal-05162338

HAL Id: hal-05162338

https://hal.science/hal-05162338v1

Submitted on 23 Jul 2025

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Closed Form Time Derivatives of the Equations of Motion of Rigid Body Systems

Andreas Müller¹ and Shivesh Kumar²

¹ Johannes Kepler University, Linz, Austria
a.mueller@jku.at

² Robotics Innovation Center, DKFI GmbH, Bremen, Germany
shivesh.kumar@dfki.de

**Abstract.** Derivatives of equations of motion (EOM) describing the dynamics of rigid body systems are becoming increasingly relevant for the robotics community and find many applications in design and control of robotic systems. Controlling robots, and multibody systems comprising elastic components in particular, not only requires smooth trajectories but also the time derivatives of the control forces/torques, hence of the EOM. This paper presents the time derivatives of the EOM in closed form up to second-order as an alternative formulation to the existing recursive algorithms for this purpose, which provides a direct insight into the structure of the derivatives. The Lie group formulation for rigid body systems is used giving rise to very compact and easily parameterized equations.

**Keywords:** Rigid body dynamics, derivatives of equations of motion, inverse dynamics, closed form, screws, Lie group, higher-order inverse dynamics

## 1 Introduction

Rigid body dynamics algorithms for evaluating the equations of motion (EOM) and their derivatives find numerous applications in the design optimization and control of modern robotic systems. The equations of motion can be differentiated with respect to state variables, control output (generalized forces), time and physical parameters of the robot (see [8] for an overview). These derivatives can be computed with several methods: 1) approximation by finite differences, 2) automatic differentiation [30], i.e. by applying the chain rule formula in an automatic way knowing the derivatives of basic functions (cos, sin or exp), 3) closed form derivatives of the EOM and 4) recursive and analytical formulations exploiting the structure of the closed form motion equations. While the first two methods are generic and numerical in nature, the latter two are analytical in nature and exploit the structure of the EOM Analytical and recursive partial derivatives of the EOM of rigid body systems with respect to state variables and generalized forces have been reported in the literature [15, 3]. These are useful in optimal control of legged robots (e.g. differential dynamic programming [17]) and
their computational design and optimization [12]. Time derivatives of EOM are required for the model-based control and motion planning of robots with higher-order continuity [25], since for highly dynamic applications not only the actuation forces but also their derivatives must be bounded in order to ensure feasibility. Flatness-based control of robots with flexible joints, and of robots equipped with series elastic actuators (SEA) or variable stiffness actuators (VSA), necessitate the first and second time derivatives of the EOM of the robot [6, 9, 24]. Therefore, recursive $O(n)$-algorithms for the evaluation of the time derivatives were developed [1, 2, 10, 11, 21, 22] extending existing $O(n)$-formulations for the evaluation of EOM. While $O(n)$-formulations are deemed computationally advantageous when dealing with large systems, formulating and evaluating the EOM in closed form remains an efficient alternative for many robotic systems and provides insights into the structure of the problem. Yet, such closed form formulations were not reported in the literature, with the exception of [13] where first-order time derivatives of the EOM were presented within the so-called spatial operator framework. A relatively recent research topic, where higher-order derivatives of the EOM are required, is the dynamic balancing of articulated mechanisms [28, 29, 4]. In [5], the time derivatives of the spatial momentum were used to derive global balancing conditions. Recently, we also proposed nth order time derivatives of EOM in both recursive and closed forms [32].

In this paper, the first and second time derivatives of the EOM are presented. For the subsequent treatment, the EOM are written in the form suitable for solving the inverse dynamics problem

$$Q = M(q)\ddot{q} + C(\dot{q}, q)\dot{q} + Q_{\text{grav}}(q) + Q_{\text{ext}}(q, t) \quad (1)$$

where the vector of generalized coordinates $q = (q_1, \dots, q_n)^T$ comprises the n joint variables, $M$ and $C$ is the generalized mass and Coriolis matrix, respectively, and $Q_{\text{grav}}$ represents generalized gravity forces. The generalized forces due to external loads (e.g. interaction/contact forces and torques) are summarized in $Q_{\text{ext}}$. Finally, $Q$ are the generalized forces (drive forces/torques) required for a prescribed motion $q(t)$.

In the following, the derivatives of (1) are derived using the Lie group formalism reported in [20], which is equivalent to those presented in [16] and [23]. A salient feature of the Lie group formulation is that it admits model description in terms of readily available data without compromising computational efficiency (of closed form expressions as well as $O(n)$ algorithms). As a side contribution, we also prove the structural properties of EOM from a closed form perspective. For sake of simplicity, and without loss of generality a single serial kinematic chain, comprising 1-DOF joints, mounted at the ground is considered. The generalizations to systems with arbitrary tree-topologies is straightforward, but will not be considered here in order to simplify notation and make the paper easily accessible.

*Organization:* Section 2 presents the equations of motion of serial kinematic chain in closed form using the body-fixed representation of the twists. Section 3 and Section 4 presents the first- and second-order time derivatives of the
equations of motion in closed form respectively. Section 5 proves the structural properties of the EOM using the closed form formulations. Section 6 presents the application of the proposed derivatives in evaluating second-order inverse dynamics of two exemplary robot manipulators and a discussion on its computational performance. Section 7 concludes the paper.

# 2 Equations of Motion in Closed Form

## 2.1 Kinematics in terms of joint screws

In the following, the notation and formulation of the EOM are adopted from [20]. The configuration (pose, posture) of body $i = 1, \dots, n$ is denoted $\mathbf{C}_i \in SE(3)$, which describes the frame transformation from a body-fixed frame (arbitrarily located at the body) to the inertial frame. Bodies and joints are numbered increasing order starting from the ground, so that joint $i$ links body $i$ to its predecessor $i-1$, while the ground is indexed with 0, and by convention $\mathbf{C}_0 = \mathbf{I}$. The relative configuration of body $j$ with respect to body $i$ is then

$$ \begin{aligned} \mathbf{C}_{i,j} &= \mathbf{C}_i^{-1} \mathbf{C}_j \\ &= \begin{pmatrix} \mathbf{R}_{ij} & {}^i\mathbf{r}_{i,j} \\ \mathbf{0} & 1 \end{pmatrix} \end{aligned} \quad (2) $$

where $\mathbf{R}_{ij} \in SO(3)$ is the rotation matrix transforming coordinates expressed in the reference frame on body $j$ to their expression in the reference frame on body $i$, and $ {}^i\mathbf{r}_{i,j} \in \mathbb{R}^3 $ is the position vector from the origin of frame $i$ to the origin of frame $j$ expressed in frame $i$. Without loss of generality, all joints are assumed to have 1-DOF. Denote with $q_i$ the joint variable (rotation angle, translation) of joint $i$. The configuration of body $i$ is determined by the product of exponential (POE) as

$$ \mathbf{C}_i(\mathbf{q}) = \mathbf{B}_1 \exp({}^1\mathbf{X}_1 q_1) \mathbf{B}_2 \exp({}^2\mathbf{X}_2 q_2) \cdots \mathbf{B}_i \exp({}^i\mathbf{X}_i q_i) \quad (3) $$

where $\mathbf{B}_i := \mathbf{C}_{i-1,i}(\mathbf{0}) \in SE(3)$ is the configuration of body $i$ relative to its predecessor $i-1$, in the reference configuration $\mathbf{q} = \mathbf{0}$, and $ {}^i\mathbf{X}_i $ is the screw coordinate vector associated to joint $i$ represented in the body-frame of body $i$ [19]. The vectors $ {}^i\mathbf{X}_i $ are constant due to the body-fixed representation. For 1-DOF lower-pair joints they are given as

$$ {}^i\mathbf{X}_i = \begin{pmatrix} {}^i\mathbf{e}_i \\ {}^i\mathbf{x}_i \times {}^i\mathbf{e}_i + {}^i\mathbf{e}_i h_i \end{pmatrix} := \begin{pmatrix} {}^i\xi_i \\ {}^i\eta_i \end{pmatrix} \quad (4) $$

where $ {}^i\mathbf{e}_i \in \mathbb{R}^3 $ is a unit vector along the joint axis, $ {}^i\mathbf{x}_i \in \mathbb{R}^3 $ is the vector to a point on this axis, and $ h_i \in \mathbb{R} $ is the pitch of the joint. In particular, for a revolute and prismatic joint, the screw coordinate vector is, respectively

$$ \text{Revolute : } {}^i\mathbf{X}_i = \begin{pmatrix} {}^i\mathbf{e}_i \\ {}^i\mathbf{x}_i \times {}^i\mathbf{e}_i \end{pmatrix}, \quad \text{Prismatic : } {}^i\mathbf{X}_i = \begin{pmatrix} \mathbf{0} \\ {}^i\mathbf{e}_i \end{pmatrix}. \quad (5) $$
In terms of these screw coordinates $\mathbf{X}$, the exponential map attains the explicit form [16, 18]

$$ \exp(\varphi \mathbf{X}) = \begin{pmatrix} \exp(\varphi \tilde{\mathbf{e}}) & (\mathbf{I} - \exp(\varphi \tilde{\mathbf{e}}))\mathbf{x} + \varphi h \mathbf{e} \\ \mathbf{0} & 1 \end{pmatrix}, \text{ for } h \neq \infty \quad (6) \\ = \begin{pmatrix} \mathbf{I} & \varphi \mathbf{e} \\ \mathbf{0} & 1 \end{pmatrix}, \text{ for } h = \infty $$

where the rotation matrix is determined by the Euler-Rodrigues formula

$$ \exp(\varphi \tilde{\mathbf{e}}) = \mathbf{I} + \sin \varphi \tilde{\mathbf{e}} + (1 - \cos \varphi) \tilde{\mathbf{e}}^2. \quad (7) $$

Denote with $\mathbf{V}_i^b = (\omega_i^b, \mathbf{v}_i^b)^T$ the twist of body $i$ represented in the body-fixed frame. The superscript $b$ is used to indicate the body-fixed representation [20, 23]. An alternative representation is so-called spatial representation of twist, which would be indicated by a superscript $s$. A recursive $O(n)$-algorithm for evaluating the EOM and their higher-order derivatives using the spatial representation was reported in [22]. In [14] the EOM were presented in closed form in terms of the spatial representation of twists. The potential advantage of using the spatial representation to express the EOM in closed form remains to be explored, however. In this paper the (classical) body-fixed representation is used.

The individual twists of all bodies are summarized in the vector $\mathbf{V} \in \mathbb{R}^{6n}$, which is referred to as the system twist in body-fixed representation. It is determined as

$$ \mathbf{V} = \mathbf{J} \dot{\mathbf{q}} \quad (8) $$

with the system Jacobian $\mathbf{J}(\mathbf{q})$. The latter admits the factorization

$$ \mathbf{J} = \mathbf{A}\mathbf{X} \quad (9) $$

in terms of the block-triangular and block-diagonal matrices

$$ \mathbf{A}(\mathbf{q}) = \begin{pmatrix} \mathbf{I} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{Ad}_{\mathbf{C}_{2,1}} & \mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{Ad}_{\mathbf{C}_{3,1}} & \mathbf{Ad}_{\mathbf{C}_{3,2}} & \mathbf{I} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \ddots & \vdots \\ \mathbf{Ad}_{\mathbf{C}_{n,1}} & \mathbf{Ad}_{\mathbf{C}_{n,2}} & \cdots & \mathbf{Ad}_{\mathbf{C}_{n,n-1}} & \mathbf{I} \end{pmatrix}, \mathbf{X} = \begin{pmatrix} \mathbf{1}\mathbf{X}_1 & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{2}\mathbf{X}_2 & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{3}\mathbf{X}_3 & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \ddots & \vdots \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{n}\mathbf{X}_n \end{pmatrix}. \quad (10) $$

The matrix $\mathbf{Ad}_{\mathbf{C}_{i,j}}$ transforms screw coordinates represented in the reference frame at body $j$ to those represented in the frame on body $i$ [16, 23, 26]. With the relative configuration (2) this matrix is

$$ \mathbf{Ad}_{\mathbf{C}_{i,j}} = \begin{pmatrix} \mathbf{R}_{ij} & \mathbf{0} \\ i \tilde{\mathbf{r}}_{i,j} & \mathbf{R}_{ij} \end{pmatrix} \quad (11) $$

where $\tilde{\mathbf{x}} \in \mathbf{so}(3)$ is the skew-symmetric matrix associated to vector $\mathbf{x} \in \mathbb{R}^3$ so that $\tilde{\mathbf{y}} = \mathbf{x} \times \mathbf{y}$. A central relation for deriving the EOM in closed form is
the following expression for the time derivative of the matrix A and thus of the system Jacobian [20]

$$ \dot{J}(\mathbf{q}, \dot{\mathbf{q}}) = -A(\mathbf{q})a(\dot{\mathbf{q}})J(\mathbf{q}) \quad (12) $$

where

$$ \mathbf{a}(\dot{\mathbf{q}}) := \mathrm{diag}(\dot{q}_1 \mathbf{a} \mathbf{d}_1 \mathbf{x}_1, \dots, \dot{q}_n \mathbf{a} \mathbf{d}_n \mathbf{x}_n). \quad (13) $$

Therein, the matrix $\mathbf{a} \mathbf{d}_i \mathbf{x}_i$ is given in terms of the joint screw coordinate vector (4) as

$$ \mathbf{a} \mathbf{d}_i \mathbf{x}_i = \begin{pmatrix} i \tilde{\xi}_i & \mathbf{0} \\ i \tilde{\eta}_i & i \tilde{\xi}_i \end{pmatrix}. \quad (14) $$

This gives rise to the closed form expressions for the system acceleration

$$ \dot{\mathbf{V}} = \mathbf{J}\ddot{\mathbf{q}} - \mathbf{A}\mathbf{J}\dot{\mathbf{q}} = \mathbf{J}\ddot{\mathbf{q}} - \mathbf{A}\mathbf{V}. \quad (15) $$

For calculating the derivatives, the time derivative of matrix A will be needed. To this end, the expression

$$ A = (\mathbf{I} - \mathbf{D})^{-1} \quad (16) $$

is used [20], with the matrix

$$ \mathbf{D}(\mathbf{q}) = \begin{pmatrix} \mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{A}\mathbf{d}_{C_{2,1}} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{0} & \mathbf{A}\mathbf{d}_{C_{3,2}} & \mathbf{0} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \ddots & \vdots \\ \mathbf{0} & \mathbf{0} & \cdots & \mathbf{A}\mathbf{d}_{C_{n,n-1}} & \mathbf{0} \end{pmatrix}. \quad (17) $$

With (16), the derivative of A is then

$$ \dot{\mathbf{A}} = (\mathbf{I} - \mathbf{D})^{-1} \dot{\mathbf{D}} (\mathbf{I} - \mathbf{D})^{-1} = \mathbf{A} \dot{\mathbf{D}} \mathbf{A}. \quad (18) $$

Using the relation $\dot{\mathbf{A}} \mathbf{d}_{C_{i,j}} = -\dot{q}_i \mathbf{a} \mathbf{d}_i \mathbf{x}_j \mathbf{A} \mathbf{d}_{C_{i,-1}}$ [20], the derivative of D attains the closed form $\dot{\mathbf{D}} = -\mathbf{a} \mathbf{D}$. Finally, with $\mathbf{D} = \mathbf{I} - \mathbf{A}^{-1}$, it follows

$$ \dot{\mathbf{A}}(\mathbf{q}, \dot{\mathbf{q}}) = \mathbf{A}(\mathbf{q}) \mathbf{a} - \mathbf{A}(\mathbf{q}) \mathbf{a}(\dot{\mathbf{q}}) \mathbf{A}(\mathbf{q}). \quad (19) $$

Clearly, the derivative (12) of the system Jacobian is recovered as $\dot{\mathbf{J}} = \dot{\mathbf{A}} \mathbf{X}$ noting that $\mathbf{a} \mathbf{X} \equiv \mathbf{0}$.

## 2.2 Equations of Motion

The generalized mass and Coriolis matrix in the EOM (1) of a simple kinematic chain mounted at the ground are found via Jourdain's principle of virtual power as (or likewise as the Lagrange equations) [20]

$$ \mathbf{M}(\mathbf{q}) = \mathbf{J}^T \mathbf{M} \mathbf{J}, \quad \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) = \mathbf{J}^T \mathbf{C} \mathbf{J} \quad (20) $$
where

$$ M := \operatorname{diag}(M_1, \dots, M_n) \qquad (21) $$

$$ C(\mathbf{q}, \dot{\mathbf{q}}, V(\dot{\mathbf{q}})) := -MAa - b^T M. \qquad (22) $$

and

$$ b(V) := \operatorname{diag}(\mathbf{a}\mathbf{d}_{V_1}, \dots, \mathbf{a}\mathbf{d}_{V_n}). \qquad (23) $$

Therein, the (constant) 6×6 inertia matrix of body i expressed in the body-frame is defined as

$$ M_i = \begin{pmatrix} \mathbf{\Theta}_i^b & i \tilde{\mathbf{c}}_i m_i \\ -i \tilde{\mathbf{c}}_i m_i & m_i \mathbf{I} \end{pmatrix} \qquad (24) $$

where $m_i$ is the body mass, $i\mathbf{c}_i$ is the position vector to the COM of body i measured in the reference frame at body i, and $\mathbf{\Theta}_i^b$ is the inertia tensor with respect to the body-fixed frame. The latter is related to the inertia tensor with respect to the COM, denoted $\mathbf{\Theta}_i^c$, via the Steiner's theorem $\mathbf{\Theta}_i^b = \mathbf{R}_{b,c}\mathbf{\Theta}_i^c\mathbf{R}_{b,c}^T - m_i\tilde{\mathbf{c}}_i\tilde{\mathbf{c}}_i^T$ where $\mathbf{R}_{b,c}$ denotes the rotation matrix of the COM frame to the body frame.

A closed form of the EOM is obtained after replacing the system twist by (8). Alternatively, first the kinematic relation (8) and then the coefficient matrices in (22) are evaluated for a given state $\mathbf{q}, \dot{\mathbf{q}}$. The generalized gravity forces are given as

$$ \mathbf{Q}_{\text{grav}}(\mathbf{q}) = \mathcal{J}^T \text{MUG} \qquad (25) $$

with

$$ \mathbf{G} := - \begin{pmatrix} \mathbf{0} \\ \mathbf{0}_{\text{g}} \end{pmatrix}, \quad U(\mathbf{q}) := A \begin{pmatrix} \mathbf{A}\mathbf{d}_{C_1}^{-1} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{pmatrix} = \begin{pmatrix} \mathbf{A}\mathbf{d}_{C_1}^{-1} \\ \mathbf{A}\mathbf{d}_{C_2}^{-1} \\ \vdots \\ \mathbf{A}\mathbf{d}_{C_n}^{-1} \end{pmatrix}. \qquad (26) $$

Here, $\mathbf{0}_{\text{g}}$ is the vector of gravitational acceleration expressed in the inertial frame, which is transformed to the individual bodies by $U$. The effect of contact or external wrenches acting on the bodies is given by the generalized forces

$$ \mathbf{Q}_{\text{ext}}(\mathbf{q}, t) = \mathcal{J}^T(\mathbf{q}) \mathbf{W}_{\text{EE}}(t) \quad \text{where} \quad \mathbf{W}_{\text{ext}}(t) = \begin{pmatrix} \mathbf{W}_{\text{ext},1}(t) \\ \mathbf{W}_{\text{ext},2}(t) \\ \vdots \\ \mathbf{W}_{\text{ext},n}(t) \end{pmatrix}. \qquad (27) $$

The vector $\mathbf{W}_{\text{ext},i}$ accounts for applied load at body i. For instance, $\mathbf{W}_{\text{ext},n}$ can be used to describe the wrench at the end-effector of a robotic arm. More generally, $\mathbf{W}_{\text{ext}}$ and thus $\mathbf{Q}_{\text{ext}}$ may account for arbitrary (time and velocity dependent) loads at the system.
### 3 First Time Derivative of the Equations of Motion

The first time derivative of the generalized forces is

$$ \dot{\mathbf{Q}} = \mathbf{M}\ddot{\mathbf{q}} + (\dot{\mathbf{M}} + \mathbf{C})\ddot{\mathbf{q}} + \dot{\mathbf{C}}\dot{\mathbf{q}} + \dot{\mathbf{Q}}_{\text{grav}} + \dot{\mathbf{Q}}_{\text{ext}}. \quad (28) $$

The time derivative of the generalized mass matrix $\mathbf{M}$ in (20) follows with $\dot{j}$ in (12) as

$$ \begin{aligned} \dot{\mathbf{M}}(\mathbf{q}, \dot{\mathbf{q}}) &= \mathbf{J}^T \mathbf{M} \dot{j} + \dot{j}^T \mathbf{M} \mathbf{J} \\ &= \mathbf{J}^T \mathbf{M}^{(1)} \mathbf{J} \end{aligned} \quad (29) $$

with

$$ \mathbf{M}^{(1)}(\mathbf{q}, \dot{\mathbf{q}}) := -\mathbf{M}\mathbf{A}\mathbf{a} - (\mathbf{M}\mathbf{A}\mathbf{a})^T \quad (30) $$

where $\mathbf{A} = \mathbf{A}(\mathbf{q})$ and $\mathbf{a} = \mathbf{a}(\dot{\mathbf{q}})$. By the same token, the time derivative of the generalized Coriolis matrix $\mathbf{C}$ is

$$ \begin{aligned} \dot{\mathbf{C}}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}) &= \mathbf{J}^T \mathbf{C} \dot{j} + \dot{j}^T \mathbf{C} \mathbf{J} + \mathbf{J}^T \dot{\mathbf{C}} \mathbf{J} \\ &= \mathbf{J}^T \mathbf{C}^{(1)} \mathbf{J} \end{aligned} \quad (31) $$

where now

$$ \mathbf{C}^{(1)} := \dot{\mathbf{C}} - \mathbf{C}\mathbf{A}\mathbf{a} - \mathbf{a}^T \mathbf{A}^T \mathbf{C}. \quad (32) $$

The expression for $\mathbf{C}$ in (22) together with (12) yields

$$ \dot{\mathbf{C}}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}, \dot{\mathbf{V}}) = \mathbf{C}(\mathbf{q}, \ddot{\mathbf{q}}, \dot{\mathbf{V}}) + \mathbf{M}\mathbf{A}\mathbf{a}\mathbf{a} - \mathbf{M}\mathbf{A}\mathbf{a}\mathbf{a} = \mathbf{M}\mathbf{A}\mathbf{a}\mathbf{a} - \mathbf{M}\mathbf{A}\mathbf{a}\mathbf{a} - \mathbf{M}\mathbf{A}\dot{\mathbf{a}} - \dot{\mathbf{b}}^T \mathbf{M}, \quad (33) $$

with $\dot{\mathbf{a}} = \dot{\mathbf{a}}(\ddot{\mathbf{q}})$, $\dot{\mathbf{b}} = \dot{\mathbf{b}}(\dot{\mathbf{V}})$, and thus

$$ \begin{aligned} \mathbf{C}^{(1)} &= \mathbf{C}(\mathbf{q}, \ddot{\mathbf{q}}, \dot{\mathbf{V}}) + \mathbf{M}\mathbf{A}\mathbf{a}\mathbf{a} - \mathbf{M}\mathbf{A}\mathbf{a}\mathbf{a} - \mathbf{C}\mathbf{A}\mathbf{a} - \mathbf{a}^T \mathbf{A}^T \mathbf{C} \\ &= -\mathbf{M}\mathbf{A}\dot{\mathbf{a}} - \dot{\mathbf{b}}^T \mathbf{M} + \mathbf{b}^T \mathbf{M}\mathbf{A}\mathbf{a} + \mathbf{a}^T \mathbf{A}^T \mathbf{b}^T \mathbf{M} + \mathbf{a}^T \mathbf{A}^T \mathbf{M}\mathbf{A}\mathbf{a} + 2\mathbf{M}\mathbf{A}\mathbf{a}\mathbf{a} - \mathbf{M}\mathbf{A}\mathbf{a}\mathbf{a}. \end{aligned} \quad (34) $$

The form (34) allows reusing the expression (22) for the matrix $\mathbf{C}$, where (22) is evaluated with $\ddot{\mathbf{q}}, \dot{\mathbf{V}}$ instead with the velocities. Also in the second form (35), terms like $\mathbf{A}\mathbf{a}, \mathbf{M}\mathbf{A}\mathbf{a}$, and $\mathbf{b}^T \mathbf{M}$ can be reused. A direct calculation yields

$$ \dot{\mathbf{Q}}_{\text{grav}}(\mathbf{q}, \dot{\mathbf{q}}) = \mathbf{J}^T \mathbf{M}^{(1)} \mathbf{U} \mathbf{G} \quad (36) $$

where the relation $\dot{\mathbf{U}} = -\mathbf{A}\mathbf{a}\mathbf{U}$ (obtained by time differentiation of (26) and using the identity in (19)) and the fact that $\mathbf{G}$ is constant has been used. The time derivative of (27) along with (12) yields the expression for calculating first-order derivative of generalized forces due to external wrenches

$$ \dot{\mathbf{Q}}_{\text{ext}}(\mathbf{q}, \dot{\mathbf{q}}) = \mathbf{J}^T(\dot{\mathbf{W}}_{\text{EE}} - (\mathbf{A}\mathbf{a})^T \mathbf{W}_{\text{EE}}). \quad (37) $$
## 4 Second Time Derivative of the Equations of Motion

The second time derivative of the generalized motor forces (1) is determined by

$$ \ddot{\mathbf{Q}} = \mathbf{M}\ddot{\mathbf{q}} + (2\dot{\mathbf{M}} + \mathbf{C})\ddot{\mathbf{q}} + (\ddot{\mathbf{M}} + 2\dot{\mathbf{C}})\ddot{\mathbf{q}} + \ddot{\mathbf{C}}\dot{\mathbf{q}} + \ddot{\mathbf{Q}}_{\text{grav}} + \ddot{\mathbf{Q}}_{\text{ext}}. \quad (38) $$

The second time derivative of the generalized mass matrix is

$$ \ddot{\mathbf{M}}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}) = \mathbf{J}^T(\dot{\mathbf{M}}^{(1)} - \mathbf{M}^{(1)}\mathbf{A}\mathbf{a} - \mathbf{a}^T\mathbf{A}^T\mathbf{M}^{(1)})\mathbf{J} \quad (39) $$

$$ = \mathbf{J}^T \mathbf{M}^{(2)} \mathbf{J} \quad (40) $$

with $\mathbf{M}^{(2)} := \dot{\mathbf{M}}^{(1)} - \mathbf{M}^{(1)}\mathbf{A}\mathbf{a} - \mathbf{a}^T\mathbf{A}^T\mathbf{M}^{(1)}$. Inserting the relation (19) in the differentiation of (30) yields

$$ \dot{\mathbf{M}}^{(1)}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}) = \mathbf{M}^{(1)}(\mathbf{q}, \ddot{\mathbf{q}}) + \mathbf{M}(\mathbf{A}\mathbf{a}\mathbf{A} - \mathbf{A}\mathbf{a})\mathbf{a} + (\mathbf{M}(\mathbf{A}\mathbf{a}\mathbf{A} - \mathbf{A}\mathbf{a})\mathbf{a})^T \quad (41) $$

$$ = \mathbf{M}^{(1)}(\mathbf{q}, \ddot{\mathbf{q}}) - \mathbf{M}\mathbf{A}\mathbf{a} + \mathbf{M}\mathbf{A}\mathbf{a}\mathbf{A} - (\mathbf{M}\mathbf{A}\mathbf{a})^T + (\mathbf{M}\mathbf{A}\mathbf{a}\mathbf{A})^T \quad (42) $$

which leads immediately to the explicit form

$$ \mathbf{M}^{(2)} = \mathbf{M}^{(1)}(\mathbf{q}, \dot{\mathbf{q}}) + \mathbf{M}(\mathbf{A}\mathbf{a}\mathbf{A} - \mathbf{A}\mathbf{a})\mathbf{a} + (\mathbf{M}(\mathbf{A}\mathbf{a}\mathbf{A} - \mathbf{A}\mathbf{a})\mathbf{a})^T - \mathbf{M}^{(1)}(\mathbf{q}, \dot{\mathbf{q}})\mathbf{A}\mathbf{a} - (\mathbf{M}^{(1)}(\mathbf{q}, \dot{\mathbf{q}})\mathbf{A}\mathbf{a})^T \quad (43) $$

$$ = -\mathbf{M}\dot{\mathbf{A}} - (\mathbf{M}\dot{\mathbf{A}}\dot{\mathbf{a}})^T + 2\mathbf{M}\mathbf{A}\mathbf{a}\mathbf{A} + 2(\mathbf{M}\mathbf{A}\mathbf{a}\mathbf{A})^T + 2\mathbf{a}^T\mathbf{A}^T\mathbf{M}\mathbf{A}\mathbf{a} \quad (44) $$

$$ -\mathbf{M}\mathbf{A}\mathbf{a} - (\mathbf{M}\mathbf{A}\mathbf{a})^T. $$

The expression (43) allows reusing $\mathbf{M}^{(1)}(\mathbf{q}, \dot{\mathbf{q}})$. It should be observed that the term $\mathbf{M}^{(1)}(\mathbf{q}, \ddot{\mathbf{q}})$ is the relation (30) evaluated with $\ddot{\mathbf{q}}$ instead of $\dot{\mathbf{q}}$.

Repeated time derivative of the generalized Coriolis matrix yields

$$ \ddot{\mathbf{C}}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}, \dddot{\mathbf{q}}) = \mathbf{J}^T \mathbf{C}^{(2)} \mathbf{J} \quad (45) $$

with

$$ \mathbf{C}^{(2)}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}, \dddot{\mathbf{q}}) = \dot{\mathbf{C}}^{(1)} - \mathbf{C}^{(1)}\mathbf{A}\mathbf{a} - \mathbf{a}^T\mathbf{A}^T\mathbf{C}^{(1)} \quad (46) $$

Taking the derivative of (32) yields $\ddot{\mathbf{C}}^{(1)}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}, \dddot{\mathbf{q}}, \dot{\mathbf{V}}, \ddot{\mathbf{V}})$ in closed form as

$$ \begin{aligned} \dot{\mathbf{C}}^{(1)} &= \ddot{\mathbf{C}} - \dot{\mathbf{C}}\mathbf{A}\mathbf{a} - \mathbf{a}^T\mathbf{A}^T\dot{\mathbf{C}} - \mathbf{C}\dot{\mathbf{A}}\dot{\mathbf{a}} - \dot{\mathbf{a}}^T\mathbf{A}^T\mathbf{C} + \mathbf{C}\mathbf{A}\mathbf{a}\mathbf{A} + \mathbf{a}^T\mathbf{A}^T\mathbf{a}^T\mathbf{A}^T\mathbf{C} \\ &\quad - \mathbf{C}\mathbf{A}\mathbf{a}\mathbf{a} - \mathbf{a}^T\mathbf{a}^T\mathbf{A}^T\mathbf{C} \\ &= \ddot{\mathbf{C}} - (\mathbf{C}^{(1)} + \mathbf{a}^T\mathbf{A}^T\mathbf{C})\mathbf{A}\mathbf{a} - \mathbf{a}^T\mathbf{A}^T(\mathbf{C}^{(1)} + \mathbf{C}\mathbf{A}\mathbf{a}) - \mathbf{C}\dot{\mathbf{A}}\dot{\mathbf{a}} - \dot{\mathbf{a}}^T\mathbf{A}^T\mathbf{C} \end{aligned} \quad (47) $$

This requires the derivative of (33), which are found as

$$ \begin{aligned} \ddot{\mathbf{C}} &= \mathbf{C}(\mathbf{q}, \ddot{\mathbf{q}}, \ddot{\mathbf{V}}) + \mathbf{M}\dot{\mathbf{A}}\mathbf{a}\mathbf{A} + 2\mathbf{M}\mathbf{A}\mathbf{a}\dot{\mathbf{A}} - 2\mathbf{M}\mathbf{A}\mathbf{a}\mathbf{A}\mathbf{a} + 2\mathbf{M}\mathbf{A}\mathbf{a}\mathbf{A}\mathbf{a} - 3\mathbf{M}\mathbf{A}\dot{\mathbf{a}}\dot{\mathbf{a}} \quad (48) \\ &\quad - \mathbf{M}\mathbf{A}\mathbf{a}\mathbf{a} + \mathbf{M}\mathbf{A}\mathbf{a}\mathbf{A} \end{aligned} $$

$$ \begin{aligned} &= -\mathbf{M}\ddot{\mathbf{A}} - \ddot{\mathbf{b}}^T\mathbf{M} + \mathbf{M}\dot{\mathbf{A}}\dot{\mathbf{a}}\mathbf{A} + 2\mathbf{M}\mathbf{A}\dot{\mathbf{a}}\dot{\mathbf{A}} - 2\mathbf{M}\mathbf{A}\mathbf{a}\mathbf{A}\mathbf{a} + 2\mathbf{M}\mathbf{A}\mathbf{a}\mathbf{A}\mathbf{a} \quad (49) \\ &\quad - 3\mathbf{M}\mathbf{A}\dot{\mathbf{a}}\dot{\mathbf{a}} - \mathbf{M}\mathbf{A}\mathbf{a}\mathbf{a} + \mathbf{M}\mathbf{A}\mathbf{a}\mathbf{A}\mathbf{a}. \end{aligned} $$
Also here it should be observed that the matrix $C(q, \ddot{q}, \dddot{V})$ is the expression for $C$ in (22) evaluated with $\ddot{q}, \ddot{V}$. Inserting (48) along with (49) into (46) yields $C^{(2)}(q, \dot{q}, \ddot{q}, \dddot{q})$ in the explicit form

$$ 
\begin{aligned}
C^{(2)} = C(q, \ddot{q}, \dddot{V}) & - 2C^{(1)}Aa - 2a^T A^T C^{(1)} - CA\dot{a} - \dot{a}^T A^T C - 2a^T A^T CAa \\
& - CAaa - a^T a^T A^T C + MA\dot{a}Aa + 2MAaA\dot{a} - 2MAaAaAa \quad (50) \\
& + 2MAaAaa - 3MAa\dot{a} - MAaaa + MAaaAa \\
& = -MA\ddot{a} - \ddot{b}^T M + 2\dot{b}^T MAa + 2a^T A^T \dot{b}^T M \\
& + 3MA\dot{a}Aa + 2MAaA\dot{a} + 2a^T A^T MA\dot{a} - CA\dot{a} - \dot{a}^T A^T C \\
& + 2CAaAa + 2a^T A^T CAa + 2a^T A^T a^T A^T C - 4MAaAaAa \\
& - 2a^T A^T MAaAa + 2MAaaAa + 2a^T A^T MAaa \quad (51) \\
& + 2MAaAaa - 3MAa\dot{a} - MAaaa + MAaaAa - CAaa - a^T a^T A^T C
\end{aligned}
$$

with $\ddot{a} = \ddot{a}(\ddot{q})$ and $\ddot{b} = \ddot{b}(\ddot{V})$ in (13) and (23) evaluated with $\ddot{q}$ and $\ddot{V}$, respectively. The first form (51) may be beneficial since it involves $C$ and $C^{(1)}$, which are already available from (22) and (34).

Taking the derivative of (36) yields the second time derivative of the generalized gravity forces

$$ \ddot{Q}_{\text{grav}}(q, \dot{q}, \ddot{q}) = J^T M^{(2)} U G. \quad (52) $$

The second time derivative of the generalized forces (27) due to end-effector loads is readily found as

$$ 
\begin{aligned}
\ddot{Q}_{\text{ext}}(q, \dot{q}, \ddot{q}) = J^T (\ddot{W}_{\text{EE}} - 2(Aa)^T \dot{W}_{\text{EE}} + (2(AaAa)^T - (A\dot{a})^T) \\
- (Aaa)^T) W_{\text{EE}}.
\end{aligned}
\quad (53)
$$

# 5 Structural Properties of the EOM

There are two important and well-known structural properties of EOM:

1. The generalized mass matrix $M$ is symmetric and positive definite.

2. $\dot{q}^T (\dot{M}(q, \dot{q}) - 2C(q, \dot{q})) \dot{q} = 0$ for any $\dot{q}$

Positive definiteness and the symmetric properties of the generalized mass matrix follows directly from its definition in (20). The symmetry property indeed applies to all higher derivatives, which is also evident from (29) and (40). An important property of the EOM (1), which is crucial for proving stability of passivity-based control schemes, is that the Coriolis matrix can be formulated as

$$ \bar{C} := -J^T(MAa + b^T M - Mb)J \quad (54) $$

so that $\dot{M} - 2\bar{C}$ is skew symmetric. To show this property, notice that $bJ\dot{q} = bV = \text{diag}(ad_{V_1}V_1, \dots, ad_{V_n}V_n) = 0$, so that $C\dot{q}$ can be rewritten as

$$ C\dot{q} = -J^T(MAa + b^T M)J\dot{q} = -J^T(MAa + b^T M - Mb)J\dot{q} = \bar{C}\dot{q} \quad (55) $$
In view of the time derivative (29) of M, the so defined Coriolis matrix $\bar{C}$ satisfies the relation

$$ \bar{C} + \bar{C}^T = -J^T(MAa + a^T A^T M)J = \dot{M}. \quad (56) $$

With (54), it hence follows

$$ \dot{M} - 2\bar{C} = \bar{C}^T - \bar{C} = -(\bar{C}^T - \bar{C})^T = -(\dot{M} - 2\bar{C})^T. \quad (57) $$

It should be remarked that the Coriolis matrix is not unique, and the property 2) holds true for the particular arrangement as in (22). It may not hold if the equations are arranged differently. The skew symmetry of $\dot{M} - 2\bar{C}$ is indeed carried over to its derivatives.

# 6 Examples

The higher-order closed-form inverse dynamics formulation presented in Section 3 and Section 4 were implemented in MATLAB<fnref n="3" />. This MATLAB implementation can also be used for efficient symbolic code generation in MATLAB and C languages. This section presents the application of this work to the computation of higher-order inverse dynamics of two robot manipulators and presents a discussion on its computational efficiency.

## 6.1 Planar 2R robot

A 2R serial chain consists of a base, two links, and two revolute joints. A simple 2R chain is shown in Figure 1. First link of length $L_1$ is connected to the base or ground through a revolute joint. The second link of length $L_2$ is connected to first link through a revolute joint. The center of mass is shown with a red circle on the links and lies at the end of each link. The mass of first and second links are $m_1$ and $m_2$ respectively. The gravity acting on the system is shown as $g$ in the figure.

**Kinematic model** The z-axes represent the joint axes, and thus the joint screw coordinates in body-fixed representation for the two revolute joints are

$$ ^1\mathbf{X}_1 = (0, 0, 1, 0, 0, 0)^T $$

$$ ^2\mathbf{X}_2 = (0, 0, 1, 0, 0, 0)^T. $$

The relative reference configurations for the two bodies are

$$ \mathbf{B}_1 = \begin{pmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix}, \quad \mathbf{B}_2 = \begin{pmatrix} 1 & 0 & 0 & L_1 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix}. $$

<footnote n="3">³ The source code as well as robot data are openly available at https://github.com/shivesh1210/2nd_order_closed_form_time_derivatives_eom</footnote>
<figure><img src="image_2.png" /><figcaption>Fig. 1: Planar 2R robot schematic</figcaption></figure>

The mass matrices of the two bodies in the body fixed configuration according to (24) are

$$ \mathbf{M}_1 = \begin{pmatrix} L_1^2 m_1 & 0 & 0 & 0 & 0 & 0 \\ 0 & L_1^2 m_1 & 0 & 0 & 0 & -L_1 m_1 \\ 0 & 0 & L_1^2 m_1 & 0 & L_1 m_1 & 0 \\ 0 & 0 & 0 & m_1 & 0 & 0 \\ 0 & 0 & L_1 m_1 & 0 & m_1 & 0 \\ 0 & -L_1 m_1 & 0 & 0 & 0 & m_1 \end{pmatrix}, $$

$$ \mathbf{M}_2 = \begin{pmatrix} L_2^2 m_2 & 0 & 0 & 0 & 0 & 0 \\ 0 & L_2^2 m_2 & 0 & 0 & 0 & -L_2 m_2 \\ 0 & 0 & L_2^2 m_2 & 0 & L_2 m_2 & 0 \\ 0 & 0 & 0 & m_2 & 0 & 0 \\ 0 & 0 & L_2 m_2 & 0 & m_2 & 0 \\ 0 & -L_2 m_2 & 0 & 0 & 0 & m_2 \end{pmatrix}. $$

**Second-order inverse dynamics** With the above information about the robot, using the closed form expressions of $\mathbf{M}(\mathbf{q})$, $\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})$ and $\mathbf{Q}_{\text{grav}}(\mathbf{q})$ provided in Section 2.2 in (1), one can arrive at the analytical expression for generalized forces $(\tau_1, \tau_2)$ which solves inverse dynamics problem as follows

$$ \tau_1 = L_1^2 \ddot{q}_1 m_1 + L_1^2 \ddot{q}_1 m_2 + L_2^2 \ddot{q}_1 m_2 + L_2^2 \ddot{q}_2 m_2 + L_2 g m_2 \cos(q_1 + q_2) \\ + L_1 g m_1 \cos(q_1) + L_1 g m_2 \cos(q_1) - L_1 L_2 \ddot{q}_2^2 m_2 \sin(q_2) \\ + 2 L_1 L_2 \ddot{q}_1 m_2 \cos(q_2) + L_1 L_2 \ddot{q}_2 m_2 \cos(q_2) - 2 L_1 L_2 \dot{q}_1 \dot{q}_2 m_2 \sin(q_2) $$

$$ \tau_2 = L_2 m_2 (L_1 \sin(q_2) \ddot{q}_1^2 + L_2 \ddot{q}_1 + L_2 \ddot{q}_2 + g \cos(q_1 + q_2) + L_1 \ddot{q}_1 \cos(q_2)). $$

These expressions are indeed identical to those reported in [16] for instance.

Using the closed form expressions of $\dot{\mathbf{M}}(\mathbf{q}, \dot{\mathbf{q}})$, $\dot{\mathbf{C}}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}})$ and $\dot{\mathbf{Q}}_{\text{grav}}(\mathbf{q}, \dot{\mathbf{q}})$ provided in Section 3 in (28), one obtains the analytical expression for first-order
time derivatives ($\dot{\tau}_1, \dot{\tau}_2$) of the generalized forces, which solves the first-order inverse dynamics problem, as follows

$$ 
\begin{aligned}
\dot{\tau}_1 = & L_1^2 \ddot{q}_1 m_1 + L_1^2 \ddot{q}_1 m_2 + L_2^2 \ddot{q}_1 m_2 + L_2^2 \ddot{q}_2 m_2 - L_2 \dot{q}_1 g m_2 \sin(q_1 + q_2) \\
& - L_2 \dot{q}_2 g m_2 \sin(q_1 + q_2) + 2 L_1 L_2 \ddot{q}_1 m_2 \cos(q_2) + L_1 L_2 \ddot{q}_2 m_2 \cos(q_2) \\
& - L_1 \dot{q}_1 g m_1 \sin(q_1) - L_1 \dot{q}_1 g m_2 \sin(q_1) - L_1 L_2 \dot{q}_2^2 m_2 \cos(q_2) \\
& - 4 L_1 L_2 \ddot{q}_1 \dot{q}_2 m_2 \sin(q_2) - 2 L_1 L_2 \ddot{q}_2 \dot{q}_1 m_2 \sin(q_2) \\
& - 3 L_1 L_2 \ddot{q}_2 \dot{q}_2 m_2 \sin(q_2) - 2 L_1 L_2 \dot{q}_1 \dot{q}_2^2 m_2 \cos(q_2)
\end{aligned}
$$

$$ 
\begin{aligned}
\dot{\tau}_2 = & L_2 m_2 (L_2 \ddot{q}_1 + L_2 \ddot{q}_2 - \dot{q}_1 g \sin(q_1 + q_2) - \dot{q}_2 g \sin(q_1 + q_2) \\
& + L_1 \ddot{q}_1 \cos(q_2) + 2 L_1 \ddot{q}_1 \dot{q}_1 \sin(q_2) - L_1 \ddot{q}_1 \dot{q}_2 \sin(q_2) + L_1 \dot{q}_1^2 \dot{q}_2 \cos(q_2)).
\end{aligned}
$$

The correctness can be easily checked taking the analytical time derivative of ($\tau_1, \tau_2$) above.

Using the closed form expressions of $\ddot{M}(q, \dot{q}, \ddot{q})$, $\ddot{C}(q, \dot{q}, \ddot{q}, \dddot{q})$ and $\ddot{Q}_{grav}(q, \dot{q}, \ddot{q})$ provided in Section 4 in (38), one obtains the analytical expression for second-order time derivatives of generalized forces ($\ddot{\tau}_1, \ddot{\tau}_2$), solving the second-order inverse dynamics problem:

$$ 
\begin{aligned}
\ddot{\tau}_1 = & L_1^2 \ddot{q}_1 m_1 + L_1^2 \ddot{q}_1 m_2 + L_2^2 \ddot{q}_1 m_2 + L_2^2 \ddot{q}_2 m_2 - 3 L_1 L_2 \ddot{q}_2^2 m_2 \sin(q_2) \\
& + L_1 L_2 \dot{q}_2^4 m_2 \sin(q_2) - L_1 \dot{q}_1^2 g m_1 \cos(q_1) - L_1 \dot{q}_1^2 g m_2 \cos(q_1) \\
& - L_2 \ddot{q}_1 g m_2 \sin(q_1 + q_2) - L_2 \ddot{q}_2 g m_2 \sin(q_1 + q_2) + 2 L_1 L_2 \ddot{q}_1 m_2 \cos(q_2) \\
& + L_1 L_2 \ddot{q}_2 m_2 \cos(q_2) - L_1 \ddot{q}_1 g m_1 \sin(q_1) - L_1 \ddot{q}_1 g m_2 \sin(q_1) \\
& - L_2 \dot{q}_1^2 g m_2 \cos(q_1 + q_2) - L_2 \dot{q}_2^2 g m_2 \cos(q_1 + q_2) - 6 L_1 L_2 \ddot{q}_1 \ddot{q}_2 m_2 \sin(q_2) \\
& - 6 L_1 L_2 \ddot{q}_1 \dot{q}_2 m_2 \sin(q_2) - 2 L_1 L_2 \ddot{q}_2 \dot{q}_1 m_2 \sin(q_2) - 4 L_1 L_2 \ddot{q}_2 \dot{q}_2 m_2 \sin(q_2) \\
& - 6 L_1 L_2 \ddot{q}_1 \dot{q}_2^2 m_2 \cos(q_2) - 6 L_1 L_2 \ddot{q}_2 \dot{q}_1^2 m_2 \cos(q_2) + 2 L_1 L_2 \dot{q}_1 \dot{q}_2^3 m_2 \sin(q_2) \\
& - 2 L_2 \dot{q}_1 \dot{q}_2 g m_2 \cos(q_1 + q_2) - 6 L_1 L_2 \ddot{q}_1 \dot{q}_2 \dot{q}_1 \dot{q}_2 m_2 \cos(q_2)
\end{aligned}
$$

$$ 
\begin{aligned}
\ddot{\tau}_2 = & -L_2 m_2 (\dot{q}_1^2 g \cos(q_1 + q_2) - L_2 \ddot{q}_2 - L_2 \ddot{q}_1 + \dot{q}_2^2 g \cos(q_1 + q_2) \\
& - 2 L_1 \ddot{q}_1^2 \sin(q_2) + \ddot{q}_1 g \sin(q_1 + q_2) + \ddot{q}_2 g \sin(q_1 + q_2) - L_1 \ddot{q}_1 \cos(q_2) \\
& + L_1 \dot{q}_1^2 \dot{q}_2^2 \sin(q_2) + 2 \dot{q}_1 \dot{q}_2 g \cos(q_1 + q_2) + L_1 \ddot{q}_1 \ddot{q}_2 \sin(q_2) - 2 L_1 \ddot{q}_1 \dot{q}_1 \sin(q_2) \\
& + 2 L_1 \ddot{q}_1 \dot{q}_2 \sin(q_2) + L_1 \ddot{q}_1 \dot{q}_2^2 \cos(q_2) - L_1 \ddot{q}_2 \dot{q}_1^2 \cos(q_2) - 4 L_1 \ddot{q}_1 \dot{q}_1 \dot{q}_2 \cos(q_2)).
\end{aligned}
$$

They can again be verified by taking analytic derivatives of the above ($\dot{\tau}_1, \dot{\tau}_2$).

## 6.2 KUKA LBR iiwa manipulator

The closed-form formulation presented in this paper are used to compute the second-order inverse dynamics of the 7 degrees of freedom KUKA LBR iiwa robot.

**Kinematic model** The body-fixed frames are introduced according to [27], which follow the modified Denavit-Hartenberg (DH) convention. As the z-axis
<figure><img src="image_3.png" /><figcaption>Fig. 2: Link frames of the KUKA LBR iiwa 14 R820 robot [27]</figcaption></figure>

represents the joint axis in this convention, the joint screw coordinates in body-fixed representation are

$$ ^{i}\mathbf{X}_{i} = (0, 0, 1, 0, 0, 0)^{T}, \quad i = 1, \dots, 7. \qquad (58) $$

The relative reference configurations $\mathbf{B}_i$ in (3) have the form (2), with the relative rotation matrix $\mathbf{R}_{i-1,i}$ and the position vector $^{i-1}\mathbf{r}_{i-1,i}$ from the origin of the reference of body $i-1$ to that of its successor body $i$. According to the zero reference configuration shown in Figure 2 in [27], the body-fixed displacement vectors are

$$ ^{i-1}\mathbf{r}_{i-1,i} = \mathbf{0}, \quad i = 1, 2, 4, 6, 7 \\ ^{2}\mathbf{r}_{2,3} = (0, r_{3}, 0)^{T}, \quad ^{4}\mathbf{r}_{4,5} = (0, -r_{5}, 0)^{T} $$

and the relative rotation matrices are

$$ \mathbf{R}_{0,1} = \mathbf{I} \\ \mathbf{R}_{i-1,i} = \begin{pmatrix} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{pmatrix}, \quad i = 2, 5, 6 \\ \mathbf{R}_{i-1,i} = \begin{pmatrix} 1 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & -1 & 0 \end{pmatrix}, \quad i = 3, 4, 7. $$
<table><caption>Table 1: Inertia data of different bodies in the KUKA robot as identified in [27]</caption><thead><tr><th>i</th><th>m<sup>i</sup></th><th><sup>i</sup>c<sub>i,x</sub></th><th><sup>i</sup>c<sub>i,y</sub></th><th><sup>i</sup>c<sub>i,z</sub></th><th>θ<sup>c</sup><sub>i,xx</sub></th><th>θ<sup>c</sup><sub>i,xy</sub></th><th>θ<sup>c</sup><sub>i,yx</sub></th><th>θ<sup>c</sup><sub>i,yy</sub></th><th>θ<sup>c</sup><sub>i,yz</sub></th><th>θ<sup>c</sup><sub>i,zz</sub></th></tr></thead><tbody><tr><td>1</td><td>3.94781</td><td>-0.00351</td><td>0.00160</td><td>-0.03139</td><td>0.00455</td><td>0.00000</td><td>-0.00000</td><td>0.00454</td><td>0.00001</td><td>0.00029</td></tr><tr><td>2</td><td>4.50275</td><td>-0.00767</td><td>0.16669</td><td>-0.00355</td><td>0.00032</td><td>0.00000</td><td>0.00000</td><td>0.00010</td><td>-0.00000</td><td>0.00042</td></tr><tr><td>3</td><td>2.45520</td><td>-0.00225</td><td>-0.03492</td><td>-0.02652</td><td>0.00223</td><td>-0.00005</td><td>0.00007</td><td>0.00219</td><td>0.00007</td><td>0.00073</td></tr><tr><td>4</td><td>2.61155</td><td>0.00020</td><td>-0.05268</td><td>0.03818</td><td>0.03844</td><td>0.00088</td><td>-0.00112</td><td>0.01144</td><td>-0.00111</td><td>0.04988</td></tr><tr><td>5</td><td>3.41000</td><td>0.00005</td><td>-0.00237</td><td>-0.21134</td><td>0.00277</td><td>-0.00001</td><td>0.00001</td><td>0.00284</td><td>-0.00000</td><td>0.00012</td></tr><tr><td>6</td><td>3.38795</td><td>0.00049</td><td>0.02019</td><td>-0.02750</td><td>0.00050</td><td>-0.00005</td><td>-0.00003</td><td>0.00281</td><td>-0.00004</td><td>0.00232</td></tr><tr><td>7</td><td>0.35432</td><td>-0.03466</td><td>-0.02324</td><td>0.07138</td><td>0.00795</td><td>0.00022</td><td>-0.00029</td><td>0.01089</td><td>-0.00029</td><td>0.00294</td></tr></tbody></table>

**Dynamic model parameters** The mass and inertia data used in this example is taken from the identified model of KUKA LBR iiwa robot reported in [27]. The COM position vector $i\mathbf{c}_i = (i\mathbf{c}_{i,x}, i\mathbf{c}_{i,y}, i\mathbf{c}_{i,z})^T \in \mathbb{R}^3$ of link $i$ is measured with respect to the link $i$ frame. The parameters in the symmetric inertia tensor of link $i$ are defined relative to the COM of link $i$ in the $i^{th}$ link frame.

**Second-order inverse dynamics** The joint trajectory is described by means of cos-function as shown in Figure 3a, and used for the inverse dynamics computation of the robotic manipulator (see Figure 3b). Figure 3a and Figure 3b show first- and second-order time derivatives of the generalized forces. The results computed from the closed form expressions are compared against the numerical differentiation. As apparent from Figure 4, the closed form derivatives match the numerical time differentiation of generalized forces in both cases which attests the correctness of the formulation presented in this paper.

**Computational Performance** The computational performance of the closed form algorithm was evaluated by measuring the total CPU time spent in 10000 evaluations of second-order inverse dynamics on a standard laptop with Intel Core i9-7960X CPU clocked at 2.80 GHz and 128 GB RAM. It was found that the closed form implementation takes 16.34 seconds leading to the average computational time of 1.6 milliseconds per evaluation which is sufficient for many real-time control applications.

# 7 Conclusion and Outlook

This paper presents closed form expressions for the first and second time derivative of the EOM of a kinematic chain. Building upon the Lie group formulation of the EOM, the formulations are advantageous as they are expressed in terms of joint screw coordinates, and thus facilitate parameterization in terms of vector quantities that can be easily obtained. The computationally efficiency of these closed form relations compared to recursive algorithms and their efficient implementation will be a topic of further research. It is already obvious from the
<figure><img src="image_4.png" /><figcaption>Fig. 3: Joint trajectories and corresponding joint torques of the KUKA LBR iiwa 14 R820 robot</figcaption></figure>
<figure><img src="image_5.png" /><figcaption>Fig. 4: Results for the higher-order inverse dynamics</figcaption></figure>
presented expressions that they involve many repeated terms that can be pre-
computed and reused. Future research will also address the time derivatives of
general mechanisms with kinematic loops and an efficient C++ based implemen-
tation in Hybrid Robot Dynamics (HyRoDyn) software framework [31].

**Acknowledgments** This work has been supported by the LCM K2 Center for Symbiotic Mechatronics within the framework of the Austrian COMET-K2 program. The second author acknowledges the support of Q-RoCK (Grant Number: FKZ 01IW18003) and VeryHuman (Grant Number: FKZ 01IW20004) projects funded by the German Aerospace Center (DLR) with federal funds from the Federal Ministry of Education and Research (BMBF).

The authors further thank Rohit Kumar from the University Genoa for help-
ing with the implementation and testing of the symbolic code.

References

1. G. Buondonno, A. De Luca: A recursive Newton-Euler algorithm for robots with elastic joints and its application to control, 2015 IEEE/RSJ IROS, 5526-5532

2. G. Buondonno, A. De Luca: Efficient Computation of Inverse Dynamics and Feed-back Linearization for VSA-Based Robots, IEEE Rob. Aut. Letters, 1(2), 2016, 908-915

3. Carpentier, Justin, and Nicolas Mansard. "Analytical derivatives of rigid body dynamics algorithms." In Robotics: Science and Systems. 2018.

4. J. J. De Jong, Y. Q. Wu, M. Carricato, J. L. Herder, A pure-inertia method for dynamic balancing of symmetric planar mechanisms, in Advances in Robot Kinematics, Vol. 5, 2018, pp. 1-8

5. J.J. de Jong, A. Müller, J.L. Herder: Higher-order derivatives of rigid body dynamics with application to the dynamic balance of spatial linkages, Mech. Mach. Theory, Vol. 155, 2021, 23 pages

6. A. De Luca: Decoupling and feedback linearization of robots with mixed rigid/elastic joints, Int. J. Rob. Nonlin. Cont., 8, 1998, 965-977

7. R. Featherstone: Rigid Body Dynamics Algorithms, Springer, 2008

8. G. Garofalo, C. Ott, A. Albu-Schäffer: On the closed form computation of the dynamic matrices and their differentiations, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, 2013, pp. 2364-2359.

9. H. Gattringer, et al.: Recursive methods in control of flexible joint manipulators, Multibody Syst Dyn. 32, 2014, 117-131

10. C. Guarino Lo Bianco, E. Fantini: A recursive Newton-Euler approach for the evaluation of generalized forces derivatives, 12th IEEE Int. Conf. Methods Models Autom. Robot., 2006, 739-744

11. C. Guarino Lo Bianco: Evaluation of Generalized Force Derivatives by Means of a Recursive Newton–Euler Approach, IEEE Trans. Rob., 25(4), 954-959

12. Sehoon Ha, Stelian Coros, Alexander Alspach, Joohyung Kim, and Katsu Yamane.
"Joint Optimization of Robot Design and Motion Parameters using the Implicit
Function Theorem." In Robotics: Science and Systems. 2017

13. A. Jain: Robot and Multibody Dynamics, Springer Science+Business Media, 2011

14. Shivesh Kumar. "Modular and Analytical Methods for Solving Kinematics and Dynamics of Series-Parallel Hybrid Robots." PhD diss., Universität Bremen, 2019
15. Sung-Hee Lee, Junggon Kim, F. C. Park, Munsang Kim and J. E. Bobrow, "Newton-type algorithms for dynamics-based robot movement optimization," In IEEE Transactions on Robotics, vol. 21, no. 4, pp. 657-667, Aug. 2005.

16. K.M. Lynch, F.C. Park: Modern Robotics, Cambridge, 2017

17. Carlos Mastalli, Rohan Budhiraja, Wolfgang Merkt, Guilhem Saurel, Bilal Hamoud, Maximilien Naveau, Justin Carpentier, Sethu Vijayakumar, and Nicolas Mansard. "Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control." 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020, pp. 2536-2542, doi: 10.1109/ICRA40945.2020.9196673.

18. A. Müller: Coordinate Mappings for Rigid Body Motions, ASME Journal of Computational and Nonlinear Dynamics, Vol. 12, No. 2, 2017

19. A. Müller: Screw and Lie group theory in multibody dynamics – Motion representation and recursive kinematics of tree-topology systems, Multibody System Dynamics, Vol. 43, No. 1, 2018, pp. 1-34

20. A. Müller: Screw and Lie group theory in multibody dynamics – Recursive algorithms and equations of motion of tree-topology systems, Multibody System Dynamics, 42(2), 2018 219-248

21. A. Müller: Recursive Second-Order Inverse Dynamics for Serial Manipulators, IEEE Int. Conf. Robotics Automations (ICRA), May 29-June 3, 2017, Singapore

22. A. Müller: An *O*(n)-Algorithm for the Higher-Order Kinematics and Inverse Dynamics of Serial Manipulators using Spatial Representation of Twists, IEEE Robotics and Automation Letters, 2020

23. R.M. Murray, Z. Li, and S.S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press BocaRaton, 1994

24. G. Palli, C. Melchiorri, A. De Luca: On the Feedback Linearization of Robots with Variable Joint Stiffness, IEEE Int. Conf. Rob. Aut. (IROS), Pasadena, CA, USA, May 19-23, 2008

25. A. Reiter, A. Müller, H. Gattringer: On Higher-Order Inverse Kinematics Methods in Time-Optimal Trajectory Planning for Kinematically Redundant Manipulators, IEEE Trans. Industrial Informatics, Vol. 14, No. 4, 2018, pp. 1681 - 1690

26. J. Selig: Geometric Fundamentals of Robotics (Monographs in Computer Science Series), Springer-Verlag New York, 2005

27. Stürz, Y. R., Affolter, L. M., and Smith, R. S.: Parameter identification of the KUKA LBR iiwa robot including constraints on physical feasibility, IFAC-PapersOnLine, Vol. 50, No. 1, 2017, pp- 6863-6868

28. V. Van der Wijk, S. Krut, F. Pierrot, J. L. Herder, Design and experimental evaluation of a dynamically balanced redundant planar 4-RRR parallel manipulator, The International Journal of Robotics Research, Vol. 32, No 6, 2013, pp. 744-759

29. Y. Wu, C. M. Gosselin: Synthesis of Reactionless Spatial 3-DoF and 6-DoF Mechanisms without Separate Counter-Rotations, The International Journal of Robotics Research, Vol. 23, No. 6, 2004, pp. 625-642

30. A. Walther, and A. Griewank. "Getting Started with ADOL-C." Combinatorial scientific computing 09061 (2009): 181-202.

31. Kumar, S., Szadkowski, K. A. V., Mueller, A., and Kirchner, F. . "An Analytical and Modular Software Workbench for Solving Kinematics and Dynamics of Series-Parallel Hybrid Robots." ASME. J. Mechanisms Robotics. April 2020; 12(2): 021114. https://doi.org/10.1115/1.4045941

32. Shivesh Kumar, Andreas Mueller. "Nth Order Analytical Time Derivatives of Inverse Dynamics in Recursive and Closed Forms." 2021 IEEE International Conference on Robotics and Automation (ICRA), 2021.