FloBaRoID — A Software Package for the Identification of Robot Dynamics Parameters

Abstract

This paper presents the open software package FloBaRoID (FLOating BAse RObot dynamical IDentification), which aims to provide a package implementing all necessary methods to identify robot dynamics parameters starting from a kinematic model. The package features data acquisition and preprocessing, extraction of identifiable base dynamics parameters, and finding physically consistent dynamics parameters for stable control and simulation. The paper details each of these steps and exemplifies the software usage with experimental results for the 7-DOF robot Kuka LWR 4+.

1 Introduction

Robot dynamics simulations and model based controllers require precise model knowledge. The Newton-Euler or Lagrange formalisms allow to derive the equations of motion that describe the dynamics. The inertial parameters appearing in this model are typically not provided by robot manufacturers. For custom built robot prototypes they can be extracted from thoroughly maintained CAD data. Although for the actuators and the structural robot components this is feasible, it is much more difficult and time consuming to precisely match the mass distributions including electrical wires and electronics boards. Manufacturing and assembly tolerances contribute additional inaccuracies along with unknown friction parameters. Especially robot prototypes are subject to frequent hardware revisions and in situ bugfixes or extensions that aggravate CAD maintenance. For these systems, an experimental identification from motion and force or torque measurements is an alternative and can also be used to refine CAD models to improve the force/torque prediction accuracy.

In the past decades, a substantial amount of research on this topic has been conducted and the reader is referred to [1] for a recent literature overview. The most popular approach is the linear regression approach [2] on which FloBaRoID is based. The related closed source project SYMORO+ allows calculating identification models only for fixed-base dynamics and does not include methods to obtain any parameters. The open source successor OpenSYMORO adds floating-base dynamics. Both expect the kinematic model defined in Denavit-Hartenberg parameters, which are not practically suited for all robot types, such as in the case of e.g. Walk-Man [3].

The aim of FloBaRoID is to provide a generic tool that implements state of the art dynamic parameter identification techniques using e.g. linear convex optimization [4] for arbitrary fixed and floating-base robots. FloBaRoID is open source to allow open community research also beyond the state of the art¹. FloBaRoID can generate optimized identification trajectories (but also works with suboptimal excitations), pre-processes the sensor readings and finds inertial parameters ensuring physical consistency. It supports general kinematic models and reads robot descriptions in the Unified Robot Description Format (URDF), supports floating-base dynamics for mobile robots and includes external contact forces. The software is implemented in the Python programming language. For the numerical fixed and floating-base dynamics computations, the iDynTree library is used [5]. While FloBaRoID can greatly simplify the dynamics identification task, it cannot provide a one-click solution. The user is required to understand the mathematical nature and limitations of the inertial parameter identification problem for multi-body systems. Therefore this paper comprises a brief introduction into the topic and describes how each step is handled by FloBaRoID.

The paper is organized as follows: Sect. 2 gives an overview of the parameter identification problem while Sects. 3, 4 and 5 describe trajectory optimization, data preprocessing, and identification methods. Section 6 evaluates the performance of exemplary identification experiments.

2 Problem Definition

For a system with $n_\text {links}$ bodies connected by $n_\text {dof}$ joints, the equations of motion for the rigid multi-body dynamics have the form:

$$\begin{aligned} \varvec{\tau } + \varvec{\rho } = \varvec{H}( \varvec{q} ) \varvec{\ddot{q}} +\varvec{C}( \varvec{q}, \varvec{\dot{q}} ) \varvec{\dot{q}} + \varvec{g}( \varvec{q} ) + \varvec{d} (\varvec{\dot{q}}) + \sum _{l \in L} \varvec{J}_{l}^T \varvec{f}_l^{ext}, \end{aligned}$$

(1)

with:

