# Recursive second-order inverse dynamics for serial manipulators

Andreas Muller

► To cite this version:

Andreas Muller. Recursive second-order inverse dynamics for serial manipulators. 2017 IEEE International Conference on Robotics and Automation (ICRA), May 2017, Singapore, Singapore. pp.2483-2489, 10.1109/ICRA.2017.7989289. hal-05162398

HAL Id: hal-05162398

https://inria.hal.science/hal-05162398v1

Submitted on 15 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
# Recursive Second-Order Inverse Dynamics for Serial Manipulators

A. Müller, Institute of Robotics, Johannes Kepler University Linz, Austria

**Abstract**— Various model-based control schemes, e.g. feedback-linearizing control of robotic manipulators with series elastic actuators require time derivatives of the control forces, i.e. of the equations of motion. This is referred to as the higher-order inverse dynamics problem. As this must be evaluated on the controller hardware, computationally efficient formulations are crucial. While recursive $O(n)$ inverse dynamics algorithms are well-established, such for the higher-order inverse dynamics are relatively new. $O(n)$ algorithms for the latter problem were published recently building upon the known recursive inverse dynamics algorithms in terms of Denavit-Hartenber (DH) parameters. The DH convention is restrictive, however. Also the higher-order formulations are naturally very complex. Both issues are addresses in this paper with concepts from screw and Lie group theory. An $O(n)$ algorithm for determining the first and second time derivative of the inverse dynamics solution is presented. The Lie group approach provides a high level of compactness while ensuring the computational efficiency. Being frame invariant this formulation allows for using representations in different frame. Two formulations are presented, one using the body-fixed and the other the so-called hybrid representation of twists. The latter is deemed to be computationally more efficient than the classical body-fixed version. Results are shown for a 6-DOF industrial manipulator.

**Index Terms**— Inverse dynamics, series-elastic actuators, feedback linearization, serial manipulator, O(n) methods

## I. INTRODUCTION

Human robot collaboration not only requires novel control schemes but further demands tailored actuation concepts. As such series elastic actuators (SEA) and in particular variable stiffness actuators (VSA) where introduced as a means to provide the required adaptive compliance characteristics. Reliable application of SEA and VSA requires model-based control for which the basis are the equations of motion (EOM). The particular case of serial robot manipulators actuated with compliant drives was addressed in [6], [21], [3]. The manipulator was modeled as serial chain of rigid links interconnected by kinematically ideal joints and is assumed to be actuated via elastic coupling to the drive units. Denote with $\mathbf{q} \in \mathbb{V}^n$ and $\theta_m \in \mathbb{V}^n$ the vector of joint and motor variables, respectively, the corresponding EOM attain the form

$$ M(\mathbf{q})\ddot{\mathbf{q}} + g(\dot{\mathbf{q}}, \mathbf{q}) = \mathbf{K}_m(\theta_m - \mathbf{q}) \quad (1) $$

$$ M_m(\mathbf{q})\ddot{\theta}_m + \mathbf{K}_m(\theta_m - \mathbf{q}) = \tau \quad (2) $$

where $M$ is the generalized mass matrix of the robotic arm and $g(\dot{\mathbf{q}}, \mathbf{q})$ represent all state dependent forces of the manipulator, $M_m$ is the diagonal mass matrix comprising the lumped inertias of the drive units, and $\mathbf{K}_m$ is the diagonal stiffness matrix containing the lumped drive stiffnesses, which are time dependent in case of VSA. In the EOM (1) of

the manipulator the generalized forces $\mathbf{Q} := \mathbf{K}_m(\theta_m - \mathbf{q})$ are the joint torques/forces due to the elastic coupling to the drives. Equation (2) accounts for the dynamics of the drives interacting with the manipulator, where $\tau$ is the vector of drive torques/forces. The control task is to achieve a prescribed joint trajectory $\mathbf{q}(t)$ by means of $\tau(t)$.

It is known that the control system is differentially flat and feedback linearizable [7], and that the vector relative degree of $\mathbf{q}$ is 4 [6], [21]. This suggests that up to the 4th time derivatives of $\mathbf{q}$ are required for feedback linearization, which translates to the second time derivative of the EOM (1) respectively $\mathbf{Q}$.

With the increasing computational performance of available controller hardware, centralized model-based control is now feasible. Feedback linearizing control of robotic systems with rigid links actuated directly by motor torques/forces merely requires solving the inverse dynamics problem, i.e. to compute the necessary joint torques $\mathbf{Q}$ for a given motion. Various recursive formulation with $O(n)$ complexity have been reported to this end that allow for evaluating the inverse dynamics on the controller hardware. Feedback linearizing control of SEA/VSA requires $O(n)$ algorithms for evaluating the time derivatives of inverse dynamics solution. Such were reported in [3], [4] that built upon the recursive algorithm [8], [9] for evaluating the first time derivative of the EOM (1).

Common to these algorithms is the use of the Denavit-Hartenberg (DH) convention, and that all vectorial quantities are resolved in moving body-fixed frames. However, DH parameters are rather restrictive since they have nothing to do with the intrinsic geometric parameters used for the manipulator design. It is also known that the non-uniqueness of DH-parameters poses difficulties for the kinematic calibration of robotic manipulators [11]. On the other hand, the basic information required for describing the manipulator kinematics are the locations and orientations of joint axes, and the relative motion of adjacent links is a screw motion. The underlying geometric concept is that of screw motions, for which screw and Lie group theory provide the mathematical foundation [19], [25]. The joint screw coordinates thus provide an alternative parameterization. This formulation is known as the product of exponential (POE) [2]. More importantly, the frame invariance of this description allows for using most appropriate reference frames for the evaluation of EOM.

Lie group formulations for robot dynamics have been proposed in the last two decades, e.g. [14], [22], [23] giving rise to compact recursive formulations of the EOM. Noteworthy, the original work by Featherstone [10] has been based solely on screws and their frame transformations. The
<figure><img src="image_2.png" /><figcaption>Fig. 1. Vectors that determine the screw coordinates, $^iX_i$ represented in body-fixed or $Y_i$ represented in IFR, for lower pair joint $i$</figcaption></figure>

appealing feature of these approaches is its compactness and lucidity. The crucial point is that rigid body motions form the Lie group $SE(3)$ (which can be represented using 'homogeneous transformation matrices') while rigid body twists form the Lie algebra $se(3)$ (which can be represented using 6-vectors), and the motion of a lower pair joint is given by the exponential of an associated joint screw coordinate vector. Moreover, all higher-order derivatives of kinematic relations are available in closed form as well as recursively in terms of Lie brackets of the joint screws [15], [16]. Even more, as screws provide a compact representation of kinematic and kinetic objects, summarizing angular and translational parts, Lie group formulations naturally provide a high level of compactness. Another advantage of using the screw/Lie group formulation is that it gives rise to geometric calibration algorithms with improved performance [5].

Now the actual number of numerical operations of the $O(n)$ algorithms is related to the description of the robot kinematics. This has been discussed for recursive kinematics [20] as well as dynamics formulations [10], [1]. For this reason $O(n)$ dynamics algorithms resolve the body-fixed twists in the global inertial frame (referred to as the hybrid representation) [1], [13], [24] as opposed to the above Lie group algorithms that use a body-fixed representation. It should be mentioned that also for the partial derivatives of the EOM recursive $O(n)$ algorithms were developed in order to linearize the EOM of multibody systems, e.g. [12].

In this paper two versions of an $O(n)$ algorithm for the first and second time derivative of the EOM (1) are presented employing screw and Lie group concepts. The first uses body-fixed and the second the hybrid representation of twists. This contribution complements the existing Lie group inverse dynamics algorithms [10]. The body-fixed version provides a compact formulation of the algorithms [3], [4], [8], [9], whereas the hybrid version provides additional efficiency.

