Linear Matrix Inequalities for Rigid-Body Inertia Parameter Identification: A Statistical Perspective

2.4 MB·

by  Patrick M. Wensing

Linear Matrix Inequalities for Rigid-Body
Inertia Parameter Identification: A Statistical Perspective
Patrick M. Wensing, Sangbae Kim, Jean-Jacques E. Slotine
Abstract—With the increased application of model-based
whole-body control methods in legged robots, there has been
anresurgenceofinterestinsystemidentificationstrategiesand
adaptive control. An important class of methods relates to the
identification of inertia parameters for rigid-body systems. For
7
eachlink,theseconsistofthemass,firstmassmoment(related
1
to center of mass location), and 3D rotational inertia tensor.
0
The main contribution of this paper is to formulate physical-
2
consistency constraints on these parameters as Linear Matrix
c
b Inequalities(LMIs).Matrixinequalitiesarefoundnotinterms
e of moments of inertia, but in terms of statistical moments,
F suggesting a statistical perspective on the mass distribution m,I¯
C
of a rigid body. Through this viewpoint, a rich set of new
6
constraintscan beformulated asLMIs thatenforceproperties
1
on the underlying mass distribution of each link. Overall, new

constraints have a simpler form, are more efficient to enforce
]
O inoptimization,andaretighterthanthoseusedinthecurrent
literature. The methods are verified in system identification Fig.1. ExperimentalsetupforsystemidentificationonalegoftheMIT
R for a leg of the MIT Cheetah 3 robot. Detailed properties of Cheetah 3 robot. This work provides new methods to identify masses m,
. transmission components are identified alongside link inertias, centerofmasslocationsc,androtationalinertiasI¯C ofrigid-bodysystems
s whileensuringthequantitiesarephysicallyrealizablethoughamassdistri-
c with parameter optimization carried out to guaranteed global butionwithinaboundingellipsoid,depictedinred.Inertiaparametersare
[ optimality through semidefinite programming. identifiedalongsidetransmissioncomponentsthroughconvexoptimization.

2 I. INTRODUCTION
v Recent advances in whole-body control of humanoid and of Slotine and Li [17] to the case of adaptive control in
5 quadruped robots alike have led to a rebirth of model-based underactuated systems. Their collocated adaptive control
9
3 control approaches in recent years [1], [2], [3], [4], [5] approachguaranteestrackinglocallyontheactuateddegrees
4 with many successful demonstrations in large-scale robotic offreedomwhilelearninginertiaparameters.Otherworkhas
0 platforms[6],[7],[8],[9].Thesediversemethodscommonly focusedmoresoonparameteridentificationissuesrelatedto
1. employ optimization, either explicitly or implicitly through the floating base. Ayusawa et. al [11], [18] demonstrated
0 pseudoinversetechniques,overtheactuatortorquestoimpart that full-body inertia parameters can be estimated from
7 desiredcharacteristicsofmotiontotherobot.Recentstrides contact forces and system kinematics, without measuring
1 in torque-controlled actuation have provided wide benefit to jointtorques,duetopropertiesofthefloating-basedynamics.
:
v whole-body control techniques, while yet emerging actuator Otherworkhasconcentratedonenforcingthephysicalre-
Xi designs for legged machines [10] suggest that the perfor- alizability of inertia parameters during identification. Sousa
mance of these methodologies will continue to improve. and Cortesa˜o [12] show how positive definite conditions on
r
a Yet, the performance of whole-body control techniques is each body’s rotational inertia at the CoM can be formulated
heavily dependent on the accuracy of the dynamic models asanLMIoverunknowninertialparameters.Jovicet.al[15]
on which they are based. This has led to a resurgence of describe a number of practical constraints to improve the
activity towards methods for accurate system identification performance of [11], for instance, enforcing the CoM to
[11], [12], [13], [14], [15]. An important class of model reside within a bounding box from CAD. Recently, Traver-
identification problems pertains to the inertia parameters saro et. al [14] described additional conditions for physical
of rigid-body systems [16]. Identification is often pursued realizability of inertia parameters that have been neglected
offline, however, the mathematics of offline identification inpreviouswork.Theconditionstightlycharacterizethesets
have close connections to online adaptive control [17]. of inertia parameters that can be realized by some physical
Anumberofrecentstudiesinidentificationandadaptation rigidbody.Attheexpenseofthistightness,theirconstraints
have focused on issues relating to the float-base structure required a sequence of nonlinear programs to perform pa-
of legged systems. Pucci et. al [13] extended the methods rameter optimization over cleverly chosen manifolds.
The main contribution of this paper is to show how ex-
Patrick Wensing, Sangbae Kim, and Jean-Jacques Slotine are tendedphysical-realizabilityconditionsfortheinertiaparam-
with the Department of Mechanical Engineering, Massachusetts
eters can be formulated as LMIs. First, it is shown that con-
Institute of Technology, Cambridge, MA 02140, U.S.A.,
email:{pwensing,sangbae,jjs}@mit.edu ditions on the so-called triangle inequalities of inertia [14]
Handles Discrete Uses
with its Lie algebra se(3). The set of symmetric matrices
Tri.Ineqs. Convex Approx. LMIs
Sousaet.al[12] (cid:88) No (cid:88) in Rn×n is denoted Sn, with Sn+ the positive semidefinite
Traversaroet.al[14] (cid:88) No cone,andSn thepositivedefinitecone[21].Theshorthand
Ayusawaet.al[18] (cid:88) (cid:88) Yes A (cid:23) B is +us+ed to denote A−B ∈ Sn for some n ∈ N.
ThisPaper (cid:88) (cid:88) No (cid:88) +
A(cid:31)B similarly denotes A−B∈Sn .
++
TABLEI
COMPARISONOFFEATURESAMONGSTPARAMETERIDENTIFICATION Definition 1 (LMI Representable). A set S ⊂ Rn is called
OPTIMIZATIONPROBLEMSFORMULATEDINTHELITERATURE. linear matrix inequality (LMI) representable if there exists
m∈N and constant matrices {A }n ∈Sm such that
i i=0
canbeefficientlyformulatedasanLMI,whileimprovingon S ={x∈Rn : A +x A +···+x A (cid:23)0}
0 1 1 n n
thecomputationalperformanceof[12].TheLMIisfoundto
match the form of a 4×4 pseudo-inertia matrix appearing AsetiscalledstrictlyLMIrepresentablewhentheinequality
in early robotics literature [19]. The matrix, however, can can be tightened to hold strictly.
be interpreted as a matrix of statistical moments, suggesting
A set that is LMI representable is convex, however the
that constraints in inertia parameter identification may be
converse is not always true. Thus, LMI representability is
more directly viewed through a perspective of statistics
a stronger condition than convexity. A set S being LMI
than one of mechanics. With this perspective, results from
representable has favorable implications for numerical opti-
the statistical problem of moments [20] are translated to
mization,asconstraintsx∈S canbehandledusingefficient
enforcenewbounding-ellipsoidconstraintswithinparameter
semidefinite programming techniques [21].
identification. Throughout, a statistical interpretation of the
mass distribution yields richer constraints on the inertia pa- B. Rigid-Body Dynamics
rameterswhicharetighterthanthoseinearlierwork.Perhaps The treatment of rigid-body dynamics here relies heavily
surprisingly,theseconstraintsalsoofasimplerform,andare on 6D spatial notation [22], [23]. Spatial notation can
more efficient to enforce in numerical optimization. be one-to-one translated to a corresponding Lie-theoretic
The paper is laid out as follows. Section II provides notation [24]. A recent technical report nicely describes
preliminaries regarding rigid-body dynamics and linear ma- notational and conceptual correspondences [25].
trix inequalities. Section III reviews technical details from Thedynamicsofarigid-bodysystemcanbedescribedby
[12] and [14], with a high-level comparison in Table I.
Section IV presents motivation for the statistical perspective H(q)ν˙ +C(q,ν)ν+g(q)=τ (1)
on the mass distribution, culminating in the (re)introduction where H∈Rnd×nd the mass matrix, nd ∈N+ the number
ofthepseudo-inertiamatrix.SectionVdrawsonconnections
of degrees of freedom, q∈Q the configuration with Q the
with the statistical problem of moments to introduce new configuration manifold, ν ∈ Rnd the generalized velocity,
characterizations of density realizability with bounding vol- Cν ∈ Rnd and g ∈ Rnd the Coriolis and gravity forces,
umeconstraints.Anexperimentalvalidationofthisapproach and τ ∈Rnd the generalized force. For legged systems, the
is provided in Section VI for identification of a leg from generalized force τ has contributions from n ∈ N joint
j +
the MIT Cheetah 3, shown in Fig. 1. Section VII provides actuator torques τj ∈ Rnj and nc ∈ N external contact
concluding remarks. wrenches {f }nc ⊂R6 according to
ck k=1
II. PRELIMINARIES (cid:88)nc
A. Notation and Definitions τ =S(cid:62)j τj + J(cid:62)ckfck
k=1
Throughout the text, the set of real and natural numbers
are denoted by R and N respectively. R+ represents the where Sj ∈Rnj×nd is an actuated joint selector matrix and
set of non-negative reals, while Rn represents the set of Jck ∈R6×nd the 6D Jacobian for contact k.
n × 1 column vectors with real-valued entries. Similarly The equations (1) can be derived using a Kanesian
Rm×n represents the set of all m × n matrices. Unless approach [26] which is effectively employed in standard
otherwise specified, scalars and scalar-valued quantities are Recursive Newton-Euler algorithms [23]. Suppose a system
denoted with italics (a,b,c,...) while vectors and vector- ofnb ∈N+ bodies,withcoordinatesystemsattachedrigidly
valued functions are denoted with upright bold characters toeachbody.Thespatial(6D)velocityvi ∈R6ofeachbody
(a,b,c,...).Matrixquantitiesaredenotedwithuprightbold can be described by
capitals (A,B,...). 1 indicates the n×n identity. (cid:20) (cid:21)
n ω
The Special Orthogonal group of rotation matrices is vi(q,ν)= vi =Ji(q)ν (2)
denoted as SO(3) = {R ∈ R3×3 : R(cid:62)R = 1 }, i
3
with its Lie algebra, the set of 3 × 3 skew-symmetric where Ji ∈ R6×nd the Jacobian for body i, ωi ∈ R3 the
matrices,denotedso(3).TheSpecialEuclideangroupSE(3) angularvelocityinbodycoordinates,andvi ∈R3 thelinear
of homogenous transformations is given by velocityofthecoordinateorigin(giveninbodycoordinates).
(cid:26)(cid:20) (cid:21) (cid:27) The spatial acceleration ai ∈R6 of each body is then
R p
SE(3)= 0 1 : R∈SO(3), p∈R3 a (q,ν,ν˙)=v˙ =J ν˙ +J˙ ν (3)
i i i i
The Newton-Euler equations for a rigid body follow [23]: This optimization problem is efficiently solvable to global
optimality. However, without including constraints, the 6D
f =I a +(v ×∗)I v (4)
i i i i i i inertias I(π ) may not correspond to any physical system.
i
wheref ∈R6thenetwrenchonbodyi,and(v ×∗)∈R6×6 Inertia parameters in any physical body are determined
i i
thespatialcrossproductmatrix[22]forvi.Thebodyspatial by its distribution of density ρi(·) : R3 → R+. The inertia
inertia Ii ∈R6×6 is a constant quantity and is given by components for each body i are a functional of ρi(·) [14]
(cid:90)(cid:90)(cid:90)
I =(cid:20) I¯i miS(ci)(cid:21) mi = ρi(x)dx (9)
i miS(ci)(cid:62) mi13 (cid:90)(cid:90)(cid:90)R3
with S(x) ∈ so(3) the skew-symmetric matrix such that mici = xρi(x)dx (10)
S(x)y = x×y for all x,y ∈ R3, ci ∈ R3 the CoM of (cid:90)(cid:90)(cid:90)R3
body i in local coordinates, m ∈ R the body mass, and I¯ = S(x)S(x)(cid:62)ρ (x)dx (11)
i + i i
I¯ ∈ S3 a standard 3D rotational inertia tensor about the R3
i ++
coordinate origin. The body dynamics can account for the Definition 2 (Density Realizable). Given a set X ⊆ R3,
effects of gravity by noting that f = fe + I ia where an inertia tensor I is called X-density realizable if ∃ρ(·) :
i i i g
iag ∈R6 is the gravitational acceleration in frame i and fie R3 →R+ such that ρ(x)=0 if x∈/ X and the components
representsthenetexternalwrenchonbodyi(fromactuators of I, (m,mc,I¯), satisfy (9)-(11) respectively. When X is
orotherexternalforces).Finally,(1)isformedbyprojecting not specified, X =R3 is assumed.
all external wrenches back to generalized forces [26]
Remark1. GivenanyX ⊆R3,thesetP∗ definedbyP∗ =
X X
(cid:88)nb {π ∈ R10 : m(π) > 0, I(π) is X−density realizable}
J(cid:62)i fie =H(q)ν˙ +C(q,ν)ν+g(q) is a convex cone. This follows from the fact that (9)-(11)
i=1 are linear in ρ(·). Previous work [18] provided a discrete
=(cid:88)nb J(cid:62)(I a −I ia +(v ×∗)I v ) (5) approximation to this cone. Without discretizing, the work
i i i i g i i i
here provides cases wherein this cone is LMI representable.
i=1
C. Inertia Parameter Effects on the Equations of Motion III. PREVIOUSRESULTS
The equations of motion (1) can be described linearly for This section focuses on physical consistency for a single
a specific parameterization of each I [16]. Letting rigidbody.Assuch,bodyindiceswillbedropped.Attempts
i
(cid:20) (cid:21) to enforce physical consistency focus on the rotational
I¯i = IIxxxy IIxyyy IIxyzz inertia I¯C about the CoM. The parallel axis theorem in
Ixz Iyz Izz 3D establishes a correspondence between I¯ and I¯, the
C
and h = [h ,h ,h ](cid:62) = m c , it follows that both the rotational inertia about the local coordinate origin, through
i x y z i i
spatial inertia I and the net wrench f in (4) can be
i i I¯=I¯ +mS(c)S(c)(cid:62) (12)
expressed linearly in the body inertia parameters π C
i
A. Physical Semi-consistency: An LMI Parameterization
π =[m,h ,h ,h ,I ,I ,I ,I ,I ,I ](cid:62) ∈R10
i x y z xx xy xz yy yz zz PositivedefiniteconstraintsonI¯ (π)havebeenenforced
C
Let πik ∈R denote the k-th parameter for body i, with Ik commonlyfortheinertiaparametersπ [12],[27].Inarigid-
thefixedbasissuchthatI(πi)=(cid:80)kIkπik.Parametersfor body system, when each mi > 0 and I¯Ci (cid:31) 0, it can be
all bodies are collected as shown that H(q)(cid:31)0 ∀q∈Q [27]. In the case of adaptive
control, satisfaction of this constraint on H(q) throughout
π =[π(cid:62),...,π(cid:62)](cid:62)
i nb adaptation is sufficient to admit tracking guarantees [27].
Expanding the role of each πik in (5) However, as detailed in [14], I¯C(π) (cid:31) 0 is not sufficient
for density realizability of I(π).
(cid:88)
τ = J(cid:62)(I a −I ia +(v ×∗)I v )π (6)
i k i k g i k i ik
Definition3(PhysicalSemi-consistency). Avectorofinertia
(i,k)
parameters π ∈ R10 is called physically semi-consistent if
Combining (6) with (2) and (3) it follows that a regressor m(π) > 0 and I¯ (π) (cid:31) 0. The set of physically semi-
matrix Y ∈Rnj×10nb [17] can be constructed such that consistent parameteCrs is denoted P ⊂R10.
τ =Y(q,ν,ν˙)π (7) Theorem 1 (LMI Representation of P). [12] The set of
physically semi-consistent parameters P is strictly LMI
The regressor matrix provides a simple method to pursue
representable. Its LMI representation is given as
inertia parameter identification. Given n ∈ N samples
s +
{q(m), ν(m), ν˙(m), τ(m)}nms=1 a simple least squares pa- P =(cid:8)π ∈R10 : I(π)(cid:31)0(cid:9)
rameter identification problem can be formulated as [12]
Extending the optimization of (8) to include physical
(cid:88)
min (cid:107)Y(m)π−τ(m)(cid:107)2 (8) semi-consistency constraints results in a semidefinite pro-
π
m gramming (SDP) problem, which can be solved to global
optimality with SDP solvers [21] A. A matrix inequality for triangle inequalities on I¯
C
min (cid:88)(cid:107)Y(m)π−τ(m)(cid:107)2 Suppose R and J as before such that I¯C =RJR(cid:62). The
π triangle inequalities on I¯ (13) can be rewritten as
m C
s.t. I(π )(cid:31)0 ∀i∈{1,...,n }
i b J +J +J ≥2J (14)
1 2 3 i
B. (Full)PhysicalConsistency:NonlinearParameterization
for each i=1,...,3. Noting that J ,...,J are the eigenval-
1 3
Definition 4 (Physical Consistency). A vector of inertia ues of I¯ , (14) is equivalent to
C
parameters π ∈R10 is called physically consistent if
Tr(I¯ )≥2λ (I¯ ) (15)
C max C
m(π)>0 and I(π) is density realizable.
where λ (·) provides the maximum eigenvalue of its ar-
The set of physically consistent parameters is denoted P∗. max
gument, and Tr(·) is the trace operator. From the eigenvalue
In comparison to P, the set of physically consistent inequality λ (I¯ ) x(cid:62)x≥x(cid:62)I¯ x, it follows that
max C C
inertia parameters P∗ ⊂ P has been shown to result
λ (I¯ )1 (cid:23)I¯ (16)
from only three additional conditions on I¯ [14]. These max C 3 C
C
additional constraints arise from considerations regarding Thus, through the use of (16), (15) is LMI equivalent to
the principal moments of inertia. Suppose R ∈ SO(3) and
J=diag(J1,J2,J3),J1,...,J3 >0suchthatI¯C =RJR(cid:62). 21Tr(I¯C)13−I¯C (cid:23)0 (17)
Then, the cartesian inertia tensor is density realizable iff B. The Density-Weighted Covariance of a Rigid Body
J +J ≥J , J +J ≥J , and J +J ≥J (13) The mathematical condition in (17) can be interpreted
1 2 3 2 3 1 1 3 2
more cleanly as requiring a positive semidefinite covariance
Definition 5 (Triangle Inequalities). A matrix in S3 is said
of the rigid body in a density-weighted sense. Towards this
to satisfy the triangle inequalities if its eigenvalues {J }3
i i=1 insight, I¯ can be expanded algebraically to verify
satisfy (13). C
(cid:90)(cid:90)(cid:90)
Theorem 2 (Nonlinear Parameterization of P∗). [14] An I¯ = S(x )S(x )(cid:62)ρ(x)dx
C c c
inertia tensor I is physically consistent if and only if there R3
(cid:90)(cid:90)(cid:90)
exists m > 0, R ∈ SO(3), J = diag(J1,J2,J3) (cid:31) 0 which = (cid:0)Tr(xcx(cid:62)c )13−xcx(cid:62)c (cid:1)ρ(x)dx
satisfies the triangle inequalities, and c∈R3 such that R3
(cid:20)RJR(cid:62)+mS(c)S(c)(cid:62) mS(c)(cid:21) where xc =x−c. To simplify this expression, the density-
I = mS(c)(cid:62) m1 weighted covariance of a rigid body is introduced as
3
(cid:90)(cid:90)(cid:90)
Even for a single rigid body, optimization with this Σ = x x(cid:62)ρ(x)dx (18)
parameterization of P∗ results is a nonlinear optimization C R3 c c
problem over a manifold From this definition, it follows that
(cid:88)
min (cid:107)Y(m)π(R,J,c,m)−τ(m)(cid:107)2 I¯ =Tr(Σ )1 −Σ , and (19)
R,J,c,m C C 3 C
m Σ = 1Tr(I¯ )1 −I¯ (20)
s.t. R∈SO(3) C 2 C 3 C
m>0, J >0,i=1,2,3 using Tr(I¯C) = 2Tr(ΣC). From (19), it can be seen that
i Σ and I¯ share a set of eigenvectors. It can further be
J +J ≥J , J +J ≥J , and J +J ≥J C C
1 2 3 2 3 1 1 3 2 verified that if µ ,µ ,µ the eigenvalues of Σ , then
1 2 3 C
Solution of this problem is possible using novel methods
J =µ +µ , J =µ +µ , J =µ +µ (21)
in[14].However,theoptimizationapproachrequirescustom 1 2 3 2 1 3 3 1 2
solvers and does not guarantee global optimality. are the eigenvalues of I¯ . Using these relationships, and in
C
light of (17), the following can be verified.
IV. CONTRIBUTION:ANLMIFORPHYSICALLY
CONSISTENTINERTIAPARAMETERS Proposition1(CovarianceInterpretationofTriangleIneqs.).
This section takes a closer look at the conditions which SupposeI¯,Σ∈S3,Σ= 1Tr(I¯)1 −I¯.ThenΣ(cid:23)0ifand
2 3
are required for physical consistency. It is shown that, in only if I¯ (cid:23) 0 and I¯ satisfies the triangle inequalities. An
fact,thetriangleinequalitiescanbeexpresseddirectlyasan analogous statement holds with all inequalities tightened to
LMIonthetraditionalparametersπ,withoutresortingtoan hold strictly.
explicit parametrization of I¯ . Section IV-A first describes
C Asimilarpropositioncanbefoundwithin[28],however,the
a matrix inequality that captures the triangle inequalities
physical connection to the density-weighted covariance pro-
on I¯ . A physical interpretation on this matrix inequality
C vided here is new. The following corollary is an immediate
is given in Section IV-B through the introduction of the
result of combining Proposition 1 with (17).
density-weightedcovarianceofarigidbodyanditsstandard-
deviationellipsoid.Followingthisinterpretation,SectionIV- Corollary 1 (Parameterization of P∗ with an LMI on Σ ).
C
C details an LMI directly over π for physical consistency. π ∈P∗ if and only if m(π)>0 and Σ (π)(cid:23)0.
C
of the parallel axis theorem for Σ , however, will admit an
C
LMI characterization similar to that provided in Theorem 1.
zˆ yˆ The matrix of second moments Σ∈R3×3 is defined as
(cid:90)(cid:90)(cid:90)
xˆ Σ= xx(cid:62)ρ(x)dx
⌃C=diag(2,3,1) ⌃C=diag(2,3,0) ⌃C=diag(2,0,0) R3
IC=diag(4,3,5) IC=diag(3,2,5) IC=diag(0,2,2)
Throughexpansionof(18)andusing(9)and(10),ananalog
Fig.2. GraphicalrepresentationofΣC.Pointmassdistributionexamples to the parallel axis theorem can be verified as:
when(a)ΣC (cid:31)0(alltriangleinequalitiesholdstrictly)(b)ΣC (cid:23)0with
onezeroeigenvalue,andthusonetriangleinequalityistight.(c)ΣC (cid:23)0 Σ=Σ +mcc(cid:62) (22)
withtwozeroeigenvalues,andthustwotriangleinequalitiesaretight.The C
banludereelplirpessoenidtsisondeefisntaenddaarsdthdeevsitaatniodnaridn-deeavcihatidoinrecetliloipnsooifditEsπexintentht.etext, As a key benefit, using the relationship Σ= 21Tr(I¯)13−I¯
from (20), Σ is verified linear in the inertia parameters π.
To help visualize ΣC, an ellipsoid Eπ is defined as Definition 6 (Pseudo-Inertia Matrix). The pseudo-inertia
E ={x∈R3 : ∃y, Σ y=(x−c), (x−c)(cid:62)y=1} matrix J for parameters π is defined by
π C
(cid:20) (cid:21)
Σ(π) h(π)
which, when ΣC (cid:31)0, takes the form J(π)=
h(π)(cid:62) m(π)
E ={x∈R3 : (x−c)(cid:62)Σ−1(x−c)=1}
π C
Theorem 3 (LMI Representation of P∗). The set of phys-
This set is named the standard-deviation ellipsoid. In the
ically consistent parameters P∗ for a single rigid body is
caseofa1Dprobabilitydistribution,someoftheprobability
strictly LMI representable. Its LMI representation is
density must exist at or beyond the standard deviation. This
intuitiongeneralizesto3D.Atleastsomeofthemassofthe P∗ =(cid:8)π ∈R10 : J(π)(cid:31)0(cid:9)
body must exist on or beyond the ellipsoidal shell E .
π
The standard-deviation ellipsoid E is shown in Figure 2 Proof. BytheSchurcomplementlemma[21,SectionA.5.5],
π
for different density distributions. When Σ has a zero J(π) (cid:31) 0 if and only if m(π) > 0 and Σ− 1hh(cid:62) (cid:31) 0.
C m
eigenvalue, the density distribution is degenerate. An in- Byapplicationoftheparallelaxistheoremforsecondmass-
finitelythinplate,forinstance,hasnovariancenormaltothe momentmatrices(22),andusingh=mc,thisisequivalent
plate,andthuswouldhaveasinglezeroeigenvalueinΣ ,as toΣ (cid:31)0.FromProposition1,thisisequivalenttoI¯ (cid:31)0
C C C
inFig.2(b).Correspondingly,oneofthetriangleinequalities and I¯ satisfies the triangle inequalities. Finally, from the
C
holds tightly in such a case. Since any physical body must main result of [14], this is equivalent to π ∈P∗.
have non-infinitesimal spatial extent in all directions, it
sufficestoconsiderphysicalrealizabilityunderthecondition Remark 4. Again, taking a statistical view on the mass
that the triangle inequalities hold with strict inequality. distribution of a rigid body, the results of Thm. 3 can be
seen to flow from a condition on the statistical first and
Remark 2. Corollary 1 can be interpreted cleanly within a second moments [34, Thm. 16.1.2]. Suppose µ ∈ R3 and
probabilistic context. Suppose X ∈ R3 a random variable E ∈ S3, then there exists a random variable X ∈ R3 such
with p(·) = ρ(·)/m its mass-normalized probability den- that µ+=E[X] and E=E[XX(cid:62)] if and only if
sity function. Define E[·] the expectation operation. Corol-
(cid:20) (cid:21)
lary 1 effectively states that the covariance of X, given E µ
(cid:23)0
by E[(X−E[X])(X−E[X])(cid:62)], is non-degenerate density µ(cid:62) 1
realizable if and only if the covariance is positive definite.
Thus,fromthestandpointofstatistics,thisnewconditionon Incomparison,theconstraintJ(π)(cid:31)0inThm.3multiplies
densityrealizabilitymayberatherunsurprisinginhindsight. each of these components by the overall mass m(π) and
enforces non-degeneracy of the distribution.
Remark 3. Corollary 1 could have applicability to inertia
identificationandadaptivemethodswithinattitudecontrolof The 4 × 4 matrix of moments J(π) was found more
aerial vehicles (e.g. [29], [30], [31]). In these applications, commonly in early robot dynamics literature (e.g. [19],
the CoM location is often assumed known, and focus is [35]) and in early work on adaptive control (e.g. [36]).
placed on estimating the CoM inertia I¯ . The triangle It has been employed more recently within the context of
C
inequalitiesareoftenoverlooked,butaretreatedine.g.[32], 4×4 matrix forms of the equations of motion for a rigid
[33].Corollary1couldbeconsideredtoaddressthetriangle body [37], [38]. Using these 4×4 equations, a parameter
inequalities within this thread of research. identificationapproachforasinglerigidbodywasproposed
in [38]. The same matrix J(π) was used to provide a
C. An LMI Representation of Physical Consistency
left-invariant Riemannian metric over SE(3) in [28] and
While (17) and its covariance interpretation provide a appearsinroboticsbooks(e.g.[39]).Despiteitsimportance,
matrix inequality for the physical consistency of I¯ , this the pseudo-inertia matrix is notably lacking from current
C
conditionisnotlinearintheinertiaparametersπ.Ananalog mainstream literature on robot dynamics.
DensityRepresentible Not DensityRepresentible
It is interesting to note how the pseudo inertia J(π) S� S�
compares to the standard spatial inertial I(π) in terms of
the kinetic energy metric each provides. Suppose S
⇢
(cid:20) (cid:21) (cid:20) (cid:21) ⇡
ω S(ω) v E
v= ∈R6 , V= ∈se(3)
v 0 0
tr(J(⇡)Q)=0.52 tr(J(⇡)Q)= 0.04
�
It can be verified that the kinetic energy satisfies [39]
S
1v(cid:62)I(π)v= 1Tr(cid:0)VJ(π)V(cid:62)(cid:1) ⇢
2 2 ⇡6
E
Thus, while physical semi-consistency ensures that the ,S zˆ
2
associated kinetic energy metric is positive definite, the ) yˆ
⇡
additional constraints from the triangle inequalities enforce c( xˆ tr(J(⇡)Q)=0.07 tr(J(⇡)Q)= 0.28
�
additional structure on the metric. As has been shown for
thefirsttimehere,thetriangleinequalitiesarepreciselywhat Fig.3. Casesillustratingtheroleofthecenterandshapeofthestandard-
deviation ellipsoid Eπ on S-densit√y re√alizability of π. In each case, the
represent the difference between I(π)(cid:31)0 and J(π)(cid:31)0. ellipsoidS ha√ssemi√axesof√length 5, 2,1inthexˆ,yˆ,zˆdirections.Eπ
The pseudo-inertia matrix is also of a lower dimension hassemiaxes 0.9, 0.2, 0.2withprincipleaxescoloredaccordinglyin
thefigure.WhenadensitydistributiononS exists,adistributionbyfour
(4×4) than the spatial inertia matrix (6×6). This may be
pointmassesisshown,asguaranteedtoexistthroughThm.4.
surprising in light of the fact that the 3 additional triangle
inequalities are embedded within the LMI for J(π). As a
Theorem4(DensityRealizabilityonanEllipsoid). Suppose
result,however,theLMIJ(π)(cid:31)0ismorecomputationally a bounding ellipsoid S as in (23). Let Q∈R4×4 such that
efficient to enforce than I(π)(cid:31)0.
(cid:40) (cid:20) (cid:21)(cid:62) (cid:20) (cid:21) (cid:41)
x x
S = x∈R3 : Q ≥0
V. CONTRIBUTION:LMICONSTRAINTSFORINERTIA 1 1
PARAMETERREALIZABILITYONELLIPSOIDS
Then, π is S-density realizable if and only if
With the statistical perspective of the mass distribution,
answering further questions of physical realizability for in- J(π)(cid:31)0 and Tr(J(π)Q)≥0. (25)
ertiaparametersfallswithinthebodyofworkontheproblem
Further, any π is S-density realizable iff it can be repre-
of moments within statistics (e.g. [20], [40]). With this
sented by four point masses m at x ∈S such that:
insight,resultsaretranslatedheretoprovidenewconditions k k
on inertia realizability with bounding volume constraints. 4 (cid:20) (cid:21)(cid:20) (cid:21)(cid:62)
(cid:88) x x
Recent work has shown the benefits of including prior J(π)= m k k
k 1 1
knowledgeontheshapeofeachrigidbodywithinparameter k=1
identification. Work by Jovic et. al [15] enforced the CoM
to reside within a bounding box estimated from CAD. New
Notethatbytakingintoaccountthesecondmoments,the
constraints are provided here that address second moments. condition Tr(J(π)Q)≥0 in (25) is a 1D linear inequality
To begin, suppose a rigid body is known to reside within constraint,whereastheloosercondition(24)isa4×4LMI.
an ellipsoid S ⊂R3 described by: Thus, again, it is found that the statistical perspective of
the mass distribution results in tighter conditions that are
S ={x∈R3 | (x−x )(cid:62)Q−1(x−x )≤1} (23)
s s s computationally more efficient to enforce. Figure 3 shows
a number of cases, illustrating both the role of the CoM
Before considering constraints on the second moments, it is
location and the shape of the standard-deviation ellipsoid
notedthattheCoMconstraint,c(π)∈S,canbeformulated
E on S-density realizability. The following corollary is
as an LMI over π. When m(π)>0, c(π)∈S iff π
provided to accommodate more complex bounding shapes.
(cid:20) m(π) h(π)(cid:62)−m(π)x(cid:62)(cid:21)
C(π)= s (cid:23)0 Corollary2(DensityRealizabilityontheUnionofEllipses).
h(π)−m(π)x m(π)Q
s s Suppose a rigid body is known to reside within the union
(24)
of n ellipses S = ∪n(cid:96) S . Then, its inertia parameters π
Again, equivalence is due to the Schur complement lemma. (cid:96) (cid:96)=1 (cid:96)
areS-densityrealizableifandonlyifthereexistparameters
Yet, as the CoM approaches the edge of S, large second {π }n(cid:96) such that π = (cid:80) π and each π is S -density
moments would imply the existence of mass outside the (cid:96) (cid:96)=1 (cid:96) (cid:96) (cid:96) (cid:96)
realizable as verified by Thm. 4.
ellipsoid. In fact, as the CoM of a rigid body approaches
the edge of a bounding ellipsoid, the rigid body necessarily Remark 5. Beyond identification, LMIs for the sets P∗ ⊂
S
degeneratestoapointmass.Thus,constraintsthatc(π)∈S P∗ ⊂ P also have applicability for projected gradient
R3
alone are not sufficient for π to be S-density realizable. methods in adaptive control [42]. In this context, tightening
Drawing on the perspective of statistics, Theorem 4.7(b) constraints on unknown parameters only increases the rate
from [41] can be translated as follows. of parameter convergence [43].
)dar( elgnA dA/bA--000...422012 14 16 18 20 22 )dar( elgnA piH0.50112 14 16 18 20 22 )dar( elgnA eenK--10-..55112 14 16 18 20 22
Time (s) Time (s) Time (s)
10 10
)mN( euqroT dA/bA-505 )mN( euqroT piH-11000 )mN( euqroT eenK-1-0505
12 14 16 18 20 22 12 14 16 18 20 22 12 14 16 18 20 22
Time (s) Time (s) Time (s)
Fig. 4. Regression errors after identification of a leg for the MIT Cheetah 3. The data shown is distinct from that used in training. Motor torques are
predictedwithanoverallRMSerrorof1.46Nm.Forcomparison,theCoulombfrictionwasidentifiedasBc=diag(3.12,1.25,0.95)Nm.
MITTheCphreoeptaohVse3Id.rcoEobnXosPtt.EraRTinIhMtesErwNoebTroAetL,ussVheAdoLwtIonDiAdinTenIFOtiiNgfy. a1,lehgasoffothuer noitadilaV SM)mN( rorrE110012 NPDhoeyn Pssihictyyasl R iSceaealm lCiiz-oaCnbosiliniststyies notecnny ac Cny oEnlsltirpasionitds
3-DoF legs where each DoF is driven by a proprioceptive R 100
100 101 102
actuator [10]. Each actuator includes a large air-gap radius Samples
brushless DC motor, with a high-inertia rotor coupled to the Fig.5. Validationerrorafteridentificationversussamplesize.
joint output by a 11.62 : 1 planetary gearbox. To address
10,000datasamplesusingCVX[44]andtheMOSEKsolver
the unique characteristics of these actuators, the leg was
[45]. The problem took 1.67s to solve to global optimality
treated as a system of n = 6 bodies (3 rigid links, and
b on a 2011 Intel Core i5 MacBook Pro. Regularization of
3 rotors). Joint rates at the output of the gearbox were used
w =10−6 was used.
as the generalized velocity ν. To account for transmission π
After identification, the RMS errors from (26) with the
losses,(7)wasmodifiedviadiagonalmatricesofviscousand
Coulomb friction coefficients B∈R3×3 and B ∈R3×3 next 10,0000 samples in the dataset were 1.48, 1.69, and
c 1.16 Nm on the ab/ad, hip, and knee respectively. For
τ −Bν−B sign(ν)=Y(q,ν,ν˙)π (26) comparison, the Coulomb friction was identified as B =
c c
diag(3.12,1.25,0.95) Nm. Figure 4 shows identification
Outer approximation bounding-ellipsoid parameters in C
i results for this validation data. Figure 5 shows how this
and Q were set using geometric properties from CAD.
i performancecomparesfordifferenttrainingsamplesizesn
With the result of Theorem 4, rich physical-consistency s
and with different constraints. As in Remark 5, tighter con-
constraints on the inertia parameters can be treated while
straintsempiricallyresultinanaccuratemodelmorequickly
also identifying transmission characteristics.
as additional data is used. This benefit is in addition to the
min 1 (cid:88)(cid:107)Y(m)π+Bν(m)+B sign(ν(m))−τ(m)(cid:107)2 fact that tightened constraints result in optimal parameters
π,Bc,B ns m c that are physically realistic.
+w (cid:107)π−πˆ(cid:107)2 (27)
π VII. CONCLUSIONS
s.t. Ci(πi)(cid:23)0 (CoM within an Ellipse) This paper has advocated for a statistical perspective of
J(π )(cid:31)0 (Density Realizability) the mass distribution for a rigid body when approaching
i
constraints within inertia parameter identification. Rather
Tr(J(π )Q )≥0 (Density on an Ellipse)
i i
than focus on moments of inertia, questions of density
∀i∈{1,...,n }
b realizabilityaremoredirectlyaddressedthroughthedensity-
A small regularizing term w (cid:107)π−πˆ(cid:107)2 is added where πˆ ∈ weighted covariance Σ of each body. Through a parallel-
π C
R10nb is a set of estimated inertia parameters from CAD or axis-liketheoremappliedtothisquantity,resultshaveshown
other sources. Such regularization is common practice [12]. the importance of the pseudo-inertia matrix J(π). This ma-
Datawasgatheredfromalegswingingexperimentshown trixofmomentsislinearintheinertiaparameters,andcanbe
in the supplementary video. To generate the motion, the used to efficiently enforce density realizability constraints.
leg was placed in a Cartesian impedance control mode, Through the form of J(π), it was argued that constraints
and the foot endpoint was commanded to move on a on the inertia parameters might instead be viewed as a
virtual ellipsoidal shell. The target point on the shell was specialcaseoftheproblemofmomentsinstatistics.Through
parameterized by spherical angles (φ,θ), governed by rates this connection, constraints on the density realizability over
φ˙ =A sin(ω t) and θ˙ =A sin(ω t) with A =12 rad/s, a bounding ellipsoid, or union of bounding ellipsoids, can
φ φ θ θ φ
A = 3.4 rad/s, ω = 1.63 rad/s, and ω = 0.265 rad/s. be formulated through LMI constraints over the unknown
θ φ θ
Data was sampled at 1 kHz, with joint actuator torques inertia parameters. This result provides a rich set of con-
τ estimated from the torques commanded to brushless straints that can be used to solve physically consistent in-
j
current controllers [10]. The problem (27) was solved with ertia parameter identification problems to guaranteed global
optimality. Future work will apply the identified models for [19] A. K. Bejczy, “Robot arm dynamics and control,” JPL Technical
high-performance control of the MIT Cheetah 3. Memorandum33-669,Tech.Rep.,1974.
[20] J. B. Lasserre, “A semidefinite programming approach to the gener-
ACKNOWLEDGMENT alized problem of moments,” Mathematical Programming, vol. 112,
no.1,pp.65–92,2008.
The authors would like to acknowledge Ben Katz and
[21] S. Boyd and L. Vandenberghe, Convex Optimization. Cambridge,
Jared DiCarlo for assistance with the experiments. Funding U.K.:CambridgeUniv.Press,2004.
for this work was supported in part through NSF award IIS- [22] R.Featherstone,RigidBodyDynamicsAlgorithms. NewYork,NY:
Springer,2008.
1350879 and by the Agency for Defense Development of
[23] R. Featherstone and D. Orin, “Chapter 2: Dynamics,” in Springer
Korea under contract UD140073ID. HandbookofRobotics,B.SicilianoandO.Khatib,Eds. NewYork:
Springer,2008.
REFERENCES [24] F. Park, J. Bobrow, and S. Ploen, “A lie group formulation of robot
dynamics,”TheInternationalJournalofRoboticsResearch,vol.14,
[1] Y. Abe, M. da Silva, and J. Popovic´, “Multiobjective control with no.6,pp.609–618,1995.
frictionalcontacts,”in2007ACMSIGGRAPH/EurographicsSymp.on [25] S. Traversaro and A. Saccon, “Multibody dynamics notation,”
ComputerAnimation,Aire-la-Ville,Switzerland,2007,pp.249–258. Technische Universiteit Eindhoven, Tech. Rep., 2016. [Online].
[2] J. Park, J. Haan, and F. Park, “Convex optimization algorithms for Available:http://repository.tue.nl/849895
activebalancingofhumanoidrobots,”IEEETransactionsonRobotics, [26] T. R. Kane and D. A. Levinson, “The use of Kane’s dynamical
vol.23,no.4,pp.817–822,Aug.2007. equations in robotics,” Int. J. of Robotics Research, vol. 2, no. 3,
[3] J. Park and O. Khatib, “Contact consistent control framework for pp.3–21,1983.
humanoidrobots,”inProc.oftheIEEEInt.ConferenceonRobotics [27] W. Li and J.-J. E. Slotine, “An indirect adaptive robot controller,”
andAutomation,May2006,pp.1963–1969. Systems&ControlLetters,vol.12,no.3,pp.259–266,1989.
[4] L.Sentis,J.Park,andO.Khatib,“Compliantcontrolofmulticontact [28] C.BeltaandV.Kumar,“AnSVD-basedprojectionmethodforinter-
and center-of-mass behaviors in humanoid robots,” IEEE Trans. on polationonSE(3),”IEEETransactionsonRoboticsandAutomation,
Robotics,vol.26,no.3,pp.483–501,June2010. vol.18,no.3,pp.334–345,Jun2002.
[5] P. M. Wensing and D. E. Orin, “Generation of dynamic humanoid [29] J.Ahmed,V.T.Coppola,andD.S.Bernstein,“Adaptiveasymptotic
behaviors through task-space control with conic optimization,” in tracking of spacecraft attitude motion with inertia matrix identifica-
IEEE Int. Conf. on Rob. and Automation, Karlsruhe, Germany, May tion,”J.Guid.ControlandDyn.,vol.21,no.5,pp.684–691,1998.
2013,pp.3103–3109. [30] N. A. Chaturvedi, D. S. Bernstein, J. Ahmed, F. Bacconi, and
[6] M.Hutter,H.Sommer,C.Gehring,M.Hoepflinger,M.Bloesch,and N.H.McClamroch,“Globallyconvergentadaptivetrackingofangular
R.Siegwart,“Quadrupedallocomotionusinghierarchicaloperational velocityandinertiaidentificationfora3-dofrigidbody,”IEEETrans.
spacecontrol,”Int.J.ofRoboticsResearch,vol.33,no.8,pp.1047– onControlSys.Technology,vol.14,no.5,pp.841–853,Sept2006.
1062,2014. [31] D.Thakur,S.Srikant,andM.R.Akella,“Adaptiveattitude-tracking
[7] J.Koenemann,A.D.Prete,Y.Tassa,E.Todorov,O.Stasse,M.Ben- controlofspacecraftwithuncertaintime-varyinginertiaparameters,”
newitz, and N. Mansard, “Whole-body model-predictive control ap- J.Guidance,Control,andDynamics,vol.38,no.1,pp.41–52,2014.
pliedtothehrp-2humanoid,”inIEEE/RSJInternationalConference [32] M. A. Peck, “Uncertainty models for physically realizable inertia
onIntelligentRobotsandSystems,Sept2015,pp.3346–3351. dyadics,”J.AstronauticalSciences,vol.54,no.1,pp.1–16,2006.
[8] A. Herzog, N. Rotella, S. Mason, F. Grimminger, S. Schaal, and [33] Z. Manchester and M. Peck, “Recursive inertia estimation with
L. Righetti, “Momentum control with hierarchical inverse dynamics semidefiniteprogramming,”inAIAAGuidance,Navigation,andCon-
onatorque-controlledhumanoid,”AutonomousRobots,vol.40,no.3, trolConference,2017.
pp.473–491,2016. [34] D. Bertsimas and J. Sethuraman, “Moment problems and semidef-
[9] S. Kuindersma, R. Deits, M. Fallon, A. Valenzuela, H. Dai, F. Per- inite optimization,” in Handbook of Semidefinite Programming, ser.
menter, T. Koolen, P. Marion, and R. Tedrake, “Optimization-based InternationalSeriesinOperationsResearch&ManagementScience,
locomotion planning, estimation, and control design for the atlas H.Wolkowicz,R.Saigal,andL.Vandenberghe,Eds. SpringerUS,
humanoidrobot,”AutonomousRobots,pp.1–27,2015. 2000,vol.27,pp.469–509.
[10] P. M. Wensing, A. Wang, S. Seok, D. Otten, J. Lang, and S. Kim, [35] J.J.Uicker,Jr.,“Dynamicforceanalysisofspatiallinkages,”Journal
“ProprioceptiveactuatordesignintheMITcheetah:Impactmitigation ofAppliedMechanics,vol.34,no.2,pp.418–424,June1967.
andhigh-bandwidthphysicalinteractionfordynamicleggedrobots,” [36] C. P. Neuman and P. K. Khosla, Adaptive and Learning Systems.
2017,IEEETransactionsonRobotics. SpringerUS,1986,ch.IdentificationofRobotDynamics:AnAppli-
[11] K. Ayusawa, G. Venture, and Y. Nakamura, “Identifiability and cationofRecursiveEstimation,pp.175–194.
identificationofinertialparametersusingtheunderactuatedbase-link [37] G.Legnani,F.Casolo,P.Righettini,andB.Zappa,“Ahomogeneous
dynamicsforleggedmultibodysystems,”Int.J.ofRoboticsResearch, matrix approach to 3D kinematics and dynamics — I. Theory,”
vol.33,no.3,pp.446–468,2014. MechanismandMachineTheory,vol.31,no.5,pp.573–587,1996.
[12] C. D. Sousa and R. Cortesa˜o, “Physical feasibility of robot base [38] E.Atchonouglo,C.Valle´e,T.Monnet,andD.Fortune´,“Identification
inertialparameteridentification:Alinearmatrixinequalityapproach,” of the ten inertia parameters of a rigid body,” Journal of Applied
Int.J.ofRoboticsResearch,vol.33,no.6,pp.931–944,2014. MathematicsandMechanics,vol.72,no.1,pp.22–25,2008.
[13] D. Pucci, F. Romano, and F. Nori, “Collocated adaptive control of [39] T.Yoshikawa,Dynamics. MITPress,2003,pp.81–126.
underactuatedmechanicalsystems,”IEEETransactionsonRobotics, [40] K.Schmudgen,“TheK-momentproblemforcompactsemi-algebraic
vol.31,no.6,pp.1527–1536,Dec2015. sets.”MathematischeAnnalen,vol.289,no.2,pp.203–206,1991.
[14] S. Traversaro, S. Brossette, A. Escande, and F. Nori, “Identification [41] L. Fialkow and J. Nie, “Positivity of riesz functionals and solutions
offullyphysicalconsistentinertialparametersusingoptimizationon of quadratic and quartic moment problems,” Journal of Functional
manifolds,”inIEEE/RSJInt.Conf.onIntelligentRob.andSys.,Oct Analysis,vol.258,no.1,pp.328–356,2010.
2016,pp.5446–5451. [42] J.-J.E.SlotineandJ.A.Coetsee,“Adaptiveslidingcontrollersynthe-
[15] J. Jovic, A. Escande, K. Ayusawa, E. Yoshida, A. Kheddar, and sisfornon-linearsystems,”InternationalJournalofControl,vol.43,
G. Venture, “Humanoid and human inertia parameter identification no.6,pp.1631–1651,1986.
using hierarchical optimization,” IEEE Transactions on Robotics, [43] P. A. Ioannou and J. Sun, Robust adaptive control. Courier
vol.32,no.3,pp.726–735,June2016. Corporation,2012.
[16] C.G.Atkeson,C.H.An,andJ.M.Hollerbach,“Estimationofinertial [44] M. Grant and S. Boyd. Cvx: Matlab software for disciplined
parametersofmanipulatorloadsandlinks,”TheInternationalJournal convex programming, version 2.0 beta. [Online]. Available: http:
ofRoboticsResearch,vol.5,no.3,pp.101–119,1986. //cvxr.com/cvx
[17] J.-J.E.SlotineandW.Li,“Ontheadaptivecontrolofrobotmanip- [45] E. D. Andersen, C. Roos, and T. Terlaky, “On implementing a
ulators,”Int.J.ofRoboticsResearch,vol.6,no.3,pp.49–59,1987. primal-dual interior-point method for conic quadratic optimization,”
[18] K.Ayusawa,G.Venture,andY.Nakamura,“Real-timeimplementa- MathematicalProgramming,vol.95,no.1,pp.249–277,2003.
tionofphysicallyconsistentidentificationofhumanbodysegments,”
inIEEEInf.Conf.onRob.andAuto.,May2011,pp.6282–6287.