$$\begin{aligned} \begin{array}{rll} \varvec{q} &{}: \text {joint position vector } (\mathbb {R}^{6+n_\text {dof}}) \\ \varvec{\tau } &{}: \text {joint torque vector } (\mathbb {R}^{6+n_\text {dof}}) \\ \varvec{\rho } &{}: \text {measurement error } (\mathbb {R}^{6+n_\text {dof}}) \\ \varvec{H} &{}: \text {joint-space inertia matrix } (\mathbb {R}^{6+n_\text {dof} \times 6+n_\text {dof}}) \\ \varvec{C}( \varvec{q}, \varvec{\dot{q}} ) \varvec{\dot{q}} &{}: \text {Coriolis and centrifugal torques } (\mathbb {R}^{6+n_{dof}}, C \in \mathbb {R}^{6+n_{dof} \times n_{dof}}) \\ \varvec{g}( \varvec{q} ) &{}: \text {Gravity torques } (\mathbb {R}^{6+n_{dof}}) \\ \varvec{d} (\varvec{\dot{q}}) &{}: \text {Frictional damping torques } (\mathbb {R}^{n_{dof}}) \\ \varvec{J}_{l} &{}: \text {Jacobian of the contact frame { l} } (\mathbb {R}^{6 \times 6+n_{dof}}) \\ \varvec{f}_l^\text {ext} &{}: \text {Wrench exerted by external environment on link l } (\mathbb {R}^{6}) \\ \end{array} \end{aligned}$$

The equations can be brought into a parameter linear form [2] with the regressor matrix $\varvec{Y( \varvec{q}, \varvec{\dot{q}} , \varvec{\ddot{q}} ) }$ and the inertial parameter vector $\varvec{x}$:

$$\begin{aligned} \varvec{\tau } + \varvec{\rho } = \varvec{Y( \varvec{q}, \varvec{\dot{q}} , \varvec{\ddot{q}} )} \varvec{x} + \sum _{l \in L} \varvec{J}_{l}^T \varvec{f}_l^{ext} \end{aligned}$$

(2)

and:

$$\begin{aligned} \begin{array}{rll} \varvec{Y( \varvec{q}, \varvec{\dot{q}} , \varvec{\ddot{q}} )} &{}: \text {dynamics regressor } (\mathbb {R}^{6+n_\text {dof} \times (10\cdot n_\text {links} + 2\cdot n_\text {dof})}), \\ \varvec{x} &{}: \text {inertial parameter vector } (\mathbb {R}^{10\cdot n_\text {links} + 2\cdot n_\text {dof}}). \end{array} \end{aligned}$$

For simplicity, the arguments are omitted when it is obvious in the following. The 10 standard inertial parameters for each link i consist of the link mass ($m_i$), the three values of the first moment of mass ($c_{ix}$, $c_{iy}$, $c_{iz}$), the six independent entries for the $3\times 3$ link inertia tensor expressed at the origin of the link frame ($I_{ixx}$, $I_{ixy}$, $I_{ixz}$, $I_{iyy}$, $I_{iyz}$, $I_{izz}$). Additionally for each joint j attached between link $j-1$ and j, there is one direction dependent constant friction parameter ($Fc_j$) and one parameter for velocity dependent friction ($Fv_j$) [6]. The parameter identification problem is then:

$$\begin{aligned} \underset{x}{\min }{\left\| \left( \varvec{\tau } - \sum _{l \in L} \varvec{J}_{l}^T \varvec{f}_l^{ext}\right) -\varvec{Y} \, \varvec{x}\right\| ^2}. \end{aligned}$$

(3)

3 Optimized Trajectories

The trajectories executed by the robot for experimental identification should be designed such that (3) is a well-conditioned problem. Under the assumption that measurement error $\varvec{\rho }$ displays a zero-mean Gaussian probability distribution, (2) can be solved in the least squares sense to obtain an accurate estimate of the inertial parameters $\varvec{x}$ (in practice however only when $\varvec{Y}$ has full rank, cf. Sect. 5). Based on a suitable trajectory parametrization, the trajectory generation can be formulated as an optimization problem. FloBaRoID uses a smooth periodic representation of joint space trajectories by means of parametrized Fourier-series [7]. A global particle swarm optimizer (ALPSO), optionally followed by a local gradient based solver (IPOPT), searches for optimal trajectory parameters. Values for the trajectory boundaries in terms of joint angle limits, maximum velocity and torque limits (using torques estimated from CAD parameters as upper bound) are taken from URDF to constrain the search space and reduce computation time.