In section II the Lie group formulation for the configurations and twists of the manipulator are recalled. A recursive formulation for the higher-order forward kinematics in body-fixed and hybrid representation is presented in section III. As

prerequisite for the higher-order inverse dynamics algorithm in section V, the NE-equations, in body-fixed and hybrid representation, and their second time derivatives are presented in section IV. Numerical results are shown for 6 DOF industrial manipulator in section VI. Some basic definitions are provided in the appendix.

**Notation:** Frames are denoted $F_i, i = 0, \dots, n$, where $0$ indicates the global inertial frame (IFR). 3-dimensional vectors are written in lower case while matrices and 6-dimensional vectors are written in upper case bold face. The notation $^iX$ indicates that the vector is resolved in frame $F_i$. In particular $^0X$ is resolved in the IFR $F_0$. For sake of simplicity the index $0$ is mostly omitted throughout the paper, i.e. $X = ^0X$. $R_i$ denotes the rotation matrix describing the orientation of link $i$ w.r.t. the IFR, and $R_{i,i-1}$ that describing the relative orientation of link $i$ and $i-1$. $r_i$ is the position vector of the origin of $F_i$ relative to $F_0$ resolved in $F_0$, and $^i\mathbf{r}_i$ the same vector resolved in $F_i$. Then $\mathbf{r}_{i,i-1} := \mathbf{r}_i - \mathbf{r}_{i-1}$ is the relative displacement of $F_i$ and $F_{i-1}$ resolved in the IFR. Accordingly $^0\mathbf{v}_i$ and $^0\omega_i$ is the translational and angular velocity of link $i$ resolved in IFR.

## II. MANIPULATOR KINEMATICS

### A. Link Configurations in Terms of Joint Screws

The motion of the manipulator is described w.r.t. the IFR $F_0$. At link $i$ is attached a body-fixed reference frame (RFR), denoted $F_i$. The configuration of $F_i$, i.e. of link $i$, w.r.t. to the IFR is represented by a rigid body transformation $C_i \in SE(3), i = 1, \dots, n$. The latter comprises the position vector $\mathbf{r}_i \in \mathbb{R}^3$ of the origin of $F_i$ resolved in the IFR and the rotation matrix $\mathbf{R}_i \in SO(3)$ describing the orientation of $F_i$ w.r.t. the IFR. Denote with $C_{i,i-1} := C_i^{-1}C_{i-1}$ the relative configuration of link $i-1$ w.r.t. link $i$.

Assume that the manipulator comprises $n$ 1-DOF joints (revolute, prismatic, screw joints). Joints and links are numbered increasingly from ground (index 0) to tip (index $n$). The corresponding joint variables are denoted with $q_i, i = 1, \dots, n$, and summarized in $\mathbf{q} \in \mathbb{V}^n$. The configuration of link $i$ can be expressed as product of exponentials (POE) [2],
[19]. The following two variants are advantageous [17]

$$
\begin{align}
C_i(\mathbf{q}) &= \mathbf{B}_1 \exp(\mathbf{e}_1 \mathbf{X}_1 \mathbf{q}_1) \mathbf{B}_2 \exp(\mathbf{e}_2 \mathbf{X}_2 \mathbf{q}_2) \cdots \mathbf{B}_i \exp(\mathbf{e}_i \mathbf{X}_i \mathbf{q}_i) \nonumber \\
&= \exp(\mathbf{Y}_1 \mathbf{q}_1) \exp(\mathbf{Y}_2 \mathbf{q}_2) \cdots \exp(\mathbf{Y}_i \mathbf{q}_i) \mathbf{A}_i \tag{3}
\end{align}
$$

Here, $\mathbf{B}_i = \mathbf{C}_{i,i-1}(0) \in SE(3)$ is the zero reference configuration of link $i$ w.r.t. link $i-1$, and $\mathbf{A}_i := \mathbf{B}_1 \cdots \mathbf{B}_i = \mathbf{C}_i(0)$ is the zero reference configuration of link $i$ w.r.t. the IFR for $\mathbf{q} = \mathbf{0}$ (Fig. 1). The screw coordinate vector of joint $j$ represented in the body-fixed RFR $\mathcal{F}_j$ is $^j\mathbf{X}_j = (^j\mathbf{e}_j, ^j\mathbf{x}_j \times ^j\mathbf{e}_j + h_j{}^j\mathbf{e}_j)^T$, where $^j\mathbf{e}_j$ is the unit vector along the joint axis and $^j\mathbf{x}_j$ is a vector to a point on the joint axis measured and resolved in $\mathcal{F}_j$. The leading superscript indicates the frame in which the vectors are resolved. Analogously, $\mathbf{Y}_j = (\mathbf{e}_j, \mathbf{y}_j \times \mathbf{e}_j + h_j\mathbf{e}_j)^T$ is the screw coordinate vector represented in the IFR (superscript 0 omitted) with the position vector $\mathbf{y}_j$ to a point on the joint axis measured and resolved in the IFR. Both representations are related via the frame transformation $\mathbf{Y}_j = \mathbf{A}\mathbf{d}_{\mathbf{A}_j}^j\mathbf{X}_j$.

**Remark 1:** In contrast to DH-parameters, this formulation involves vectorial quantities that are intuitive and readily available from a design drawing or manufacturer specifications. In the first form (3) of the POE, the screw coordinates are determined in the body-fixed reference frames and the link geometry is encoded in $\mathbf{B}_i$. In the second form of (3) all vectors are determined in the spatial IFR, which makes this form extremely easy to use as the necessary vectors and the reference configurations $\mathbf{A}_i$ can be simply deduced from any reference configurations.

## B. Link Velocities

The twist (velocity) of a link can be represented in various forms. The body-fixed and spatial representations are most commonly used. For control purpose and for dynamics modeling the body-fixed representation is frequently used.

1) The *body-fixed* twist of link $i$, denoted $\mathbf{V}_i^b = (^i\omega_i, ^i\mathbf{v}_i)^T$, is the aggregate of the angular velocity $^i\omega_i$ of the RFR $\mathcal{F}_i$ and the translational velocity $^i\mathbf{v}_i$ of its origin relative to the IFR measured at this origin. Both vectors are resolved in the body-fixed RFR $\mathcal{F}_i$.

2) An alternative is the *hybrid* representation of the twist [19], [26], denoted with $\mathbf{V}_i^h = (^0\omega_i, ^0\mathbf{v}_i)^T$. The physical meaning of the angular velocity vector $^0\omega_i$ and the translational velocity vector $^0\mathbf{v}_i = ^0\dot{\mathbf{r}}_i$ is as before for the body-fixed version, but now the vectors are resolved in the spatial IFR. The leading superscript '0' will be omitted in the following. The significance of the hybrid twist is that the twists of all links in the manipulator are resolved in the IFR so that the propagation of twists with the kinematic chain simplifies (sec. III). Body-fixed and hybrid twists are related by a coordinate transformation $\mathbf{V}_i^h = \mathbf{A}\mathbf{d}_{\mathbf{R}_i}^h \mathbf{V}_i^b$ with (32).

# III. RECURSIVE HIGHER-ORDER KINEMATICS

## A. Body-Fixed Representation

The body-fixed twist (i.e. screw coordinates) of link $i-1$ expressed in frame $\mathcal{F}_{i-1}$ transforms to its representation in $\mathcal{F}_i$ according to $\mathbf{A}\mathbf{d}_{\mathbf{C}_{i,i-1}}^h \mathbf{V}_{i-1}^b$ with the transformation

