Table Of ContentIncremental Semiparametric Inverse Dynamics Learning
Raffaello Camoriano∗†, Silvio Traversaro‡,
Lorenzo Rosasco(cid:5), Giorgio Metta(cid:77), and Francesco Nori‡
TABLE I: Summary of related works on semiparametric or
Abstract—This paper presents a novel approach for incre-
mental semiparametric inverse dynamics learning. In partic- incremental robot dynamics learning.
ular, we consider the mixture of two approaches: Parametric
modeling based on rigid body dynamics equations and non- Author, Year Parametric Nonparametric
parametric modeling based on incremental kernel methods,
Nguyen-Tuong, 2010 [7] Batch Batch
6 with no prior information on the mechanical properties of
Gijsberts, 2011 [8] - Incremental
1 the system. This yields to an incremental semiparametric
Tingfan Wu, 2012 [9] Batch Batch
0 approach, leveraging the advantages of both the parametric
De La Cruz, 2012 [10] CAD∗ Incremental
2 andnonparametricmodels.Wevalidatetheproposedtechnique
Camoriano, 2015 Incremental Incremental
learningthedynamicsofonearmoftheiCubhumanoidrobot.
n
a ∗ In[10]theparametricpartisusedonlyforinitializingthenonparametric
J model.
I. INTRODUCTION
8
1 In order to control a robot a model describing the relation
no prior knowledge about the target function to be approxi-
between the actuator inputs, the interactions with the world
] and bodies accelerations is required. This model is called mated. Therefore, they need a sufficient amount of training
L
the dynamics model of the robot. A dynamics model can examples in order to produce accurate predictions on the
M
entire input space. If the learning phase has been performed
be obtained from first principles in mechanics, using the
. techniques of rigid body dynamics (RBD) [1], resulting in a offline, both approaches are sensitive to the variation of
t
a parametric model in which the values of physically mean- the mechanical properties over long time spans, which are
t mainly caused by temperature shifts and wear. Even the
s ingful parameters must be provided to complete the fixed
[ inertial parameters can change over time. For example if the
structure of the model. Alternatively, the dynamical model
robotgraspsaheavyobject,theresultingchangeindynamics
1 can be obtained from experimental data using Machine
v Learning techniques, resulting in a nonparametric model. can be described by a change of the inertial parameters
9 of the hand. A solution to this problem is to address the
Traditional dynamics parametric methods are based on
4 variations of the identified system properties by learning
several assumptions, such as rigidity of links or that friction
5
incrementally, continuously updating the model as long as
4 has a simple analytical form, which may not be accurate
newdatabecomesavailable.Inthispaperweproposeanovel
0 in real systems. On the other hand, nonparametric methods
. basedonalgorithmssuchasKernelRidgeRegression(KRR) technique that joins parametric and nonparametric model
1
learning in an incremental fashion.
0 [2],[3],[4],KernelRegularizedLeastSquares(KRLS)[5]or
6 Gaussian Processes [6] can model dynamics by extrapolat- Classical methods for physics-based dynamics modeling
1 ing the input-output relationship directly from the available can be found in [1]. These methods require to identify the
v: data1. If a suitable kernel function is chosen, then the mechanical parameters of the rigid bodies composing the
i nonparametric model is a universal approximator which can robot [11], [12], [13], [14], which can then be employed in
X
account for the dynamics effects which are not considered model-based control and state estimation schemes.
ar by the parametric model. Still, nonparametric models have In [7] the authors present a learning technique which
combines prior knowledge about the physical structure of
∗Correspondingauthor. the mechanical system and learning from available data
†Raffaello Camoriano is with iCub Facility, Istituto Italiano di Tec- with Gaussian Process Regression (GPR) [6]. A similar
nologia, Via Morego 30, Genoa 16163, Italy, and DIBRIS, Universita`
approach is presented in [9]. Both techniques require an
degli Studi di Genova, Via All’Opera Pia, 13, Genoa 16145, Italy. Email:
[email protected] offline training phase and are not incremental, limiting them
‡Silvio Traversaro and Francesco Nori are with RBCS Department, to scenarios in which the properties of the system do not
IstitutoItalianodiTecnologia,ViaMorego30,Genoa16163,Italy.Email:
[email protected] change significantly over time.
(cid:5)Lorenzo Rosasco is with LCSL, Istituto Italiano di Tecnologia and In [10] an incremental semiparametric robot dynamics
Massachusetts Institute of Technology, Cambridge, MA 02139, USA, and learning scheme based on Locally Weighted Projection Re-
DIBRIS, Universita` degli Studi di Genova, Via All’Opera Pia, 13, Genoa
16145,Italy.Email:[email protected] gression (LWPR) [15] is presented, that is initialized using
(cid:77)GiorgioMettaiswithiCubFacility,IstitutoItalianodiTecnologia,Via a linearized parametric model. However, this approach uses
Morego30,Genoa16163,Italy.Email:[email protected] a fixed parametric model, that is not updated as new data
1NotethatKRRandKRLShaveaverysimilarformulation,andthatthese
becomes available. Moreover, LWPR has been shown to
are also equivalent to the techniques derived from Gaussian Processes, as
explainedforinstanceinChapter6of[4]. underperform with respect to other methods (e.g. [8]).
In [8], a fully nonparametric incremental approach for • The Frobenius norm of either a vector or a matrix of
inverse dynamics learning with constant update complexity real numbers is denoted by (cid:107)·(cid:107).
is presented, based on kernel methods [16] (in particular • In ∈Rn×n denotes the identity matrix of dimension n;
KRR) and random features [17]. The incremental nature of 0 ∈ Rn denotes the zero column vector of dimen-
n
this approach allows for adaptation to changing conditions sion n; 0 ∈ Rn×m denotes the zero matrix of
n×m
in time. The authors also show that the proposed algorithm dimension n×m.
outperforms other methods such as LWPR, GPR and Local
B. Parametric Models of Robot Dynamics
Gaussian Processes (LGP) [18], both in terms of accuracy
Robot dynamics parametric models are used to represent
and prediction time. Nevertheless, the fully nonparametric
therelationconnectingthegeometricandinertialparameters
nature of this approach undermines the interpretability of
with some dynamic quantities that depend uniquely on the
the inverse dynamics model.
robot model. A typical example is obtained by writing the
Inthisworkweproposeamethodthatisincrementalwith
robot inverse dynamics equation in linear form with respect
fixed update complexity (as [8]) and semiparametric (as [7]
to the robot inertial parameters π:
and [9]). The fixed update complexity and prediction time
are key properties of our method, enabling real-time perfor- τ =M(q)q¨+C(q,q˙)q˙ +g(q)=Φ(x)π, (1)
mances.Boththeparametricandnonparametricpartscanbe
where:q∈Rndof isthevectorofjointpositions,τ ∈Rndof
updated,asopposedto[10]inwhichonlythenonparametric
is the vector of joint torques, π ∈ Rnp is the vector of the
partis.Acomparisonbetweentheexistingliteratureandour
identifiable (base) inertial parameters [1], Φ(x)∈Rndof×np
incremental method is reported in Table I. We validate the
is the “regressor”, i.e. a matrix that depends only on the
proposed method with experiments performed on an arm of
robot kinematic parameters. In the rest of the paper we will
the iCub humanoid robot [19].
indicatewithxthetriplegivenby(q,q˙,q¨).Otherparametric
models write different measurable quantities as a product of
a regressor and a vector of parameters, for example the total
energy of the robot [20], the istantaneous power provided to
the robot [21], the sum of all external forces acting on the
robot [22] or the center of pressure of the ground reaction
forces[23].Regardlessofthechoiceofthemeasuredvariable
y, the structure of the regressor is similar:
y=Φ(q,q˙,q¨)π =Φ(x)π, (2)
where y∈Rt is the measured quantity.
The π vector is composed of certain linear combinations
of the inertial parameters of the links, the base inertial
parameters [24]. In particular, the inertial parameters of
a single body are the mass m, the first moment of mass
mc ∈ R3 expressed in a body fixed frame and the inertia
matrix I ∈ R3×3 expressed in the orientation of the body
fixed frame and with respect to its origin.
Fig. 1: iCub learning its right arm dynamics. In parametric modeling of robot dynamics, the regressor
structure depends on the kinematic parameters of the robot,
The article is organized as follows. Section II introduces that are obtained from CAD models of the robot through
the existing techniques for parametric and nonparametric kinematic calibration techniques. Similarly, the inertial pa-
robot dynamics learning. In Section III, a complete descrip- rameters π can also be obtained from CAD models of
tion of the proposed semiparametric incremental learning the robot, however these models may be unavailable (for
techniqueisintroduced.SectionIVpresentsthevalidationof example) because the manufacturer of the robot does not
ourapproachontheiCubhumanoidroboticplatform.Finally, provide them. In this case the usual approach is to estimate
Section V summarizes the content of our work. π fromexperimentaldata[14].Todothat,givennmeasures
of the measured quantity y (with i = 1...n), stacking (2)
i
II. BACKGROUND for the n samples it is possible to write:
A. Notation
y Φ(x )
1 1
The following notation is used throughout the paper.
y2 Φ(x2)
• The set of real numbers is denoted by R. Let u and v .. = .. π. (3)
be two n-dimensional column vectors of real numbers . .
(unless specified otherwise), i.e. u,v ∈Rn, their inner yn Φ(xn)
product is denoted as u(cid:62)v, with “(cid:62)” the transpose This equation can then be solved in least squares (LS)
operator. sense to find an estimate πˆ of the base inertial parameters.
Given the training trajectories it is possible that not all where λ is called regularization parameter. The solution to
parameters in π can be estimated well as the problem in (6) exists and is unique. Following the representer theorem
(3) can be ill-posed, hence this equation is usually solved as [16], the solution can be conveniently expressed as
a Regularized Least Squares (RLS) problem. Defining
n
y Φ(x ) fˆλ(x)=(cid:88)αik(xi,x) (7)
1 1
y2 Φ(x2) i=1
yn = ... , Φn = ... , with α = (K +λIn)−1Y ∈ Rn×t, αi i-th row of α and
y Φ(x ) Y = (cid:2)y(cid:62),...,y(cid:62)(cid:3)(cid:62). It is therefore necessary to invert and
n n 1 n
storethekernelmatrixK ∈Rn×n,whichimpliesO(n3)and
the RLS problem that is solved for the parametric identifi- O(n2) time and memory complexities, respectively. Such
cation is:
complexities render the above-mentioned KRLS approach
πˆ =argmin(cid:0)(cid:107)Φ π−y (cid:107)2+λ(cid:107)π(cid:107)2(cid:1),λ>0. (4) prohibitive in settings where n is large, including the one
n n treated in this work. This limitation can be dealt with by
π∈Rnp
resorting to approximated methods such as random features,
C. Nonparametric Modeling with Kernel Methods which will now be described.
Consider a probability distribution ρ over the probability 1) RandomFeatureMapsforKernelApproximation: The
space X ×Y, where X ⊆ Rd is the input space (the space random features approach was first introduced in [17], and
of the d measured attributes) and Y ⊆ Rt is the output since then is has been widely applied in the field of large-
space (the space of the t outputs to be predicted). In a scale Machine Learning. This approach leverages the fact
nonparametricmodelingsetting,thegoalistofindafunction that the kernel function can be expressed as
f∗ :X →Y belonging to a set of measurable functions H,
k(x,x(cid:48))=(cid:104)φ(x),φ(x(cid:48))(cid:105) , (8)
called hypothesis space, such that H
(cid:90) where x,x(cid:48) ∈X are row vectors, φ:Rd →Rp is a feature
f∗ =argmin (cid:96)(f(x),y)dρ(x,y), (5)
map associated with the kernel, which maps the input points
f∈H X×Y
(cid:124) (cid:123)(cid:122) (cid:125) from the input space X to a feature space of dimensionality
E(f)
p ≤ +∞, depending on the chosen kernel. When p is very
where x ∈ X are row vectors, y ∈ Y, E(f) is called large, directly computing the inner product as in (8) enables
expected risk and (cid:96)(f(x),y) is the loss function. In the rest the computation of the solution, as we have seen for KRLS.
ofthiswork,wewillconsiderthesquaredloss(cid:96)(f(x),y)= However,K canbecometoocumbersometoinvertandstore
(cid:107)f(x)−y(cid:107)2. Note that the distribution ρ is unknown, and as n grows. A random feature map φ˜:Rd →RD, typically
that we assume to have access to a discrete and finite set of with D (cid:28) p, directly approximates the feature map φ, so
measured data points S = {x ,y }n of cardinality n, in that
i i i=1
whichthepointsareindependentlyandidenticallydistributed k(x,x(cid:48))=(cid:104)φ(x),φ(x(cid:48))(cid:105) ≈φ˜(x)φ˜(x(cid:48))(cid:62). (9)
H
(i.i.d.) according to ρ.
In the context of kernel methods [16], H is a reproducing D can be chosen according to the desired approximation
kernelHilbertspace(RKHS).AnRKHSisaHilbertspaceof accuracy, as guaranteed by the convergence bounds reported
functionssuchthat∃k :X×X →Rforwhichthefollowing in [17], [25]. In particular, we will use random Fourier
properties hold: features for approximating the Gaussian kernel
1) ∀x∈X k (·)=k(x,·)∈H
2) g(x)=(cid:104)g,kxx(cid:105)H∀g ∈H,x∈X , k(x,x(cid:48))=e−(cid:107)x−2σx2(cid:48)(cid:107)2. (10)
where(cid:104)·,·(cid:105)HindicatestheinnerproductinH.Thefunctionk The approximated feature map in this case is φ˜(x) =
isareproducingkernel,anditcanbeshowntobesymmetric (cid:2)eixω1,...,eixωD(cid:3), where
positive definite (SPD). We also define the kernel matrix
Kand∈poRsint×ivne sesm.ti.defiKnii,tje =(SPkS(Dx)i,∀xxj),,xwh∈ichXi,swsyitmhmi,ejtri∈c ω ∼p(ω)=(2π)−D2e−(cid:107)2ωσ(cid:107)22, (11)
i j
{1,...,n},n∈N+.
with ω ∈ Rd column vector. The fundamental theoretical
The optimization problem outlined in (5) can be ap-
resultonwhichrandomFourierfeaturesapproximationrelies
proachedempiricallybymeansofmanydifferentalgorithms,
is Bochner’s Theorem [26]. The latter states that if k(x,x(cid:48))
amongwhichoneofthemostwidelyusedisKernelRegular-
is a shift-invariant kernel on Rd, then k is positive definite
ized Least Squares (KRLS) [3], [5]. In KRLS, a regularized
solution fˆ :X →Y is found solving if and only if its Fourier transform p(ω) ≥ 0. If this holds,
λ
by the definition of Fourier transform we can write
(cid:32) n (cid:33)
fˆλ =argmin (cid:88)(cid:107)f(xi)−yi(cid:107)2+λ(cid:107)f(cid:107)2H ,λ>0, (6) k(x,x(cid:48))=k(x−x(cid:48))=(cid:90) p(ω)ei(x−x(cid:48))ωdω, (12)
f∈H i=1 Rd
which can be approximated by performing an empirical E. Recursive Regularized Least Squares (RRLS) with
average as follows: Cholesky Update
(cid:104) (cid:105)
k(x−x(cid:48))=E ei(x−x(cid:48))ω ≈ Inscenariosinwhichsupervisedsamplesbecomeavailable
ω∼p (13) sequentially, a very useful extension of the RLS algorithm
≈ 1 (cid:80)D ei(x−x(cid:48))ω =φ˜(x)φ˜(x(cid:48))(cid:62).
D k=1 consists in the definition of an update rule for the model
Therefore, it is possible to map the input data as x˜ = whichallowsittobeincrementallytrained,increasingadap-
φ˜(x) ∈ RD, with x˜ row vector, to obtain a nonlinear and tivity to changes of the system properties through time.
nonparametric model of the form ThisalgorithmiscalledRecursiveRegularizedLeastSquares
n (RRLS). We will consider RRLS with the Cholesky update
f˜(x)=x˜W˜ ≈fˆλ(x)=(cid:88)αik(xi,x) (14) rule [27], which is numerically more stable than others (e.g.
i=1 the Sherman-Morrison-Woodbury update rule). In adaptive
approximating the exact kernelized solution fˆ(x), with filtering,thisupdateruleisknownastheQRalgorithm[28].
λ
W˜ ∈RD×t. Note that the approximated model is nonlinear Let us define A = Z(cid:62)Z + λI with λ > 0 and B =
b
in the input space, but linear in the random features space. Z(cid:62)U. Our goal is to update the model (fully described by
We can therefore introduce the regularized linear regression A and B) with a new supervised sample (z ,u ), with
k+1 k+1
problem in the random features space as follows: z ∈Rb, u ∈Rc row vectors.
k+1 k+1
(cid:16) (cid:17) Consider the Cholesky decomposition A = R(cid:62)R. It can
W˜λ = argmin (cid:107)X˜W˜ −Y(cid:107)2+λ(cid:107)W˜(cid:107)2 ,λ>0, (15)
always be obtained, since A is positive definite for λ > 0.
W˜∈Rd×t
Thus, we can express the update problem at step k+1 as:
where X˜ ∈Rn×D is the matrix of the training inputs where
each row has been mapped by φ˜. The main advantage of Ak+1 =Rk(cid:62)+1Rk+1
performing a random feature mapping is that it allows us =Ak+z(cid:62)k+1zk+1 (18)
to obtain a nonlinear model by applying linear regression =Rk(cid:62)Rk+z(cid:62)k+izk+1,
methods. For instance, Regularized Least Squares (RLS) √
can compute the solution W˜λ of (15) with O(nD2) time where R is full rank and unique, and R0 = λIb.
and O(D2) memory complexities. Once W˜λ is known, the By defining
predictionyˆ ∈R1×tforamappedsamplex˜canbecomputed (cid:20) R (cid:21)
as yˆ =x˜W˜λ. R˜k = z k ∈Rb+1×b, (19)
k+1
D. Regularized Least Squares
we can write A = R˜(cid:62)R˜ . However, in order to com-
Let Z ∈ Ra×b and U ∈ Ra×c be two matrices of k+1 k k
pute R from the obtained A it would be necessary
real numbers, with a,b,c ∈ N+. The Regularized Least to recokm+p1ute its Cholesky decomk+p1osition, requiring O(b3)
Squares (RLS) algorithm computes a regularized solution
computational time. There exists a procedure, based on
Wλ ∈Rb×c of the potentially ill-posed problem ZW =U,
Givens rotations, which can be used to compute R from
k+1
enforcingitsnumericalstability.Consideringthewidelyused R˜ with O(b2) time complexity. A recursive expression can
Tikhonov regularization scheme, Wλ ∈Rb×c is the solution k
be obtained also for B as follows:
k+1
to the following problem:
Wλ = argmin(cid:0)(cid:107)ZW −U(cid:107)2+λ(cid:107)W(cid:107)2(cid:1), λ>0, (16) Bk+1 =Zk(cid:62)+1Uk+1 (20)
W∈Rb×c(cid:124) (cid:123)(cid:122) (cid:125) =Zk(cid:62)Uk+z(cid:62)k+1uk+1.
J(W,λ)
OnceR andB areknown,theupdatedweightsmatrix
where λ is the regularization parameter. By taking the k+1 k+1
W can be obtained via back and forward substitution as
gradient of J(W,λ) with respect to W and equating it to k
zero, the minimizing solution can be written as W =R \(R(cid:62) \B ). (21)
k+1 k+1 k+1 k+1
Wλ =(Z(cid:62)Z+λI )−1Z(cid:62)U. (17)
b The time complexity for updating W is O(b2).
Both the parametric identification problem (4) and the AsforRLS,theRRLSincrementalsolutioncanbeapplied
nonparametric random features problem (15) are specific to both the parametric (4) and nonparametric with random
instances of the general problem (16). features(15)problems,assumingλ>0.Inparticular,RRLS
In particular, the parametric problem (4) is equivalent to
can be applied to the parametric case by noting that the
(16) with:
arrival of a new sample (Φ ,y ) adds t rows to Z =Φ
r r k r−1
Wλ =πˆ, Z =Φn, U =yn and Uk = yr−1. Consequently, the update of A must be
decomposed in t update steps using (20). For each one of
whiletherandomfeatureslearningproblem(15)isequivalent
these t steps we consider only one row of Φ and y(cid:62),
to (16) with: r r
namely:
Wλ =W˜λ, Z =X˜, U =Y.
z =(Φ ) , u =(y(cid:62)) , i=1...t
k+i r i k+i r i
Hence, both problems for a given set of n samples can be
solved applying (17). where (V) is the i-th row of the matrix V.
i
For the nonparametric random features case, RRLS can IV. EXPERIMENTALRESULTS
be simply applied with:
A. Software
For implementing the proposed algorithm we used two
z =x˜ , u =y .
k+1 r k+1 r existing open source libraries. For the RRLS learning part
we used GURLS [29], a regression and classification library
where (x˜ ,y ) is the supervised sample which becomes
r r based on the Regularized Least Squares (RLS) algorithm,
available at step r.
available for Matlab and C++. For the computations of the
regressors Φ(q,q˙,q¨) we used iDynTree2 , a C++ dynamics
library designed for free floating robots. Using SWIG [30],
III. SEMIPARAMETRICINCREMENTAL
iDynTree supports calling its algorithms in several program-
DYNAMICSLEARNING
ming languages, such as Python, Lua and Matlab. For pro-
ducing the presented results, we used the Matlab interfaces
We propose a semiparametric incremental inverse dy-
of iDynTree and GURLS.
namics estimator, designed to have better generalization
propertieswithrespecttofullyparametricandnonparametric B. Robotic Platform
ones, both in terms of accuracy and convergence rates.
The estimator, whose functioning is illustrated by the block
diagraminFigure2,iscomposedoftwomainparts.Thefirst
Upper arm
one is an incremental parametric estimator taking as input
Forearm
therigidbodydynamicsregressorsΦ(x)andcomputingtwo
(cid:1)
quantities at each step: (cid:63) (cid:1)
• An estimate yˆ of the output quantities of interest (cid:1)(cid:11)(cid:1)
• An estimate πˆ of the base inertial parameters of the (cid:0)(cid:0)(cid:18)
links composing the rigid body structure (cid:0)
(cid:0)
The employed learning algorithm is RRLS. Since it is su- (cid:0)
pervised, during the model update step the measured output FT sensor
y is used by the learning algorithm as ground truth. The
parametric estimation is performed in the first place, and
it is independent of the nonparametric part. This property Fig. 3: CAD drawing of the iCub arm used in the experi-
is desirable in order to give priority to the identification ments. The six-axis F/T sensor used for validation is visible
of the inertial parameters π. Moreover, being the estimator in the middle of the upper arm link.
incremental, the estimated inertial parameters πˆ adapt to
changes in the inertial properties of the links, which can
iCub is a full-body humanoid with 53 degrees of freedom
occur if the end-effector is holding a heavy object. Still, this
[19]. For validating the presented approach, we learned the
adaptationcannotaddresschangesinnonlineareffectswhich
dynamics of the right arm of the iCub as measured from
do not respect the rigid body assumptions.
the proximal six-axis force/torque (F/T) sensor embedded in
The second estimator is also RRLS-based, fully nonpara-
the arm. The considered output y is the reading of the F/T
metric and incremental. It leverages the approximation of
sensor,andtheinertialparametersπ arethebaseparameters
the kernel function via random Fourier features, as outlined
of the arm [31]. As y is not a input variable for the system,
in Section II-C.1, to obtain a nonlinear model which can
the output of the dynamic model is not directly usable for
be updated incrementally with constant update complexity
control, but it is still a proper benchmark for the dynamics
O(D2),whereDisthedimensionalityoftherandomfeature
learning problem, as also shown in [8]. Nevertheless, the
space (see Section II-E). This estimator receives as inputs
joint torques could be computed seamlessly from the F/T
the current vectorized x and yˆ, normalized and mapped
sensor readings if needed for control purposes, by applying
to the random features space approximating an infinite-
the method presented in [32].
dimensionalfeaturespaceintroducedbytheGaussiankernel.
The supervised output is the residual (cid:52)y = y − yˆ. The C. Validation
nonparametric estimator provides as output the estimate (cid:52)y˜
The aim of this section is to present the results of the ex-
of the residual, which is then added to yˆ to obtain the
perimentalvalidationoftheproposedsemiparametricmodel.
semiparametric estimate y˜. Similarly to the parametric part,
Themodelincludesaparametricpartwhichisbasedonphys-
in the nonparametric one the estimator’s internal nonlinear
ical modeling. This part is expected to provide acceptable
model can be updated during operation, which constitutes
prediction accuracy for the force components in the whole
an advantage in the case in which the robot has to explore
workspace of the robot, since it is based on prior knowledge
a previously unseen area of the state space, or when the
aboutthestructureoftherobotitself,whichdoesnotabruptly
mechanical conditions change (e.g. due to wear, tear or
temperature shifts). 2https://github.com/robotology/idyntree
πˆ y˜
q
x Φ(x) yˆ
q˙ x˜ (cid:52)y˜
+
q¨
f y − (cid:52)y
τ +
Fig. 2: Block diagram displaying the functioning of the proposed prioritized semiparametric inverse dynamics estimator. f
and τ indicate measured force and torque components, concatenated in the measured output vector y. The parametric part
is composed of the RBD regressor generator and of the parametric estimator based on RRLS. Its outputs are the estimated
parameters πˆ and the predicted output yˆ. The nonparametric part maps the input to the random features space with the
Random Features Mapper block, and the RFRRLS estimator predicts the residual output (cid:52)y˜, which is then added to the
parametric prediction yˆ to obtain the semiparametric prediction y˜.
change as the trajectory changes. On the other hand, the differentapproachesintheinitialtransientphase.Weobserve
nonparametricpartcanprovidehigherpredictionaccuracyin similarbehaviorsfortheforceandtorqueRMSEs.Afterfew
specific areas of the input space for a given trajectory, since seconds, the nonparametric (NP) and semiparametric (SP)
it also models nonrigid body dynamics effects by learning models provide more accurate predictions than the paramet-
directlyfromdata.Inordertoprovideempiricalfoundations ric (P) model with statistical significance. At regime, their
totheaboveinsights,avalidationexperimenthasbeensetup forcepredictionerrorisapproximately1N,whiletheoneof
usingthe rightarmof theiCubhumanoid robot,considering thePmodelisapproximatelytwotimeslarger.Similarly,the
as input the positions, velocities and accelerations of the 3 torque prediction error is 0.1Nm for SP and NP, which is
shoulder joints and of the elbow joint, and as outputs the 3 considerably better than the 0.4Nm average RMSE of the P
forceand3torquecomponentsmeasuredbythesix-axisF/T model.ItshallalsobenotedthatthemeanaverageRMSEof
sensor in-built in the upper arm. We employ two datasets the SP model is lower than the NP one, both for forces and
for this experiment, collected at 10Hz as the end-effector torques. However, this slight difference is not very signifi-
tracks (using the Cartesian controller presented in [33]) cant, since it is relatively small with respect to the standard
circumferenceswith10cmradiusonthetransverse(XY)and deviation.Giventheseexperimentalresults,wecanconclude
sagittal (XZ) planes3 at approximately 0.6m/s. The total thatintermsofpredictiveaccuracytheproposedincremental
number of points for each dataset is 10000, corresponding semiparametric method outperforms the incremental para-
to approximately 17 minutes of continuous operation. The metric one and matches the fully nonparametric one. The
steps of the validation experiment for the three models are SP method also shows a smaller standard deviation of the
the following: error with respect to the competing methods. Considering
1) Initialize the recursive parametric, nonparametric and the previous results and observations, the proposed method
semiparametricmodelstozero.Theinertialparameters has been shown to be able to combine the main advantages
are also initialized to zero ofparametricmodeling(i.e.interpretability)withtheonesof
2) Train the models on the whole XY dataset (10000 nonparametric modeling (i.e. capacity of modeling nonrigid
points) body dynamics phenomena). The incremental nature of the
3) Split the XZ dataset in 10 sequential parts of 1000 algorithm,inbothitsPandNPparts,allowsforadaptationto
samples each. Each part corresponds to 100 seconds changingconditionsoftherobotitselfandofthesurrounding
of continuous operation environment.
4) Test and update the models independently on the 10
V. CONCLUSIONS
splitted datasets, one sample at a time.
We presented a novel incremental semiparametric model-
In Figure 4 we present the means and standard deviations
ing approach for inverse dynamics learning, joining together
of the average root mean squared error (RMSE) of the
the advantages of parametric modeling derived from rigid
predicted force and torque components on the 10 different
body dynamics equations and of nonparametric Machine
test sets for the three models, averaged over a 3-seconds
Learning methods. A distinctive trait of the proposed ap-
sliding window. The x axis is reported in log-scale to
proach lies in its incremental nature, encompassing both
facilitate the comparison of predictive performance for the
the parametric and nonparametric parts and allowing for
the prioritized update of both the identified base inertial
3For more information on the iCub reference frames, see http://
eris.liralab.it/wiki/ICubForwardKinematics parameters and the nonparametric weights. Such feature
[5] R. Rifkin, G. Yeo, and T. Poggio, “Regularized least-squares classi-
3 SP Recursive fication,” Nato Science Series Sub Series III Computer and Systems
)secrof( E2.52 NPaPr aRmeceutrrisci vReec. [6] SPCcr.ioecnecEses.se,snRo.afs1om9r0u,sspeMpn.a1c3ha1inn–ed154,CL2e.0a0r3nK.in.g. I. MWITilliamPsr,ess,Gau2s0si0a6n.
SM1.5 [Online].Available:http://www.gaussianprocess.org/gpml;http://www.
R
e 1 bibsonomy.org/bibtex/257ca77b8164cba5c6a0ac94918219119/3mta3
g
are0.5 [7] D.Nguyen-TuongandJ.Peters,“Usingmodelknowledgeforlearning
vA 0 inversedynamics.”inICRA. IEEE,2010,pp.2677–2682.
[8] A. Gijsberts and G. Metta, “Incremental learning of robot dynamics
0.1 1 10 100 usingrandomfeatures.”inICRA. IEEE,2011,pp.951–956.
Seconds
[9] T.WuandJ.Movellan,“Semi-parametricGaussianprocessforrobot
systemidentification,”inIntelligentRobotsandSystems(IROS),2012
0.6 SP Recursive
)seu0.5 NPaPr aRmeceutrrisci vReec. [10] IJE.ESEun/RSdJeIlnateCrnruazti,oDna.lKCuolnicfe,rWen.ceOowne,nO,cEt.2C01al2i,sgpapn.,7a2n5d–7E3.1.Croft,
q
rot( ESMR00..34 [11] “SKOy.mnY-paLomisniauenmeD,yo“nnParRmacoitcbicoMatloCkdoiennletLrmoelaa,trivncoinal.ngd1f0od,rynMnoaa.mn1ii,cpu2cl0aa1ltio2br,raCptpioo.nn8tr6mo9le,–”t8hi7on4dI.sFAfoCr
eg0.2 force-controlledhumanoidrobots.”inHumanoids. IEEE,2011,pp.
arevA0.1 [12] 2S6.9T–r2av7e5r.saro,A.D.Prete,R.Muradore,L.Natale,andF.Nori,“In-
0 ertialparameteridentificationincludingfrictionandmotordynamics.”
0.1 1 10 100
Seconds inHumanoids. IEEE,2013,pp.68–73.
Fig. 4: Predicted forces (top) and torques (bottom) compo- [13] Y.Ogawa,G.Venture,andC.Ott,“Dynamicparametersidentification
ofahumanoidrobotusingjointtorquesensorsand/orcontactforces.”
nents average RMSE, averaged over a 30-samples window
inHumanoids. IEEE,2014,pp.457–462.
for the recursive semiparametric (blue), nonparametric (red) [14] J. Hollerbach, W. Khalil, and M. Gautier, “Model identification,” in
and parametric (green) estimators. The solid lines indicate SpringerHandbookofRobotics. Springer,2008,pp.321–344.
[15] S.VijayakumarandS.Schaal,“LocallyWeightedProjectionRegres-
the mean values over 10 repetitions, and the transparent
sion: Incremental Real Time Learning in High Dimensional Space.”
areas correspond to the standard deviations. On the x axis, inICML,P.Langley,Ed. MorganKaufmann,2000,pp.1079–1086.
time(inseconds)isreportedinlogarithmicscale,inorderto [16] B. Scho¨lkopf and A. J. Smola, Learning with Kernels: Support
VectorMachines,Regularization,Optimization,andBeyond(Adaptive
clearly show the behavior of the estimators during the initial
ComputationandMachineLearning). MITPress,2002.
transientphase.Ontheyaxis,theaverageRMSEisreported. [17] A. Rahimi and B. Recht, “Random Features for Large-Scale Kernel
Machines,”inNIPS. CurranAssociates,Inc.,2007,pp.1177–1184.
[18] D. Nguyen-Tuong, M. Seeger, and J. Peters, “Model Learning
with Local Gaussian Process Regression,” Advanced Robotics,
is key to enabling robotic systems to adapt to mutable vol. 23, no. 15, pp. 2015–2034, 2009. [Online]. Available:
conditions of the environment and of their own mechanical http://dx.doi.org/10.1163/016918609X12529286896877
[19] G.Metta,L.Natale,F.Nori,G.Sandini,D.Vernon,L.Fadiga,C.von
properties throughout extended periods. We validated our
Hofsten, K. Rosander, M. Lopes, J. Santos-Victor, A. Bernardino,
approach on the iCub humanoid robot, by analyzing the and L. Montesano, “The iCub Humanoid Robot: An Open-systems
performancesofasemiparametricinversedynamicsmodelof Platform for Research in Cognitive Development,” Neural Netw.,
vol.23,no.8-9,pp.1125–1134,Oct.2010.
itsrightarm,comparingthemwiththeonesobtainedbystate
[20] M. Gautier and W. Khalil, “On the identification of the inertial
of the art fully nonparametric and parametric approaches. parameters of robots,” in Decision and Control, 1988., Proceedings
ofthe27thIEEEConferenceon. IEEE,1988,pp.2264–2269.
ACKNOWLEDGMENT [21] M.Gautier,“Dynamicidentificationofrobotswithpowermodel,”in
Robotics and Automation, 1997. Proceedings., 1997 IEEE Interna-
This paper was supported by the FP7 EU projects tionalConferenceon,vol.3. IEEE,1997,pp.1922–1927.
CoDyCo (No. 600716 ICT-2011.2.1 - Cognitive Systems [22] K. Ayusawa, G. Venture, and Y. Nakamura, “Identifiability and
identificationofinertialparametersusingtheunderactuatedbase-link
and Robotics), Koroibot (No. 611909 ICT-2013.2.1 - Cogni-
dynamics for legged multibody systems,” The International Journal
tive Systems and Robotics), WYSIWYD (No. 612139 ICT- ofRoboticsResearch,vol.33,no.3,pp.446–468,2014.
2013.2.1 - Robotics, Cognitive Systems & Smart Spaces, [23] J. Baelemans, P. van Zutven, and H. Nijmeijer, “Model parameter
estimation of humanoid robots using static contact force measure-
Symbiotic Interaction), and Xperience (No. 270273 ICT-
ments,”in Safety,Security,and RescueRobotics(SSRR),2013IEEE
2009.2.1 - Cognitive Systems and Robotics). InternationalSymposiumon,Oct2013,pp.1–6.
[24] W. Khalil and E. Dombre, Modeling, identification and control of
REFERENCES robots. Butterworth-Heinemann,2004.
[25] A. Rahimi and B. Recht, “Uniform approximation of functions with
[1] R. Featherstone and D. E. Orin, “Dynamics.” in Springer Handbook randombases,”inCommunication,Control,andComputing,200846th
of Robotics, B. Siciliano and O. Khatib, Eds. Springer, 2008, pp. AnnualAllertonConferenceon,Sept2008,pp.555–561.
35–65. [26] W. Rudin, Fourier Analysis on Groups, ser. A Wiley-interscience
[2] A.E.HoerlandR.W.Kennard,“RidgeRegression:BiasedEstimation publication. Wiley,1990.
for Nonorthogonal Problems,” Technometrics, vol. 12, no. 1, pp. pp. [27] A˚. Bjo¨rck, Numerical Methods for Least Squares Problems. Siam
55–67,1970. Philadelphia,1996.
[3] C. Saunders, A. Gammerman, and V. Vovk, “Ridge Regression [28] A.H.Sayed,AdaptiveFilters. Wiley-IEEEPress,2008.
LearningAlgorithminDualVariables.”inICML,J.W.Shavlik,Ed. [29] A. Tacchetti, P. K. Mallapragada, M. Santoro, and L. Rosasco,
MorganKaufmann,1998,pp.515–521. “GURLS:aleastsquareslibraryforsupervisedlearning,”TheJournal
[4] N. Cristianini and J. Shawe-Taylor, An Introduction to Support ofMachineLearningResearch,vol.14,no.1,pp.3201–3205,2013.
Vector Machines and Other Kernel-based Learning Methods. [30] D. M. Beazley et al., “SWIG: An easy to use tool for integrating
Cambridge University Press, 2000. [Online]. Available: https: scripting languages with C and C++,” in Proceedings of the 4th
//books.google.it/books?id=B-Y88GdO1yYC USENIXTcl/Tkworkshop,1996,pp.129–139.
[31] S.Traversaro,A.DelPrete,S.Ivaldi,andF.Nori,“Inertialparameters the iCub,” in Humanoid Robots (Humanoids), 2011 11th IEEE-RAS
identificationandjointtorquesestimationwithproximalforce/torque InternationalConferenceon,Oct2011,pp.521–528.
sensing,” in 2015 IEEE International Conference on Robotics and [33] U. Pattacini, F. Nori, L. Natale, G. Metta, and G. Sandini, “An
Automation(ICRA2015). experimentalevaluationofanovelminimum-jerkcartesiancontroller
forhumanoidrobots,”inIntelligentRobotsandSystems(IROS),2010
[32] S. Ivaldi, M. Fumagalli, M. Randazzo, F. Nori, G. Metta, and IEEE/RSJInternationalConferenceon,Oct2010,pp.1668–1674.
G. Sandini, “Computing robot internal/external wrenches by means
of inertial, tactile and F/T sensors: Theory and implementation on