Calculating the dynamics for each step of the parameter optimization is done using iDynTree, which does not handle external contacts and as such the optimization can only be used for fixed-based robots at the moment. Finding optimal trajectories for floating-base robots such as humanoids is a problem of much higher complexity and is not easily integrated into a generic package. A possible approach is described in [8]. However, using data from existing balanced movements, parameters for floating-base robots can also be identified.

FloBaRoID interfaces with the robot middlewares YARP and ROS, so that trajectories can be executed on a real robot with minimal efforts. The open architecture of FloBaRoID generally allows the extension to other robot control interfaces upon demand.

4 Preprocessing

The data to be acquired comprises joint positions and torques as well as contact forces, if available. Numerical differentiation yields velocity and acceleration signals. Zero phase forward-backward digital filtering using a Butterworth filter reduces signal and differentiation noise. The filter cut-off frequencies may need to be changed from the defaults in order to not filter out signal frequencies or to reduce noise further, as the optimal values depend on the individual robot mass, operating velocities and sensor properties. For floating base robots, the base link orientation, linear acceleration and rotational velocity readings are derived from inertial measurement unit data. The rotational acceleration is derived through numerical differentiation and filtering as described above. The base link linear velocity does not influence the joint torques [9] and can be ignored.

5 Identification

A straight forward method to solve (3) is the ordinary least squares estimation (OLS) method based on the pseudoinverse $\varvec{Y}^+$ of the regressor matrix $\varvec{Y}$ in (2). For most practical robots, this is however not directly possible for two reasons:

• 1. Not all of the standard inertial parameters actually influence the dynamics. They are called non-identifiable parameters and correspond to zero columns in $\varvec{Y}$.

• 2. Other parameters cannot be identified independently as their columns in $\varvec{Y}$ are linear dependent. These parameters can only be identified together and individual values are unknown.

Both reasons cause the regressor matrix to be rank deficient and the problem is singular.

5.1 Base Parameter Identification

To resolve this issue, [10] introduces the base regressor $\varvec{Y}_b$ with:

$$\begin{aligned} \varvec{Y}_b = \varvec{Y} \varvec{B}. \end{aligned}$$

(4)

The base regressor matrix $\varvec{Y}_b$ is the surjective projection of the standard regressor matrix $\varvec{Y}$ to a linear independent subspace using a projection matrix $\varvec{B}$. Therefore, $\varvec{Y}_b$ comprises a non-unique choice of linearly independent columns of $\varvec{Y}$. The corresponding non-unique choice of base parameters is $\varvec{x}_b$ with $\dim {\varvec{x}_b} \le \dim {\varvec{x}}$. After projection, (2) can be restated and solved for the set of base parameters:

$$\begin{aligned} \varvec{x}_b = \varvec{Y}_b^+\ (\varvec{\tau } + \varvec{\rho }) + \varvec{Y}_b^+\sum _{l \in L} \varvec{J}_{l}^T \varvec{f}_l^{ext}, \end{aligned}$$

(5)

minimizing the torque prediction error:

$$\begin{aligned} \underset{\varvec{x}_b}{\min }{\left\| \varvec{\tau }-\varvec{Y}_b \, \varvec{x}_b\right\| ^2}. \end{aligned}$$

(6)

5.2 Standard Parameter Identification

The identified base parameter vector $\varvec{x}_b$ is sufficient to predict joint torque readings. However, in order to update URDF files and improve simulators, it is beneficial to retrieve a full set of standard inertial parameters $\varvec{x}$. Since the projection (4) is not bijective, there is no unique solution for $\varvec{x}$ and among the possible solutions, not all are physically consistent.