matrix (26). The twist of link $i$, connected to its predecessor link $i-1$ by joint $i$, is thus

$$ \mathbf{V}_i^b = \mathbf{A}\mathbf{d}_{\mathbf{C}_{i,i-1}}^h \mathbf{V}_{i-1}^b + {}^i\mathbf{X}_i \ddot{\mathbf{q}}_i. \quad (4) $$

The time derivative of the screw transformation matrix (26) is $\mathbf{A}\mathbf{d}_{\mathbf{C}_{i,i-1}}^h \mathbf{V}_{i-1}^b = [\mathbf{A}\mathbf{d}_{\mathbf{C}_{i,i-1}}^h \mathbf{V}_{i-1}^b, {}^i\mathbf{X}_i \ddot{\mathbf{q}}_i]$. Inserting (4) yields the recursive expression for the acceleration

$$ \ddot{\mathbf{V}}_i^b = \mathbf{A}\mathbf{d}_{\mathbf{C}_{i,i-1}}^h \ddot{\mathbf{V}}_{i-1}^b + {}^i\mathbf{X}_i \ddot{\mathbf{q}}_i \quad (5) $$

Repeated application of these relations yields the jerk

$$ \ddot{\mathbf{V}}_i^b = \mathbf{A}\mathbf{d}_{\mathbf{C}_{i,i-1}}^h \ddot{\mathbf{V}}_{i-1}^b + {}^i\mathbf{X}_i \ddot{\mathbf{q}}_i \\ - \mathbf{a}\mathbf{d}_{\mathbf{X}_i}^h (\ddot{\mathbf{q}}_i \mathbf{V}_i^b + 2\dot{\mathbf{q}}_i \ddot{\mathbf{V}}_i^b) - \dot{\mathbf{q}}_i^2 \mathbf{a}\mathbf{d}_{\mathbf{X}_i}^2 \mathbf{V}_i^b \quad (6) $$

and the jounce in body-fixed representation

$$ \dddot{\mathbf{V}}_i^b = \mathbf{A}\mathbf{d}_{\mathbf{C}_{i,i-1}}^h \dddot{\mathbf{V}}_{i-1}^b + {}^i\mathbf{X}_i \dddot{\mathbf{q}}_i \\ - \mathbf{a}\mathbf{d}_{\mathbf{X}_i}^h (\dddot{\mathbf{q}}_i \mathbf{V}_i^b + 3\ddot{\mathbf{q}}_i \dddot{\mathbf{V}}_i^b + 3\dot{\mathbf{q}}_i \dddot{\mathbf{V}}_i^b) \\ - 3\dot{\mathbf{q}}_i \mathbf{a}\mathbf{d}_{\mathbf{X}_i}^2 (\ddot{\mathbf{q}}_i \mathbf{V}_i^b + \dot{\mathbf{q}}_i \dddot{\mathbf{V}}_i^b) \\ - \dot{\mathbf{q}}_i^3 \mathbf{a}\mathbf{d}_{\mathbf{X}_i}^3 \mathbf{V}_i^b. \quad (7) $$

## B. Hybrid Representation

The transformation of the hybrid twist of link $i-1$ resolved in $\mathcal{F}_{i-1}$ to its resolution in $\mathcal{F}_i$ is $\mathbf{A}\mathbf{d}_{\mathbf{r}_{i,i-1}}^h \mathbf{V}_i^h$ where $\mathbf{r}_{i,i-1} := \mathbf{r}_i - \mathbf{r}_{i-1}$ is the relative displacement vector of $\mathcal{F}_i$ w.r.t. $\mathcal{F}_{i-1}$ resolved in IFR. This transformation only involves translations since the twists of all links are resolved in the IFR. This leads to the recursive relation for hybrid twists

$$ \mathbf{V}_i^h = \mathbf{A}\mathbf{d}_{\mathbf{r}_{i,i-1}}^h \mathbf{V}_{i-1}^h + {}^0\mathbf{X}_i^i \dot{\mathbf{q}}_i. \quad (8) $$

Now $^0\mathbf{X}_i^i$ is the screw coordinate vector of joint $i$ measured in RFR $\mathcal{F}_i$ but resolved in IFR $\mathcal{F}_0$, and is thus not constant anymore but configuration dependent. It is related to the constant body-fixed representation by

$$ {}^0\mathbf{X}_i^i = \mathbf{A}\mathbf{d}_{\mathbf{R}_i}^i \mathbf{X}_i. \quad (9) $$

Using $\dot{\mathbf{A}}\mathbf{d}_{\mathbf{r}_{i,i-1}} = \mathbf{a}\mathbf{d}_{\dot{\mathbf{r}}_{i,i-1}}$ the acceleration recursion is

$$ \ddot{\mathbf{V}}_i^h = \mathbf{A}\mathbf{d}_{\dot{\mathbf{r}}_{i,i-1}}^h \ddot{\mathbf{V}}_{i-1}^h + \mathbf{a}\mathbf{d}_{\dot{\mathbf{r}}_{i,i-1}}^h \mathbf{V}_{i-1}^h \\ + {}^0\mathbf{X}_i^i \ddot{\mathbf{q}}_i + \dot{\mathbf{q}}_i \mathbf{a}\mathbf{d}_{\omega_i}^0 \mathbf{X}_i^i \quad (10) $$

where $\dot{\mathbf{r}}_i$ is the translation part in $\mathbf{V}_i^h$ and $\dot{\mathbf{r}}_{i,i-1} = \dot{\mathbf{r}}_i - \dot{\mathbf{r}}_{i-1}$. The same arguments yield the recursion for the hybrid jerk

$$ \dddot{\mathbf{V}}_i^h = \mathbf{A}\mathbf{d}_{\dot{\mathbf{r}}_{i,i-1}}^h \dddot{\mathbf{V}}_{i-1}^h + 2\mathbf{a}\mathbf{d}_{\dot{\mathbf{r}}_{i,i-1}}^h \dddot{\mathbf{V}}_{i-1}^h + \mathbf{a}\mathbf{d}_{\dot{\mathbf{r}}_{i,i-1}}^h \mathbf{V}_{i-1}^h \quad (11) \\ + {}^0\mathbf{X}_i^i \dddot{\mathbf{q}}_i + 2\ddot{\mathbf{q}}_i \mathbf{a}\mathbf{d}_{\omega_i}^0 \mathbf{X}_i^i + \dot{\mathbf{q}}_i \mathbf{a}\mathbf{d}_{\omega_i}^0 \mathbf{X}_i^i + \dot{\mathbf{q}}_i \mathbf{a}\mathbf{d}_{\omega_i}^2 \mathbf{X}_i^i $$

and jounce

$$ \dddot{\mathbf{V}}_i^h = \mathbf{A}\mathbf{d}_{\dot{\mathbf{r}}_{i,i-1}}^h \dddot{\mathbf{V}}_{i-1}^h + 3\mathbf{a}\mathbf{d}_{\dot{\mathbf{r}}_{i,i-1}}^h \dddot{\mathbf{V}}_{i-1}^h \\ + 3\mathbf{a}\mathbf{d}_{\dot{\mathbf{r}}_{i,i-1}}^h \dddot{\mathbf{V}}_{i-1}^h + \mathbf{a}\mathbf{d}_{\dot{\mathbf{r}}_{i,i-1}}^h \mathbf{V}_{i-1}^h \\ + (3\ddot{\mathbf{q}}_i \mathbf{a}\mathbf{d}_{\omega_i} + 3\ddot{\mathbf{q}}_i (\mathbf{a}\dot{\omega}_i + \mathbf{a}\dot{\omega}_i^2)) {}^0\mathbf{X}_i^i + {}^0\mathbf{X}_i^i \dddot{\mathbf{q}}_i \\ + \dot{\mathbf{q}}_i (\mathbf{a}\dot{\omega}_i \mathbf{a}\dot{\omega}_i + 2\mathbf{a}\mathbf{d}_{\omega_i} \mathbf{a}\dot{\omega}_i + \mathbf{a}\dot{\omega}_i^3 + \mathbf{a}\dot{\omega}_i) {}^0\mathbf{X}_i^i. \quad (12) $$
**Remark 2:** The hybrid representation of twists is extensively used for O(n) algorithm in multibody dynamics because the twist transformations do not involve any rotations [1], [13], [24]. This computational advantage is expected to carry over to the recursive derivatives.

