Professional Documents
Culture Documents
Flobaroid PDF
Flobaroid PDF
Flobaroid PDF
net/publication/315716936
CITATIONS READS
7 227
4 authors, including:
Some of the authors of this publication are also working on these related projects:
All content following this page was uploaded by Stefan Bethge on 17 February 2020.
1 Introduction
Robot dynamics simulations and model based controllers require precise model
knowledge. The Newton-Euler or Lagrange formalisms allow to derive the equa-
tions 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 dis-
tributions including electrical wires and electronics boards. Manufacturing and
assembly tolerances contribute additional inaccuracies along with unknown fric-
tion 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 iden-
tification 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
2 Stefan Bethge et al.
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 art1 . Flo-
BaRoID can generate optimized identification trajectories (but also works with
suboptimal excitations), pre-processes the sensor readings and finds inertial pa-
rameters 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 con-
tact 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 iden-
tification 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: Section 2 gives an overview of the pa-
rameter identification problem while Sections 3, 4 and 5 describe trajectory op-
timization, data preprocessing, and identification methods. Section 6 evaluates
the performance of exemplary identification experiments.
2 Problem Definition
For a system with nlinks bodies connected by ndof joints, the equations of motion
for the rigid multi-body dynamics have the form:
X
τ + ρ = H(q)q̈ + C(q, q̇)q̇ + g(q) + d(q̇) + JlT flext , (1)
l∈L
with:
q : joint position vector (R6+ndof )
τ : joint torque vector (R6+ndof )
ρ : measurement error (R6+ndof )
H : joint-space inertia matrix (R6+ndof ×6+ndof )
C(q, q̇)q̇ : Coriolis and centrifugal torques (R6+ndof , C ∈ R6+ndof ×ndof )
g(q) : Gravity torques (R6+ndof )
d(q̇) : Frictional damping torques (Rndof )
Jl : Jacobian of the contact frame l (R6×6+ndof )
flext : Wrench exerted by external environment on link l (R6 )
1
Source and Documentation is available at https://github.com/kjyv/flobaroid
FloBaRoID 3
The equations can be brought into a parameter linear form[2] with the re-
gressor matrix Y (q, q̇, q̈) and the inertial parameter vector x:
X
τ + ρ = Y (q, q̇, q̈)x + JlT flext (2)
l∈L
and:
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 (mi ),
the three values of the first moment of mass (cix , ciy , ciz ), the six independent
entries for the 3 × 3 link inertia tensor expressed at the origin of the link frame
(Iixx , Iixy , Iixz , Iiyy , Iiyz , Iizz ). Additionally for each joint j attached between
link j − 1 and j, there is one direction dependent constant friction parameter
(F cj ) and one parameter for velocity dependent friction (F vj )[6]. The parameter
identification problem is then:
!
2
X
min
τ − JlT flext − Y x
. (3)
x
l∈L
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 ρ displays a zero-mean Gaussian probability distribution, (2)
can be solved in the least squares sense to obtain an accurate estimate of the iner-
tial parameters x (in practice however only when Y has full rank, cf. Section 5).
Based on a suitable trajectory parametrization, the trajectory generation can be
formulated as an optimization problem. FloBaRoID uses a smooth periodic rep-
resentation of joint space trajectories by means of parametrized Fourier-series[7].
A global particle swarm optimizer (ALPSO), optionally followed by a local gradi-
ent 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 opti-
mization can only be used for fixed-based robots at the moment. Finding op-
timal 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.
4 Stefan Bethge et al.
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 con-
tact 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 Y + of the regressor matrix 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 Y .
2. Other parameters cannot be identified independently as their columns in 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.
Yb = Y B . (4)
The base regressor matrix Yb is the surjective projection of the standard regressor
matrix Y to a linear independent subspace using a projection matrix B. There-
fore, Yb comprises a non-unique choice of linearly independent columns of Y . The
FloBaRoID 5
with
(τ − Ybase Bxstd )T
u
Uτ (u, xstd ) = .
τ − Ybase Bxstd 1
The matrix Uτ is the Schur complement of:
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,
Pisa2 , 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 Bielefeld3 is used. It serves as prior knowledge during the identifica-
tion, 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 re-
spect 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 identi-
fied 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 consis-
tent solutions as described in Section 5.2. For this example, the link masses are
2
https://github.com/CentroEPiaggio/kuka-lwr/
3
https://github.com/corlab/cogimon-gazebo-models
FloBaRoID 7
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.
constrained to an interval of ±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.
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.
8 Stefan Bethge et al.
The identified standard parameters reduce the normalized RMS torque pre-
diction error from 0.90% with the parameters from [12] to 0.29% on the identifi-
cation 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 Figure 1
and identified parameters in barycentric form are given in Table 2.
While the measured data is predicted accurately for smooth motions, direc-
tion changes can produce physical effects that are not modeled such as static
friction or backlash. The maximum amplitude of the errors is within ±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 expe-
rienced 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.
40
10
τ1 (Nm)
τ2 (Nm)
τ3 (Nm)
5
20
0 0
0
−20
−10
−5 −40
0 2 4 6 8 10 0 2 4 6 8 10 0 2 4 6 8 10
10 1 1
τ4 (Nm)
τ5 (Nm)
τ6 (Nm)
0 0
0
−1 −1
−10 −2 −2
0 2 4 6 8 10 0 2 4 6 8 10 0 2 4 6 8 10
0.5
Maxima of absolute errors, τ1 : 1.36 Nm (0.38%),
τ7 (Nm)
0
τ2 : 2.23 Nm (0.63 %), τ3 : 1.69 Nm (0.84 %), τ4 :
−0.5 2.16 Nm (1.08 %), τ5 : 1.31 Nm (0.64 %), τ6 : 1.21
−1 Nm (1.60 %), τ7 : 0.43 Nm (0.57 %).
0 2 4 6 8 10
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.
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 op-
timization 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.
10 REFERENCES
References
[1] J. Wu, J. Wang, and Z. You, “An overview of dynamic parameter identi-
fication of robots,” Robotics and Computer-Integrated Manufacturing, vol.
26, no. 5, pp. 414–419, 2010.
[2] C. G. Atkeson, C. H. An, and J. M. Hollerbach, “Estimation of inertial
parameters of manipulator loads and links,” The International Journal of
Robotics Research, vol. 5, no. 3, pp. 101–119, 1986.
[3] N. G. Tsagarakis, D. G. Caldwell, A. Bicchi, F. Negrello, M. Garabini,
W. Choi, L. Baccelliere, V. Loc, J. Noorden, M. Catalano, et al., “Walk-
man: A high performance humanoid platform for realistic environments,”
Journal of Field Robotics (JFR), 2016.
[4] C. D. Sousa and R. Cortesão, “Physical feasibility of robot base inertial
parameter identification: A linear matrix inequality approach,” The Inter-
national Journal of Robotics Research, vol. 33, no. 6, pp. 931–944, 2014.
[5] F. Nori, S. Traversaro, J. Eljaik, F. Romano, A. Del Prete, and D. Pucci,
“Icub whole-body control through force regulation on rigid non-coplanar
contacts,” Frontiers in Robotics and AI, vol. 2, p. 6, 2015.
[6] P. Dupont, “Friction modeling in dynamic robot simulation,” Proceedings.,
IEEE International Conference on Robotics and Automation, pp. 1370–
1376, 1990.
[7] J. Swevers, C. Ganseman, D. Bilgin Tükel, J. De Schutter, and H. Van
Brüssel, “Optimal robot excitation and identification,” IEEE Transactions
on Robotics and Automation, vol. 13, no. 5, pp. 730–740, 1997.
[8] V. Bonnet, P. Fraisse, A. Crosnier, M. Gautier, A. González, and G. Ven-
ture, “Optimal Exciting Dance for Identifying Inertial Parameters of an
Anthropomorphic Structure,” IEEE Transactions on Robotics, vol. 32, no.
4, pp. 823–836, 2016.
[9] A. Del Prete, N. Mansard, O. E. Ramos, O. Stasse, and F. Nori, “Imple-
menting Torque Control with High-Ratio Gear Boxes and without Joint-
Torque Sensors,” International Journal of Humanoid Robotics (submitted
to), vol. 13, no. 1, pp. 1–29, 2015.
[10] H. Mayeda, K. Yoshida, and K. Osuka, “Base parameters of manipulator
dynamic models,” IEEE Transactions on Robotics and Automation, vol.
6, no. 3, pp. 312–321, Jun. 1990.
[11] G. Zak, B. Benhabib, R. G. Fenton, and I. Saban, “Application of the
Weighted Least Squares Parameter Estimation Method to the Robot Cal-
ibration,” Journal of mechanical design (1990), vol. 116, no. 3, p. 890,
1994.
[12] C. Gaz and A. D. Luca, “Extracting Feasible Robot Parameters from Dy-
namic Coefficients using Nonlinear Optimization Methods,” Proceedings.,
IEEE International Conference on Robotics and Automation, pp. 2075–
2081, 2016.