A standard parameter vector $\varvec{x}$ could be obtained from:

$$\begin{aligned} \varvec{x} = \varvec{B} \varvec{x}_b \end{aligned}$$

(7)

and this vector would still minimize the torque prediction error (3). However it likely includes physically inconsistent parameter values such as negative masses, center of mass coordinates far outside the link body or inertia values that do not yield positive definite link inertia matrices. Such values threaten model based controller and simulation stability.

5.3 Physical Consistency Through Constrained Optimization

Improving the data quality and operation space coverage for instance through trajectory optimization, better sensors and Weighted Least Squares techniques (WLS, [11]) based on the expected parameter value magnitude or estimated covariance, can reduce the likelihood of arriving at inconsistent parameter values. Nevertheless, the only way to guarantee physical consistency of the obtained parameter values is to translate physical consistency into mathematical constraints, while solving (6) as a constrained optimization problem.

FloBaRoID implements a method using semidefinite programming (SDP) [4], which solves the constrained optimization problem directly for (3) in the standard parameter space. This way, physically consistent solutions $\varvec{x}$ are found that minimize the estimation error. Alternative successful non-linear optimization approaches are reported in the literature but they cannot formally guarantee to find a global minimum of the estimation error [4].

Using the projection matrix $\varvec{B}$ that combines the standard columns to the base columns, FloBaRoID identifies physically consistent standard parameters directly using the SDP program:

$$\begin{aligned} \begin{array}{llll} &{} \underset{(u, \varvec{B} \varvec{x}_{std})}{\text {minimize}} &{} &{} u \\ &{} \text {subject to} &{} &{} \varvec{U_{\tau }}(u, \varvec{B} \varvec{x}_{std}) \succeq 0 \,, \end{array} \end{aligned}$$

with

$$\begin{aligned} \varvec{U_{\tau }}(u,\varvec{x}_{std}) = \left[ \begin{matrix} u &{} (\varvec{\tau } - \varvec{Y}_{base} \varvec{B} \varvec{x}_{std})^T \\ \tau - \varvec{Y}_{base} \varvec{B} \varvec{x}_{std} &{} 1 \\ \end{matrix} \right] . \end{aligned}$$

The matrix $\varvec{U_\tau }$ is the Schur complement of:

$$\begin{aligned} u \ge (\varvec{\tau }-\varvec{Y}_{base}\varvec{B}\varvec{x}_{std})^T(\varvec{\tau }-\varvec{Y}_{base}\varvec{B}\varvec{x}_{std}), \end{aligned}$$

(8)

which is equal to minimizing u subject to $u \ge \left\| \varvec{\tau }-\varvec{Y}_{base}\varvec{B}\varvec{x}_{std}\right\| ^2$.

As the non-identifiable parameters are not expressed in the measurement data, it is in general hard to find the exact parameters of the physical bodies, regardless of the measurement data quality. Results can be improved depending on the chosen constraints, thereby also influencing non-identifiable parameters. In addition to physical consistency, other constraints such as boundaries using previously known values from CAD data, directly measured parameters, symmetries or otherwise known values can be included as well.

6 Application Examples

This section exemplifies the dynamic parameter identification with FloBaRoID in two stages based on the Kuka LWR 4+ robot arm. The first stage is an identification performed in an ideal simulation environment. The second example is based on experimental data acquired on the real hardware.

6.1 Simulations

Two URDF files with standard parameters of a Kuka LWR 4+ arm are used for simulations. One is part of a package that is available from Centre E. Piaggio, Pisa², which also allows to connect the Kuka FRI library to standard ROS tools. This URDF represents the “real” parameters to be identified in the simulation. A second URDF file with quite different mass distribution from the CoR-Lab at University of Bielefeld³ is used. It serves as prior knowledge during the identification, simulating knowledge from CAD data which allows to formulate parameter constraints in addition to the physical consistency constraints.