## IV. NEWTON-EULER EQUATIONS AND DERIVATIVES

NE-equations arise from the coordinate-free principle of momentum balance. The choice of a representation that is necessary to actually evaluate the NE-equations determines the number of operations involved.

### A. Body-Fixed Representation

Denote with $\Theta_i^b$ the inertia tensor of link $i$ w.r.t. its RFR $\mathcal{F}_i$, with $m_i$ the mass of link $i$, and with $i\mathbf{d}_{ic}$ the distance vector from the origin of $\mathcal{F}_i$ to the COM of link $i$. The inertia matrix of body $i$ w.r.t. $\mathcal{F}_i$ is then defined as

$$ \mathbf{M}_i^b = \begin{pmatrix} \mathbf{\Theta}_i^b & i\tilde{\mathbf{d}}_{ic}m_i \\ -i\tilde{\mathbf{d}}_{ic}m_i & \mathbf{I}m_i \end{pmatrix} \quad (13) $$

such that $\Pi_i^b := \mathbf{M}_i^b \mathbf{V}_i^b \in se^*$ (3) is the momentum co-screw in body-fixed representation. The momentum balance equations give rise to the NE-equations. Invoking the expression (28) for the derivative of co-screw coordinates represented in a moving frame yields,

$$ \begin{aligned} \mathbf{W}_i^b &= \dot{\Pi}_i^b - \mathbf{ad}_{\mathbf{V}_i^b}^T \Pi_i^b \\ &= \mathbf{M}_i^b \dot{\mathbf{V}}_i^b - \mathbf{ad}_{\mathbf{V}_i^b}^T \mathbf{M}_i^b \mathbf{V}_i^b. \end{aligned} \quad (14) $$

$\mathbf{W}_i^b = (\mathbf{t}_i^b, \mathbf{f}_i^b)^T$ is the wrench applied to $\mathcal{F}_i$ (including gravity), where $\mathbf{t}_i^b \in \mathbb{R}^3$ is the vector of applied torques, and $\mathbf{f}_i^b \in \mathbb{R}^3$ the vector of forces applied at the origin of $\mathcal{F}_i$. This is merely a compact expression of the NE-equations

$$ \begin{aligned} \mathbf{\Theta}_i^b \dot{\omega}_i^b + \tilde{\omega}_i^b \mathbf{\Theta}_i^b \omega_i^b + m_i i \tilde{\mathbf{d}}_{ic} (\dot{\mathbf{v}}_i^b + \tilde{\omega}_i^b \mathbf{v}_i^b) &= \mathbf{t}_i^b \quad (15) \\ m_i (\dot{\mathbf{v}}_i^b + \tilde{\omega}_i^b \mathbf{v}_i^b + (\tilde{\omega}_i^b + \tilde{\omega}_i^b \tilde{\omega}_i^b)^i \mathbf{d}_{ic}) &= \mathbf{f}_i^b. \end{aligned} $$

A straightforward calculation yields the first and second time derivative of the NE-equations, noting that $\mathbf{M}_i^b$ is constant,

$$ \dot{\mathbf{W}}_i^b = \mathbf{M}_i^b \ddot{\mathbf{V}}_i^b - \mathbf{ad}_{\mathbf{V}_i^b}^T \mathbf{M}_i^b \dot{\mathbf{V}}_i^b - \mathbf{ad}_{\mathbf{V}_i^b}^T \mathbf{M}_i^b \mathbf{V}_i^b \quad (16) $$

$$ \begin{aligned} \ddot{\mathbf{W}}_i^b = \mathbf{M}_i^b \ddot{\mathbf{V}}_i^b - \mathbf{ad}_{\mathbf{V}_i^b}^T \mathbf{M}_i^b \dot{\mathbf{V}}_i^b - \mathbf{ad}_{\mathbf{V}_i^b}^T \mathbf{M}_i^b \mathbf{V}_i^b & \quad (17) \\ -2\mathbf{ad}_{\mathbf{V}_i^b}^T \mathbf{M}_i^b \dot{\mathbf{V}}_i^b & \end{aligned} $$

### B. Hybrid Representation

The hybrid mass matrix w.r.t. to the RFR $\mathcal{F}_i$ is

$$ \mathbf{M}_i^h = \begin{pmatrix} \mathbf{\Theta}_i^h & \tilde{\mathbf{d}}_{ic}m_i \\ -\tilde{\mathbf{d}}_{ic}m_i & \mathbf{I}m_i \end{pmatrix}, \quad \mathbf{\Theta}_i^h = \mathbf{R}_i \mathbf{\Theta}_i^b \mathbf{R}_i^T \quad (18) $$

and the momentum co-screw in hybrid representation is thus $\Pi_i^h := \mathbf{M}_i^h \mathbf{V}_i^h$. Now the inertia tensor and the vector $\mathbf{d}_{ic}$ are resolved in IFR, and thus the mass matrix is configuration dependent. Since all vectors are resolved in the IFR, the

momentum balance in hybrid representation is $\dot{\mathbf{\Pi}}_i^h = \mathbf{W}_i^h$. Using $\dot{\mathbf{A}}\mathbf{d}_{ic}^{-1} = -\mathbf{a}\mathbf{d}_{ic} = \mathbf{A}\mathbf{d}_{ic}^{-1}\mathbf{a}\mathbf{d}_i - \mathbf{a}\mathbf{d}_i\mathbf{A}\mathbf{d}_{ic}^{-1}$ yields

$$ \mathbf{W}_i^h = \mathbf{M}_i^h \dot{\mathbf{V}}_i^h + \mathbf{a}\mathbf{d}_i\mathbf{M}_i^h \dot{\mathbf{V}}_i^h \quad (19) $$

where $\dot{\mathbf{V}}_i^h$ denotes the vector with zero translational velocity (29) and the notation (33) for the ad matrix is used. Separating the angular and translational part results in

$$ \begin{aligned} \mathbf{\Theta}_i^h \dot{\omega}_i + \tilde{\omega}_i \mathbf{\Theta}_i^h \omega_i + m_i \tilde{\mathbf{d}}_{ic} \ddot{\mathbf{r}}_i &= \mathbf{t}_i^h \quad (20) \\ m_i (\ddot{\mathbf{r}}_i + (\tilde{\omega}_i + \tilde{\omega}_i \tilde{\omega}_i) \mathbf{d}_{ic}) &= \mathbf{f}_i^h. \end{aligned} $$

The hybrid equations (19) are apparently simpler than the body-fixed equations (15). Time derivatives are obtained by invoking the relation $\dot{\mathbf{M}}_i^h = \mathbf{a}\mathbf{d}_i\mathbf{M}_i^h - \mathbf{M}_i^h \mathbf{a}\mathbf{d}_i$. Algebraic simplification yields the first and second time derivative

$$ \begin{aligned} \dot{\mathbf{W}}_i^h = \mathbf{M}_i^h \dot{\mathbf{V}}_i^h + \mathbf{a}\mathbf{d}_i\mathbf{M}_i^h (\dot{\mathbf{V}}_i^h + \dot{\mathbf{V}}_i^h) \\ + (\mathbf{a}\mathbf{d}_i^2 + \mathbf{a}\mathbf{d}_i)\mathbf{M}_i^h \dot{\mathbf{V}}_i^h - \mathbf{M}_i^h \mathbf{a}\mathbf{d}_i \dot{\mathbf{V}}_i^h. \end{aligned} \quad (21) $$

$$ \begin{aligned} \ddot{\mathbf{W}}_i^h = \mathbf{M}_i^h \ddot{\mathbf{V}}_i^h + 2(\mathbf{a}\mathbf{d}_i\mathbf{M}_i^h - \mathbf{M}_i^h \mathbf{a}\mathbf{d}_i)\ddot{\mathbf{V}}_i^h \\ + (\mathbf{a}\mathbf{d}_i\mathbf{M}_i^h - \mathbf{M}_i^h \mathbf{a}\mathbf{d}_i\mathbf{\omega}_i)\dot{\mathbf{V}}_i^h + \mathbf{a}\mathbf{d}_i\mathbf{M}_i^h \ddot{\mathbf{V}}_i^h \\ + ((2\mathbf{a}\mathbf{d}_i^2 + 2\mathbf{a}\mathbf{d}_i)\mathbf{M}_i^h - \mathbf{a}\mathbf{d}_i\mathbf{M}_i^h \mathbf{a}\mathbf{d}_i)\dot{\mathbf{V}}_i^h \\ + (\mathbf{a}\mathbf{d}_i^3 + \mathbf{a}\mathbf{d}_i\mathbf{a}\mathbf{d}_i\mathbf{\omega}_i + \mathbf{a}\mathbf{d}_i\ddot{\mathbf{\omega}}_i)\mathbf{M}_i^h \dot{\mathbf{V}}_i^h. \end{aligned} \quad (22) $$

The complexity of these equations is comparable to that of the body-fixed representation but number of operations is smaller. The implementation can exploit the sparsity of the expressions. E.g. the first-order reads explicitly

$$ \begin{aligned} \mathbf{\Theta}_i^h (\ddot{\omega}_i - \tilde{\omega}_i \dot{\omega}_i) + (\tilde{\omega}_i^2 + \tilde{\omega}_i) \mathbf{\Theta}_i^h \omega_i \\ + 2\tilde{\omega}_i \mathbf{\Theta}_i^h \dot{\omega}_i + m_i (\tilde{\mathbf{d}}_{ic} \ddot{\mathbf{r}}_i + \tilde{\mathbf{r}}_i \tilde{\mathbf{d}}_{ic} \omega_i) &= \dot{\mathbf{t}}_i^h \\ m_i (\ddot{\mathbf{r}}_i - \tilde{\mathbf{d}}_{ic} + \tilde{\omega}_i^3 \mathbf{d}_{ic} + 3\tilde{\omega}_i \tilde{\omega}_i \mathbf{d}_{ic} + \tilde{\mathbf{d}}_{ic} \tilde{\omega}_i \tilde{\omega}_i \mathbf{d}_{ic}) &= \mathbf{f}_i^h. \end{aligned} $$

## V. RECURSIVE SECOND-ORDER INVERSE DYNAMICS

The above derivatives of the NE-equations provide the basis for the recursive higher-order inverse dynamics algorithm, which is the main contribution of this paper. The first run is the forward kinematics recursion where the manipulator's state and its derivatives are computed from the given joint variables $\mathbf{q}$ and their time derivatives. The second run is the backward recursion for computing the generalized forces (joint torques/forces) $\mathbf{Q}$ and their time derivatives. Two versions of the algorithm are given, one for the body-fixed and one for the hybrid representation.

### A. Body-Fixed Representation

The forward kinematics recursion evaluates the equations (3) for the link configurations, and the equations (4), (5), (6), and (7) for the twist, acceleration, jerk, and jounce. The output of this run are configurations, body-fixed twist,
accelerations, jerks, and jounces of the individual links. The inverse dynamics backward recursion determines the generalized forces and their first and second time derivatives by evaluating the NE equations (14), and their derivatives (16) and (17).

## Forward Kinematics

* Input: $\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}, \dddot{\mathbf{q}}$

* For $i = 1, \dots, n$

$$ 
\begin{aligned}
C_i &= C_{i-1} B_i \exp(i \mathbf{X}_i q_i) \\
    &= \exp(Y_1 q_1) \cdots \exp(Y_i q_i) A_i
\end{aligned}
$$

$$ V_i^b = \mathrm{Ad}_{C_{i,i-1}} V_{i-1}^b + {}^i\mathbf{X}_i \dot{q}_i $$

$$ \dot{V}_i^b = \mathrm{Ad}_{C_{i,i-1}} \ddot{V}_{i-1}^b - \dot{q}_i \mathrm{ad}_{\mathbf{X}_i} \mathbf{V}_i^b + {}^i\mathbf{X}_i \ddot{q}_i $$

$$ \ddot{V}_i^b = \mathrm{Ad}_{C_{i,i-1}} \dddot{V}_{i-1}^b + {}^i\mathbf{X}_i \dddot{q}_i - \mathrm{ad}_{\mathbf{X}_i} (\ddot{q}_i \mathbf{V}_i^b + 2\dot{q}_i \dot{\mathbf{V}}_i^b) - \dot{q}_i^2 \mathrm{ad}^2_{\mathbf{X}_i} \mathbf{V}_i^b $$

$$ \dddot{V}_i^b = \mathrm{Ad}_{C_{i,i-1}} \dddot{\mathbf{V}}_{i-1}^b + {}^i\mathbf{X}_i \dddot{q}_i - \mathrm{ad}_{\mathbf{X}_i} (\dddot{q}_i \mathbf{V}_i^b + 3\ddot{q}_i \dot{\mathbf{V}}_i^b + 3\dot{q}_i \dot{\mathbf{V}}_i^b) $$

$$ -3\dot{q}_i \mathrm{ad}^2_{\mathbf{X}_i} (\ddot{q}_i \mathbf{V}_i^b + \dot{q}_i \dot{\mathbf{V}}_i^b) $$

$$ - \dot{q}_i^3 \mathrm{ad}^3_{\mathbf{X}_i} \mathbf{V}_i^b $$

* Output: $C_i, V_i^b, \dot{V}_i^b, \ddot{V}_i^b, \dddot{V}_i^b$

## Inverse Dynamics

* Input: $C_i, V_i^b, \dot{V}_i^b, \ddot{V}_i^b, \dddot{V}_i^b, W_i^{\mathrm{b,app}}, \dot{W}_i^{\mathrm{b,app}}, \ddot{W}_i^{\mathrm{b,app}}$

* For $i = n - 1, \dots, 1$

$$ W_i^b = \mathrm{Ad}_{C_{i+1,i}}^T W_{i+1}^b + M_i^b \dot{V}_i^b - \mathrm{ad}_{\dot{V}_i^b} M_i^b \mathbf{V}_i^b + W_i^{\mathrm{b,app}} $$

$$ \dot{W}_i^b = \mathrm{Ad}_{C_{i+1,i}}^T (\dot{W}_{i+1}^b - \dot{q}_{i+1} \mathrm{ad}_{i+1}^T \mathbf{X}_{i+1} W_{i+1}^b) + M_i^b \ddot{V}_i^b - \mathrm{ad}_{\dot{V}_i^b} M_i^b \dot{\mathbf{V}}_i^b - \mathrm{ad}_{\ddot{V}_i^b} M_i^b \mathbf{V}_i^b + \dot{W}_i^{\mathrm{b,app}} $$