As it appears, both URDF files do not contain identified parameters, because many of the values have exact decimals. Regardless of the correctness with respect to the real hardware, the simulation objective is to demonstrate how well the difference in model parameters can be recovered through identification. The example leverages the fact that the simulated sensor readings are bias and noise free. With optimal excitation trajectories, the base parameters can be identified up to machine precision. The accuracy regarding the identified standard parameters depends on the constraints and the distance of the prior parameter values to the actual ones, because there are theoretically infinitely many consistent solutions as described in Sect. 5.2. For this example, the link masses are constrained to an interval of $\pm 50\%$ around the a priori masses, while the center of mass coordinates are constrained to the enclosing hulls of CAD meshes of each link. To excite the dynamics, the robot performs an optimized trajectory motion. A different trajectory from random parameters is used as validation dataset. Table 1 lists the identification result. Although not all parameters get identified exactly, the squared distance of the a priori parameter vector to the ground truth parameters is decreased from 1.6353 to 0.3552. At the same time the torque prediction normalized RMS error is reduced from $0.907 \%$ to $0.022 \%$ on the identification data set and to $0.04 \%$ on the validation data set.

Table 1. Identified standard parameters of the simulated Kuka LWR 4+. Non-identifiable parameters are marked with*. These have no effect on dynamics and are determined only to satisfy consistency constraints.

6.2 Experiments

For the experimental example with real measurements, the robot is controlled to follow the same optimized trajectory as before. As the true inertial parameters of the robot are unknown, only the torque prediction error on the validation data set can be evaluated as a measure of accuracy. In addition, the identified standard parameters are referenced against the identified parameters reported in [12], for which however the authors cannot confirm physical consistency.

The identified standard parameters reduce the normalized RMS torque prediction error from $0.90 \%$ with the parameters from [12] to $0.29 \%$ on the identification data set, while the validation error for a random trajectory was reduced from $0.81\%$ to $0.36\%$. The resulting torque estimations are shown in Fig. 1 and identified parameters in barycentric form are given in Table 2.

While the measured data is predicted accurately for smooth motions, direction changes can produce physical effects that are not modeled such as static friction or backlash. The maximum amplitude of the errors is within $\pm 2.23$ Nm or less for each joint, which corresponds to $1.6\%$ or less of the maximum motor torques of each joint, cf. Fig. 1. The differently large errors possibly result from differently large torques produced by the trajectory so that measurement and identification errors play a larger part. Also these joints might simply have experienced more wear and exhibit a higher amount of unmodelled effects, e.g. joint 5. Hence, modelling these effects may increase the overall estimation accuracy for this particular robot further, the overall error is however already small.

Fig. 1.

Plots of estimated (orange) and measured torques (green) and estimation error (blue, dashed) for each joint using optimized trajectories on a physical Kuka LWR 4+.

Table 2. Identified standard parameters including link side Coulomb and viscous friction for Kuka LWR 4+ compared to values from [12]. Parameters can also be found as URDF within the FloBaRoID git repository.

7 Conclusions and Outlook

The paper presents the robot dynamics identification software package FloBaRoID. It briefly introduces the reader into the topic of robot dynamics identification and exemplifies the software usage based on the dynamics identification of a Kuka LWR 4+ robot arm. The experimental identification example is referenced against the identified parameters reported by [12]. It has to be noted that the difference in the prediction error norms between the two parameter sets is likely partially due to parameter scattering among the different units of the same type. The units under consideration are produced at different times in different lots and have an entirely different operation history. However, the optimization method used in [12] does not guarantee finding global optima which the difference in accuracy can possibly also be attributed to.

The simulation and experimental example given in this paper demonstrate the potential and challenges associated with state of the art robot dynamics identification. The targeted scope of FloBaRoID is to support the identification of fixed-base and of tree-like floating-base robots such as humanoids with a high number of degrees of freedom. For such robots, additional challenges occur. For instance, classical excitation trajectory optimization approaches may lead to a loss of balance during the experiment. The integration of balancing constraints in the optimization as well as elasticity in the equations of motion are planned for future versions.