$$ \ddot{W}_i^b = \mathrm{Ad}_{C_{i+1,i}}^T (\ddot{W}_{i+1}^b - 2\dot{q}_{i+1} \mathrm{ad}_{i+1}^T \mathbf{X}_{i+1} \dot{\mathbf{W}}_{i+1}^b + (\dot{q}_{i+1}^2 \mathrm{ad}_{i+1}^{2T} \mathbf{X}_{i+1} - \ddot{q}_{i+1} \mathrm{ad}_{i+1}^{T} \mathbf{X}_{i+1}) \mathbf{W}_{i+1}^b) + M_i^b \dddot{\mathbf{V}}_i^b - \mathrm{ad}_{\dot{V}_i^b} M_i^b \ddot{\mathbf{V}}_i^b - \mathrm{ad}_{\ddot{V}_i^b} M_i^b \mathbf{V}_i^b - 2\mathrm{ad}_{\dot{V}_i^b} M_i^b \dot{\mathbf{V}}_i^b + \ddot{W}_i^{\mathrm{b,app}} $$

$$ Q_i = {}^i\mathbf{X}_i^T \mathbf{W}_i^b, \dot{Q}_i = {}^i\mathbf{X}_i^T \dot{\mathbf{W}}_i^b, \ddot{Q}_i = {}^i\mathbf{X}_i^T \ddot{\mathbf{W}}_i^b $$

* Output: $\mathbf{Q}, \dot{\mathbf{Q}}, \ddot{\mathbf{Q}}$

## B. Hybrid Representation

The hybrid version of the forward kinematics is identical to the body-fixed version but with the recursions (8), (10), (11), and (12) for the link twists, accelerations, jerks, and jounces. In addition the instantaneous joint screw coordiantes (9) are computed. Output are the twists of the links in hybrid representation as well as the accelerations, jerks, and jounces. The inverse dynamics run evaluates the hybrid NE equations (19) and their derivatives (21), (22), and thus the generalized forces and their first and second derivatives.

## Forward Kinematics

* Input: $\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}, \dddot{\mathbf{q}}$

* For $i = 1, \dots, n$

$$ C_i = C_{i-1} B_i \exp(i \mathbf{X}_i q_i) = \exp(Y_1 q_1) \cdots \exp(Y_i q_i) A_i $$

$$ {}^0\mathbf{X}_i^i = \mathrm{Ad}_{\mathbf{R}_i} {}^i\mathbf{X}_i = \mathrm{Ad}_{-\mathbf{r}_i(0)} Y_i $$

$$ V_i^h = \mathrm{Ad}_{\mathbf{r}_{i,i-1}} V_{i-1}^h + {}^0\mathbf{X}_i \dot{q}_i $$

$$ \dot{V}_i^h = \mathrm{Ad}_{\mathbf{r}_{i,i-1}} \ddot{V}_{i-1}^h + \mathrm{ad}_{\mathbf{r}_{i,i-1}} \mathbf{V}_{i-1}^h + {}^0\mathbf{X}_i^i \ddot{q}_i + \dot{q}_i \mathrm{ad}_{\omega_i} {}^0\mathbf{X}_i^i $$

$$ \ddot{V}_i^h = \mathrm{Ad}_{\mathbf{r}_{i,i-1}} \dddot{V}_{i-1}^h + 2\mathrm{ad}_{\dot{\mathbf{r}}_{i,i-1}} \dot{\mathbf{V}}_{i-1}^h + \mathrm{ad}_{\dot{\mathbf{r}}_{i,i-1}} \mathbf{V}_{i-1}^h + {}^0\mathbf{X}_i^i \dddot{q}_i + 2\ddot{q}_i \mathrm{ad}_{\omega_i} {}^0\mathbf{X}_i^i + \dot{q}_i \mathrm{ad}_{\dot{\omega}_i} {}^0\mathbf{X}_i^i + \dot{q}_i \mathrm{ad}_{\ddot{\omega}_i} {}^0\mathbf{X}_i^i $$

$$ \dddot{V}_i^h = \mathrm{Ad}_{\mathbf{r}_{i,i-1}} \dddot{\mathbf{V}}_{i-1}^h + 3\mathrm{ad}_{\ddot{\mathbf{r}}_{i,i-1}} \dot{\mathbf{V}}_{i-1}^h + \mathrm{ad}_{\ddot{\mathbf{r}}_{i,i-1}} \mathbf{V}_{i-1}^h + (3\dddot{q}_i \mathrm{ad}_{\omega_i} + 3\ddot{q}_i (\mathrm{ad}_{\dot{\omega}_i} + \mathrm{ad}_{\ddot{\omega}_i}^2)) {}^0\mathbf{X}_i^i + {}^0\mathbf{X}_i^i \dddot{q}_i + \dot{q}_i (\mathrm{ad}_{\omega_i} \mathrm{ad}_{\dot{\omega}_i} + 2\mathrm{ad}_{\dot{\omega}_i} \mathrm{ad}_{\omega_i} + \mathrm{ad}_{\ddot{\omega}_i}^3 + \mathrm{ad}_{\ddot{\omega}_i}) {}^0\mathbf{X}_i^i $$

* Output: $C_i, V_i^b, \dot{V}_i^b, \ddot{V}_i^b, \dddot{V}_i^b$

## Inverse Dynamics

* Input: $C_i, V_i^b, \dot{V}_i^b, \ddot{V}_i^b, \dddot{V}_i^b, W_i^{\mathrm{b,app}}, \dot{W}_i^{\mathrm{b,app}}, \ddot{W}_i^{\mathrm{b,app}}$

* For $i = n - 1, \dots, 1$

$$ W_i^h = \mathrm{Ad}_{\dot{\mathbf{d}}_{i+1,i}}^T \dot{\mathbf{W}}_{i+1}^h + M_i^h \dot{\mathbf{V}}_i^h + \mathrm{ad}_{\dot{\omega}_i} M_i^h \dot{\mathbf{V}}_i^h $$

$$ \dot{\mathbf{W}}_i^h = \mathrm{Ad}_{\dot{\mathbf{d}}_{i+1,i}}^T \dot{\mathbf{W}}_{i+1}^h + \mathrm{ad}_{\dot{\mathbf{d}}_{i+1,i}}^T \mathbf{W}_{i+1}^h $$

$$ + M_i^h \ddot{\mathbf{V}}_i^h + \mathrm{ad}_{\dot{\omega}_i} M_i^h (\dot{\mathbf{V}}_i^h + \dot{\mathbf{V}}_i^h) $$

$$ + (\mathrm{ad}_{\ddot{\omega}_i}^2 + \mathrm{ad}_{\dot{\omega}_i}) M_i^h \dot{\mathbf{V}}_i^h - M_i^h \mathrm{ad}_{\dot{\omega}_i} \dot{\mathbf{V}}_i^h $$

$$ \ddot{\mathbf{W}}_i^h = \mathrm{Ad}_{\dot{\mathbf{d}}_{i+1,i}}^T \ddot{\mathbf{W}}_{i+1}^h + 2\mathrm{ad}_{\ddot{\mathbf{d}}_{i+1,i}}^T \dot{\mathbf{W}}_{i+1}^h + \mathrm{ad}_{\ddot{\mathbf{d}}_{i+1,i}}^T \mathbf{W}_{i+1}^h $$

$$ + M_i^h \dddot{\mathbf{V}}_i^h + 2(\mathrm{ad}_{\dot{\omega}_i} M_i^h - M_i^h \mathrm{ad}_{\dot{\omega}_i}) \ddot{\mathbf{V}}_i^h $$

$$ + (\mathrm{ad}_{\dot{\omega}_i} M_i^h - M_i^h \mathrm{ad}_{\dot{\omega}_i} + \mathrm{ad}_{\ddot{\omega}_i}^2 M_i^h + M_i^h \mathrm{ad}_{\ddot{\omega}_i}^2 $$

$$ - 2\mathrm{ad}_{\ddot{\omega}_i} M_i^h \mathrm{ad}_{\dot{\omega}_i} ) \dot{\mathbf{V}}_i^h + \mathrm{ad}_{\dot{\omega}_i} M_i^h \dot{\mathbf{V}}_i^h $$

$$ + ((2\mathrm{ad}_{\ddot{\omega}_i}^2 + 2\mathrm{ad}_{\dot{\omega}_i}) M_i^h - \mathrm{ad}_{\dot{\omega}_i} M_i^h \mathrm{ad}_{\dot{\omega}_i}) \dot{\mathbf{V}}_i^h $$

$$ + (\mathrm{ad}_{\ddot{\omega}_i}^3 + \mathrm{ad}_{\dot{\omega}_i} \mathrm{ad}_{\dot{\omega}_i} + \mathrm{ad}_{\ddot{\omega}_i}) M_i^h \dot{\mathbf{V}}_i^h $$

$$ Q_i = {}^0\mathbf{X}_i^T \mathbf{W}_i^h, \dot{Q}_i = {}^0\mathbf{X}_i^T (\dot{\mathbf{W}}_i^h - \mathrm{ad}_{\dot{\omega}_i} \mathbf{W}_i^h), $$

$$ \ddot{Q}_i = {}^0\mathbf{X}_i^T (\ddot{\mathbf{W}}_i^h - 2\mathrm{ad}_{\dot{\omega}_i} \dot{\mathbf{W}}_i^h + (\mathrm{ad}_{\ddot{\omega}_i}^2 - \mathrm{ad}_{\dot{\omega}_i}) \mathbf{W}_i^h) $$

* Output: $\mathbf{Q}, \dot{\mathbf{Q}}, \ddot{\mathbf{Q}}$

**Remark 3:** The computational costs for frame transformations of twists and joint screws are reduced compared to body-fixed version. However, the screw coordinate vector $^0\mathbf{X}_i$ is resolved in IFR and thus not constant. It is determined from its representation $^i\mathbf{X}_i$ in body-fixed frame by (9), which only involves translations.

## VI. EXAMPLE: INDUSTRIAL 6-DOF MANIPULATOR

Both algorithms were applied to a $n = 6$ DOF manipulator Stäubli TX90L whose geometric and dynamic parameters were identified [18]. The 'stretched' reference configuration
with $\mathbf{q} = \mathbf{0}$ is shown in figure 2. The body-fixed RFRs are located at the joint axes and oriented so that, for $\mathbf{q} = \mathbf{0}$, they are aligned with IFR. Then the joint screw coordinates $^i\mathbf{X}_i$ become very simple since the distance vectors to the joint axes are $^i\mathbf{x}_i = \mathbf{0}, i = 1, \dots, 6$. The unit vectors along the joint axes, expressed in the respective RFR, are $^1\mathbf{e}_1 = ^4\mathbf{e}_4 = ^6\mathbf{e}_6 = (0, 0, 1)^T$, $^2\mathbf{e}_2 = ^3\mathbf{e}_3 = ^5\mathbf{e}_5 = (0, 1, 0)^T$. Thus $^i\mathbf{X}_i = (^i\mathbf{e}_i, \mathbf{0})^T$ since all joints are revolute joints. Since all RFRs are initially aligned with the IFR, the relative reference configurations of the links attain the form

$$ \mathbf{B}_i = \begin{pmatrix} \mathbf{I} & \mathbf{r}_i \\ \mathbf{0} & 1 \end{pmatrix} \qquad (24) $$

so that only the relative displacements of the RFRs are needed. According to Fig. 1 these displacement vectors are

$$ \begin{aligned} \mathbf{r}_1 &= (0, 0, z_1)^T, \quad \mathbf{r}_2 = (x_2, y_2, z_2)^T, \quad \mathbf{r}_3 = (0, y_3, z_3)^T \\ \mathbf{r}_4 &= (0, -y_4, z_4)^T, \quad \mathbf{r}_5 = (0, 0, z_5)^T, \quad \mathbf{r}_6 = (0, 0, z_6)^T \end{aligned} $$

with the geometric data (in mm): $z_1 = 268$, $x_2 = 50$, $y_2 = 140$, $z_2 = 210$, $y_3 = 48$, $z_3 = 500$, $y_4 = 137$, $z_4 = 138$, $z_5 = 413$, $z_6 = 100$. The simplicity of this modeling approach is apparent, in contrast to DH modeling.

The inertia data w.r.t. the RFRs was determined as reported in [18] providing the body-fixed link mass matrices (13). Due to space the limitation, details are omitted here. A time optimal path respecting the limitations on accelerations and jerks is used as test scenario. Its duration is 0.27s. However, any trajectory can be used for the test as the purpose is not the actual trajectory planning or control, but merely to compare the algorithms. For completeness at least the joint trajectory and joint torques are shown in Fig. 3 and 4.

Both algorithms yield indeed the same solution of the higher-order forward kinematics and inverse dynamics problem (within the numerical accuracy). The algorithms were implemented in Matlab and run on a standard PC (Intel i7-4550U, 1.5 GHz). The equations were implemented without any optimization, e.g. avoiding multiplication with zeros etc. The average execution time for a run of the body-fixed inverse dynamics algorithm in section V.A is 2.5 ms. The average execution time for the hybrid version in section V.B was 2 ms. This indicates an increase of computation speed when using the hybrid representation. Notice that these time are only preliminary indicatives that will be smaller when the implementation is optimized and eventually the C-code is optimized. Moreover, since the body-fixed algorithm is simply the algorithm presented in [4] its optimized version will exhibit the same performance. What is important (besides the compactness) is the relative increase of speed obtained with the hybrid version, as it is known from the multibody dynamics algorithms [1], [13], [24].

## VII. CONCLUSIONS

The higher-order inverse dynamics is crucial for feedback linearizing control of robotic manipulators with series elastic actuators. Recently recursive $O(n)$ algorithms were proposed for efficient evaluation. In this paper two versions of such a recursive algorithm are presented that possess two

<figure><img src="image_3.png" /><figcaption>Fig. 2. Stäubli TX90 in its reference configuration with geometric parameters indicated.</figcaption></figure>

<figure><img src="image_4.png" /><figcaption>Fig. 3. Joint angles for the test trajectory.</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>Fig. 4. Joint torques required for the trajectory.</figcaption></figure>
main advantages: 1) the kinematics is formulated in terms of general and readily available geometric data (instead of DH parameters), and 2) the expressions are very compact, which makes their implementation less prone to implementation errors. Results are reported for a 6-DOF manipulator. It is concluded that the algorithm using the so-called hybrid representation performs best. Future work will focus on optimized implementations of these algorithms.

## APPENDIX: SCREWS AND RIGID BODY MOTIONS

Here only the basic concepts are given as far as necessary for the paper. Details can be found in [19], [25].

Rigid body configurations are represented by homogenous transformation matrices that form the Lie group SE (3). The configuration of body i, i.e. the configuration of its body-fixed frame $\mathcal{F}_i$ w.r.t., the IFR, is then denoted by the matrix

$$C_i = \begin{pmatrix} \mathbf{R}_i & \mathbf{r}_i \\ \mathbf{0} & 1 \end{pmatrix} \in SE(3) \quad (25)$$

with rotation matrix $\mathbf{R}_i \in SO(3)$ and position vector $\mathbf{r}_i \in \mathbb{R}^3$. The time evolution of $C_i(t)$ describes the spatial motion.

Rigid body twists and wrenches are coordinate invariant representations of rigid body velocities and couples. Twists are screws that form the Lie algebra $se(3)$ and wrenches are co-screws forming the dual $se^*(3)$. In a chosen reference frame, both are represented as 6-vectors. Denote with $^i\mathbf{X} \in \mathbb{R}^6$ the screw coordinate vector represented in body-fixed frame $\mathcal{F}_i$ and with $\mathbf{X} \in \mathbb{R}^6$ its representation in the spatial IFR. Screw coordinate vectors transform from body-fixed to spatial representation according to $\mathbf{X} = \mathbf{Ad}_{C_i}^i\mathbf{X}$, with

$$\mathbf{Ad}_{C_i} = \begin{pmatrix} \mathbf{R}_i & \mathbf{0} \\ \tilde{\mathbf{r}}_i \mathbf{R}_i & \mathbf{R}_i \end{pmatrix}. \quad (26)$$

The corresponding transformation of co-screws (wrenches) is $^i\mathbf{W} = \mathbf{Ad}_{C_i}^T \mathbf{W}$, where $^i\mathbf{W}, \mathbf{W} \in se^*(3)$ is the co-screw coordinate vector in body-fixed and spatial representation, respectively. The time derivative of screw and co-screw coordinates represented in moving frame $\mathcal{F}_i$ is respectively

$$\mathbf{Ad}_{C_i}^{-1} \dot{\mathbf{X}} = {}^i\dot{\mathbf{X}} + \mathbf{ad}_{\mathbf{V}_i^b} {}^i\mathbf{X}, \text{ for } \mathbf{X} \in se(3) \quad (27)$$

$$\mathbf{Ad}_{C_i}^{-T} \dot{\mathbf{W}} = {}^i\dot{\mathbf{W}} - \mathbf{ad}_{\mathbf{V}_i^b} {}^i\mathbf{W}, \text{ for } \mathbf{W} \in se^*(3) \quad (28)$$

Twists are represented as screw vectors $\mathbf{V} = (\omega, \mathbf{v})^T$. For convenience the following reduced vectors are introduced

$$\dot{\mathbf{V}} = \begin{pmatrix} \omega \\ \mathbf{0} \end{pmatrix}, \quad \mathbf{v} = \begin{pmatrix} \mathbf{0} \\ \mathbf{v} \end{pmatrix} \quad (29)$$

Denote with $\mathbf{X} = (\xi, \eta)^T$ a general screw coordinate vector. The Lie bracket (screw product) is given as

$$[\mathbf{X}_1, \mathbf{X}_2] = (\xi_1 \times \xi_2, \eta_1 \times \xi_2 + \xi_1 \times \eta_2)^T = \mathbf{ad}_{\mathbf{X}_1} \mathbf{X}_2 \quad (30)$$

with the matrix

$$\mathbf{ad}_{\mathbf{X}} = \begin{pmatrix} \tilde{\xi} & \mathbf{0} \\ \tilde{\eta} & \tilde{\xi} \end{pmatrix}. \quad (31)$$

This operation is occasionally called the 'generalized cross product' [10], [24]. With slight abuse of notation the following transformation matrices are used

$$\mathbf{Ad}_{\mathbf{R}_i} = \begin{pmatrix} \mathbf{R}_i & \mathbf{0} \\ \mathbf{0} & \mathbf{R}_i \end{pmatrix}, \quad \mathbf{Ad}_{\mathbf{r}_i} = \begin{pmatrix} \mathbf{I} & \mathbf{0} \\ \tilde{\mathbf{r}}_i & \mathbf{I} \end{pmatrix} \quad (32)$$

$$\mathbf{ad}_{\xi} = \begin{pmatrix} \tilde{\xi} & \mathbf{0} \\ \mathbf{0} & \tilde{\xi} \end{pmatrix}, \quad \mathbf{ad}_{\eta} = \begin{pmatrix} \mathbf{0} & \mathbf{0} \\ \tilde{\eta} & \mathbf{0} \end{pmatrix}. \quad (33)$$

## REFERENCES

[1] D.S. Bae, R.S. Hwang, E.J. Haug: A Recursive Formulation for Real-Time Dynamic Simulation of Mechanical Systems, ASME Journal of Mechanical Design, 113(2), 1988, 158-166

[2] R. W. Brockett: Robotic manipulators and the product of exponentials formula, Mathematical Theory of Networks and Systems, Lecture Notes in Control and Information Sciences, 58, 1984, pp. 120-129

[3] 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

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

[5] G. Chen, H. Wang, Z. Lin: Determination of the Identifiable Parameters in Robot Calibration Based on the POE Formula, IEEE Trans. Rob., 30(5), 2014, 1066-1077

[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] H. Gattringer, et al.: Recursive methods in control of flexible joint manipulators, Multibody Syst Dyn. 32, 2014, 117-131

[8] 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

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

[10] R. Featherstone: Rigid Body Dynamics Algorithms, Springer, 2008

[11] S.A. Hayati: Robot arm geometric link parameter estimation, 22nd IEEE Conference on Decision and Control, 1983, 1477-1483

[12] Y. Hsu, K.S. Anderson: Recursive sensitivity analysis for constrained multi-rigid-body dynamic systems design optimization, Struct. Multi-disc. Optim. 24, 2002, 312-324

[13] W. Lilly, D.E. Orin: Alternate Formulations for the Manipulator Inertia Matrix, Int. J. Rob. Res., 10(1), 1991, 64-74

[14] A. Müller, P. Maisser: Lie group formulation of kinematics and dynamics of constrained MBS and its application to analytical mechanics, Multibody System Dynamics, 9, 2003, 311-352

[15] A. Müller: Partial Derivatives of the inverse Mass Matrix of Multibody Systems via its Factorization, IEEE Trans. Rob., 23(1), 2007, 164-168

[16] A. Müller: Higher Derivatives of the Kinematic Mapping and some Applications, Mechanism and Machine Theory, 76, 2014, Pages 70-85

[17] A. Müller: MBS Motion Equation without explicit definition of Body-Fixed Reference Frames, 38 Mechanisms and Robotics Conference (MECH), ASME 2014 International Design Engineering Technical Conferences, August 17-20, 2014, Buffalo, NY

[18] M. Neubauer, H. Gattringer, H. Bremer: A persistent method for parameter identification of a seven-axes manipulator, Robotica, 33, 2015, 1099-1112

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

[20] D. Orin, W. Schrader: Efficient computation of the Jacobian for robot manipulators, Int. J. Robotics Research, 3(4), 1984

[21] 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

[22] F. C. Park, J. E. Bobrow, S. R. Ploen: A Lie group formulation of robot dynamics, Int. J. Rob. Research, Vol. 14, No. 6, 1995, 609-618

[23] S.R. Ploen, F.C. Park: Coordinate-Invariant Algorithms for Robot Dynamics, IEEE Tran. Rob. Aut., Vol. 15, No. 6, 1999, 1130-1135

[24] G. Rodriguez, A. Jain, K. Kreutz-Delgado: A Spatial Operator Algebra for Manipulator Modelling and Control, Int. J. Robotics Research, 10(4), 1991, 371-381

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

[26] K.J. Waldron: Geometrically based manipulator rate control algorithms. Mech. Mach. Theory, 17(6), 1982, 379-385