1
On Centroidal Dynamics and
Integrability of Average Angular Velocity
Alessandro Saccon, Silvio Traversaro, Francesco Nori, Henk Nijmeijer
Abstract—In the literature on robotics and multibody dy-
namics, the concept of average angular velocity has received
considerable attention in recent years. We address the question
of whether the average angular velocity defines an orientation
frame that depends only on the current robot configuration and
provide a simple algebraic condition to check whether this holds.
In the language of geometric mechanics, this condition cor-
responds to requiring the flatness of the mechanical connection
associated to the robotic system. Here, however, we provide both
a reinterpretation and a proof of this result accessible to readers
with a background in rigid body kinematics and multibody dy-
namics but not necessarily acquainted with differential geometry,
still providing precise links to the geometric mechanics literature.
This should help spreading the algebraic condition beyond the
scope of geometric mechanics, contributing to a proper utilization
and understanding of the concept of average angular velocity.
I. INTRODUCTION
The total momentum of floating articulated robotic systems,
such as aerial manipulators and humanoid robots, has received
considerable attention in the robotic literature. There is a
growing consensus that the dynamics of total momentum can
be used as a reduced but still exact model of the original
system that can ease, e.g., the development of posture and bal-
ance controllers as well as planning algorithms for humanoid
robots [1], [2], [3], [4], [5], [6], [7], [8]. The total momentum
is defined as the sum of all linear and angular momenta
of the (rigid) bodies composing the articulated system. The
momentum is typically computed with respect to a frame
which orientation is that of the inertial frame and origin is
the total center of mass [9]. Its time evolution depends only
on the external forces and torques acting on the system, such
as gravity and contact forces. The total angular momentum
can be split into a linear and an angular component. The
linear component, when divided by the total mass, captures the
average linear velocity of the mechanism, i.e, the velocity of
the center of mass (CoM). Although still debating about which
value it should be regulated to, the angular component has
been used to define a concept of “average angular velocity” of
the entire mechanism. The concept of average angular velocity
is discussed in [9] and it corresponds, roughly speaking, to the
angular velocity of entire mechanism, for the given pose but
assuming the internal joints to be fixed, corresponding to the
current value of the angular momentum.
The geometric mechanics community has been employing
a concept strictly related to the average velocity, the locked
velocity, for at least two decades [10, Section 3.3] [11, Section
5.3], but an explicit link between the two concepts appears to
be missing. We aim at providing this link in this paper, hoping
that this will help the communication of key results among
different research communities with a different theoretical
background and research focus.
Paper’s contributions: (1) Equation of motions for a free-
floating robot written employing a robot-specific nota-
tion consistent with differential geometry notation: this
paper presents the dynamics of a simply supported articulated
rigid-body system subject to external forcing by employing
a notation which is inspired by the spatial vector algebra
notation [12], [13] while allowing for a one-to-one mapping
with the concepts used in geometric mechanics and differential
geometry related to the theory of differentiable manifolds and
Lie groups [14], [15] (e.g., X corresponds to Ad, × to ad,
and
¯×∗to −ad∗, see next section for details). While the
employment of Lie group formalism is robotics is certainly
not new (see, e.g., the excellent publications [16] and [17]),
we felt that an explicit parallelism between spatial vector
algebra and Lie group notations was still missing. This holds
true, in particular, for the free-floating dynamics case treated
here, required for assessing the integrability of the average
angular velocity; (2) difference between the total momentum
Noether’s theorem and total momentum as commonly
encounter in robotics: we highlight how the total momentum
considered in the robotic literature [9] actually differs from
the total momentum (the momentum map) that derives from
the application of geometric mechanics version of Noether’s
theorem. As a consequence, the average velocity in the robotic
literature and locked velocity in the geometric mechanics
literature represent the same velocity, although expressed
with respect two different reference frames. This apparently
unessential detail plays however a key role (see discussion in
Section III-D) in understanding the main result of this paper.
In highlighting the difference between the two velocities, the
angular momentum of the center of mass, an extra component
of the angular part of the total momentum once expressed with
respect to the inertial frame, receives particular attention; (3)
Integrability condition for the average angular velocity:
We use the results and insights of the previous two points
to show that the fundamental question “when does the inte-
gration of the average angular velocity define an orientation
frame that depends only on the current internal joint position,
independently of their time evolution?” is equivalent to asking
if a series of vector valued functions can be interpreted
as the (right trivialized) partial derivatives of a nonlinear
function of the internal joints. While pointing out that this
is equivalent to requiring, in terms of geometric mechanics,
that the associated mechanical connection is flat (see, e.g., the
discussion on holonomy in [14, Section 3.14] and reference
there in), we provide a reinterpretation of the result, and the
arXiv:1701.02514v1  [cs.RO]  10 Jan 2017
2
associated algebraic condition ensuring the flatness of the
connection, that can be easily followed by a reader acquainted
with multibody dynamics, kinematics of rigid transformations,
undergraduate calculus, and the understanding of Schwartz’s
theorem (symmetry of second derivatives), but with no or
limited experience with differential geometry. Our hope is
to contribute to the diffusion and utilization of this algebraic
condition beyond the scope of geometric mechanics.
Rigid body notation is reviewed in Section II. Section III
reviews the dynamics of floating mechanical systems and the
evolution of the total momentum. In Section IV, we review
the concepts of average and locked velocities, centroidal
frame, and provide the algebraic condition to show when it
depends only of the current pose and shape of the mechanism.
Conclusions and a discussion are provided in Section VI.
II. NOTATION
This section introduces basic notation used for dynamics
computations. We refer to [18] for details.
Given a vector w = (x, y, z)T ∈R3, w∧(read w hat) is
the 3 × 3 skew-symmetric matrix associated with the cross
product × in R3, so that w∧x = w × x. Given the skew-
symmetric matrix W = w∧, W ∨∈R3 (read W vee) denotes
the inverse transformation. The set of rotational matrices is
denoted SO(3), the set of rigid transformations SE(3). An
element of SE(3) has the structure [R, o ; 01×3, 1] ∈R4×4,
with R ∈SO(3), o ∈R3, where ; denotes row concatenation.
A. Frames notation
A frame is defined by a point, called origin, and an
orientation frame [19]. We use capital letters to indicate
frames. Given a frame F, we denote with oF its origin and
with [F] its orientation frame, writing F = (oF , [F]).
A, B, . . .
Reference frames
p
An arbitrary point
B[A]
Frame with origin oB and orientation [A]
B. Coordinates vectors and transformation matrices notation
Ap ∈R3
Coordinates of p w.r.t. A
AoB ∈R3
Coordinates of oB w.r.t. to A
ARB ∈R3×3
Rotation matrix from [B] to
[A]
AHB =
h
ARB
AoB
01×3
1
i
Rigid transf. from B to A
AXB =
h ARB
Ao∧
B
ARB
03×3
ARB
i
Velocity transf. from B to A
CvA,B =
h CvA,B
CωA,B
i
∈R6
Velocity
of
B
w.r.t.
to
A
expressed in C
BvA,B× =

Bω∧
A,B
Bv∧
A,B
03×3
Bω∧
A,B

Vector cross product in R6
AXB =
h
ARB 03×3
Ao∧
B
ARB
ARB
i
Wrench transf. from B to A
(AXB = AX−T
B )
Bf =
h CfA,B
CτA,B
i
∈R6
Coordinates of the wrench f
w.r.t. B
BvA,B ¯×∗=

Bω∧
A,B
03×3
Bv∧
A,B
Bω∧
A,B

Dual cross product in R6
BMB =
h
m13×3
m Bc∧
−m Bc∧
BIB
i
Generalized
inertia
matrix
w.r.t. frame B
In the expression for BMB (where B is typically a body
fixed frame), m is the body mass, Bc the CoM coordinates,
and BIB the rotational inertia w.r.t. B. We use
DJA,C/B
to indicate the Jacobian relating the velocity of frame C with
respect to A expressed in D with the velocity of the base
link expressed in B, so that DvA,C = DJA,C/B(q) ν with
ν = (BvA,B, ˙qJ).
C. Frame velocity representation
The velocity of B w.r.t. A is given by A ˙HB, however, it
is more common to express it as a one the following six-
dimensional vectors,
AvA,B =
A ˙oB −Aω∧
A,B
AoB
AωA,B

,
BvA,B =
 BRAA ˙oB
BRAAωA,B

,
B[A]vA,B =
 A ˙oB
AωA,B

,
where AωA,B := (A ˙RBAR⊤
B)∨. We refer to AvA,B, BvA,B,
and B[A]vA,B as, respectively, the right-trivialized, the left-
trivialized, and the mixed velocity of B w.r.t. A. The mixed
representation is also known as hybrid representation [20].
The left- and right-trivialized representations are widespread
in the literature of Lie group-based geometric mechanics
[16] (where they are called spatial and body velocities) and
recursive robot dynamics algorithms [12], [21] (where they
are called spatial velocities). The mixed velocity is commonly
used in multi-task control frameworks [22], [23], [2].
D. Single Rigid Body Dynamics
Given a rigid body whose position in space is determined
by AHL with L fixed to the body the classical Newton-Euler
equations are written, in a combined form, as
LML
L ˙vA,L + LvA,L ¯×∗
LML
LvA,L = Lf
(1)
with Lf denoting the external wrench (combined force and
torque vector) expressed w.r.t. L and
¯×∗denotes the dual
6D cross product (LvA,L ¯×∗is equivalent to −ad∗
LvA,L in the
language of Lie groups). We use the letter L since a rigid body
on an articulated mechanism is usually referred to as a link.
III. RIGID-BODY DYNAMICS
A. Floating systems with gravity and external contact forces
Consider a robotic system whose configuration is given by
q = (H, s) ∈SE(3)×RnJ, where H = AHB denotes the base
link’s homogeneous transformation matrix and s represents the
displacement of the nJ internal joints. We will refer to H as
the pose and to s as the shape of the robot. The velocity of
the mechanism is parameterized via ν = (v, ˙s) ∈R6 × RnJ
with v = BvA,B denoting the velocity of the base frame with
respect the inertial frame expressed in the base frame (BvA,B
is a left trivialized velocity, cf. the notation section).
3
The dynamics of a floating articulated robotic system such
as, e.g., a humanoid robot [24] is usually written as
M(q) ˙ν + C(q, ν)ν + G(q) = [0 ; τ] +
X
i
(iJ)T
if
(2)
where M, C, and G are, respectively, the mass matrix, Coriolis
matrix, and potential force vector, τ is the internal joint
torques, and iJ and if are the i-th contact Jacobian and contact
force, both expressed with respect to a contact frame Ci,
fixed with respect to its corresponding link. To derive (2),
one can take a Newton-Euler approach summing up all the
contributions of the internal and external forces for each body
using (1) or setting up a Lagrangian L(H, s, ˙H, ˙s) and employ
the Euler-Lagrange equations. However, as H is not a vector
quantity, either one use a local vector parametrization for
H (based, e.g., on Euler angles) or employ the tools from
geometric mechanics and form the left trivialized Lagrangian
l(H, s, v, ˙s) = 1
2
v
˙s
T  L(s)
A(s)
AT (s)
S(s)
 v
˙s

(3)
where v = BvA,B satisfies ˙H = Hv∧and L, A, and S are
suitable partitions of the overall mass matrix M(q) appearing
in (2) in accordance with the dimension of v and ˙s. The
matrix L is typically referred to as the locked inertia tensor
as is corresponds to the (generalized) inertia of the entire
mechanism computed with respect to B assuming no motions
of its internal joints. To be more precise, one should write L
as BLB and A as BA to indicate the output (and, for L, also
input) frame that these transformations accept. This should
help, e.g., to better interpret the expression for the combined
linear and angular momentum given by (9), later in the text.
The dynamics of the articulated mechanism can be then
derived using Hamel equations (see, e.g., [25], [15, Section
13.6], [26]), namely
d
dt
∂l
∂v + v ¯×∗∂l
∂v = 0
(4)
d
dt
∂l
∂˙s −∂l
∂s = 0.
(5)
Hamel equations are a combination of standard Euler-
Lagrange equations (5) and Newton-Euler equations (4), the
latter also called Euler-Poincar´e equations for a generic Lie
group [15, Section 13.5]. In the presence of internal and
external forces and the presence of potential energy due to,
e.g., the effect of gravity, (4)-(5) become
d
dt
∂l
∂v + v ¯×∗∂l
∂v = H−1 ∂l
∂H +
X
i
(iX)T
if
(6)
d
dt
∂l
∂˙s −∂l
∂s = τ +
X
i
(iS)T
if,
(7)
where τ and if are as in (2), H−1∂l/∂H the vector represen-
tation of the linear map w 7→D1l(H, s, v, ˙s) · Hw∧, w ∈R6,
and iX and iS define, respectively, the pose and shape parts
of the i-th contact Jacobian iJ. More precisely, the (mixed)
velocity of the i-th contact point satisfies
iv = iJ ν = iXv + iS ˙s
(8)
with iv := Ci[A]vA,Ci and iJ := Ci[A]JA,Ci/B the i-th contact
Jacobian (we refer to Section II for a clarification on the
notation DJA,C/B). Note that, by definition, iX = Ci[A]XB,
implying iXT = BXCi[A], a wrench transformation. Except
for the notation, (6)-(7) are equivalent to the more common (2)
but they provide extra structure that helps to understand the
definition and time evolution of the total momentum directly
from the equations of motion. From a computational point of
view, forward and inverse dynamics for (2) can be obtained
using, e.g., the floating-base recursive Newton-Euler algorithm
and composite-rigid-body algorithm presented in [12].
B. The total momentum expressed in the inertial frame
The Lagrangian (3) is not a function of H meaning it is
invariant with respect to a rigid transformation. As shown in
Appendix A, standard results of geometric mechanics imply
that the quantity
AJ = AXB (L v + A ˙s)
(9)
is a constant of motion for the unforced system. In (9), L and
A are as in (3) and AXB as in Section II with A denoting the
inertial frame and B the base link frame.
Recalling that (3) is obtained by summing up all kinetic
energies of each link dynamics (1) employing link Jacobian
with a structure similar to (8), it is straightforward to recognize
in AJ the total momentum given by the sum of the all the
linear and angular momenta of each rigid body expressed with
respect to the origin of A. When gravity and external forcing
are present, AJ evolves according to
d
dt AJ = AXB
 
H−1 ∂l
∂H +
X
i
(iX)T
if
!
.
(10)
This result can be derived directly from a straightforward mod-
ification of Noether’s theorem (for a proof of the geometric
version of Noether’s theorem, see [14, Chapter 3]). One careful
inspection of the above formula shows, however, that (10) is
actually equivalent to (6), only written in the inertial frame A.
C. The total momentum expressed at the center of mass
The momentum of the system can be expressed also
with respect to other frames. In particular, the frame G :=
(pcom, [A]), that has as origin the combined CoM pcom and the
orientation of the inertial frame A, is commonly found in the
robotic literature [27], [9]. With respect to G, the momentum
is given by
GJ (H, s, v, ˙v) := GXA(H, s) AJ (H, s, v, ˙v).
(11)
We refer to GJ as the centroidal momentum (in accordance
with, e.g., [9]). Remarkably, when no external forces and
potential are present, this quantity is also constant as AJ
given in (9) is constant and because (G ˙XA)AJ is always a
zero. This last fact is related to the angular momentum of the
CoM. Denoting with m the total mass, the angular momentum
of the CoM is simply given by Apcom × m A ˙pcom. The only
difference between GJ and AJ is indeed that AJ contains
within its angular part (i.e., its last three elements) also the
4
angular momentum of the CoM. Then (G ˙XA)AJ ≡0 as
A ˙pcom × m A ˙pcom ≡0. This also implies that replacing A
with G in (10) is all we need to obtain the evolution of GJ .
D. Locked and average velocities
In geometric mechanics, a special role is played by the left-
trivialized locked velocity Bvloc defined in such a way that
BJ = L(s)v + A(s)˙s =: L(s) Bvloc
(12)
or, equivalently,
Bvloc := v + L−1(s)A(s)˙s.
(13)
The locked velocity is, for each instant of time, the velocity at
which the base link should move, while considering the inter-
nal joints as locked, to get the same value of the momentum
corresponding to the current velocity of the mechanism. Note
how the locked velocity is expressed with respect to the base
frame, so it is only a function of v, s, and ˙s.
In the robotic literature, the average velocity is defined as
Gvave := GXBBvloc [9]. Note that we can obtain Gvave from
Gvave = GL−1
G
GJ
(14)
with GLG := GXBBLBBXG block diagonal. At this point is
key to observe that Gvave depends also on H, other than v, s,
and ˙s while Bvloc does not. We will show in the next section,
that the latter is then preferable in answering the integrability
question for the average (angular) velocity.
As a side note, it is worth recalling at this point that both
the locked velocity and average velocity can be used to block
diagonalize the mass matrix. However, as the kinetic energy
is now written with respect to another velocity one must be
careful in deriving the equations of motion from it. When
using the locked velocity, Lagrange-Poincar´e equations (see,
e.g., [15, Chapter 13]) can be use to retrive a set of equations
equivalent to (4)-(5).
IV. THE CENTROIDAL FRAME AND THE MAIN RESULT
In this section, we recall the concept of centroidal frame
and provide the algebraic condition ensuring that it depends
only on the configuration (H, s).
A. The centroidal frame and the main question
The definition of the locked velocity given by (13) allows
one to write the momentum with respect to A simply as
AJ = AXB
BLB
Bvloc.
(15)
Posing ALA := AXBBLBBXA and Avloc := AXBBvloc, the
equation above simply becomes
AJ = ALA
Avloc.
(16)
For a given initial configuration AHC(t0) ∈SE(3) at a given
time t = t0, one can then integrate the differential equation
A ˙HC = Av∧
loc
AHC
(17)
to get a frame that has, as right trivialized velocity, the
locked velocity Avloc. A key remark that keeps appearing
in the robotic literature [27], [9], [28] is that the solution
AHC of (17) is not guaranteed to depend only on the con-
figuration (H, s). That is, when AHC satisfies AHC(t) =
AF(H(t), s(t)) for a suitable function AF : SE(3) × RnJ →
SE(3). It is well known that this does not always happen
as in the simulation results presented in [29] and references
therein. What appear to be less known is that there is a simple
condition to check when this happens and that this is related
to asking if the columns of L−1A are partial derivatives of a
nonlinear function of the joint displacements.
Note that the initial condition for C at t = t0 is completely
arbitrary and therefore the function AF(H, s), where it exists,
is determined up to an arbitrary right multiplication by an ele-
ment of SE(3). Furthermore, given the fact that the kinematics
(17) is actually invariant to an arbitrary pose transformation,
one gets the extra condition that if AF exists, it must be of the
form AF(H, s) = H BF(s) with BF(s) : RnJ →SE(3). The
dependence of the centroidal frame only on the configuration
is therefore equivalent to the existence of this SE(3)-valued
function of the internal joints. Tipically, the frame C at t = t0
is taken to have its origin coinciding with the total center of
mass because it can be shown (see remark below) that (17)
will maintain the equivalence of the two points: this justifies
the use of centroidal frame as name for C.
Remark. Independently of the existence of a configuration-
dependent-only frame satisfying AHC(t) = AF(H(t), s(t)),
the CoM pcom has always constant coordinates with respect
to a frame C that evolves according to (17). In formulas,
C ˙pcom = d/dt Cpcom ≡0. A proof of this fact is given in the
Appendix B. Therefore, in case indeed we can find a function
AF such that AHC(t) = AF(H(t), s(t)), it then seems natural
to choose the frame C such that pcom is its origin.
■
B. The main result
Define
A(s) := L−1(s)A(s)
(18)
with L(s) and A(s) expressed with respect to B as in (3).
Then, the following holds.
Proposition 4.1: The centroidal frame satisfying (17) is
integrable, that is, there exist a (differentiable) function BF :
RnJ →SE(3) such that
AHC(t) = H(t) BF(s(t))
(19)
if and only if
Bij := ∂Ai
∂sj
−∂Aj
∂si
+ Ai × Aj ≡0
(20)
for every i, j ∈{1, 2, . . . , nJ}, where Ai, Aj are the i-th and
j-th columns of A and × the vector cross product in R6. □
Proof.
The result is, roughly speaking, a generalization to
SE(3) of Schwartz’s theorem (symmetry of second derivatives)
and the related theorem stating that closed differential forms
are locally exact (existence of a potential function). We start by
the observation that if we take an arbitrary sufficiently smooth
5
function F : RnJ →SE(3) the right trivialized velocity
associated to the homogeneous transformation matrix
H F(s) ∈SE(3),
(21)
with H = H(t) ∈SE(3) and s = s(t) ∈RnJ sufficiently
smooth curves is equal to
 d
dt(HF)(HF)−1
∨
= AXB ( v + A(s)˙s ).
(22)
The above formula has exactly the same structure of the
locked velocity given in (13) with the extra interpretation
that the columns of the matrix A(s) above satisfy, for i ∈
{1, 2, . . . , nJ},
Ai(s) =
∂F
∂si
(s)F(s)−1
∨
∈R6,
(23)
that is, Ai can be interpreted as the right trivialized partial
derivatives of the nonlinear function F depending on the
internal joints. Thinking F as a function F : RnJ →R4×4
one knows that its second derivative must satisfy
∂2F
∂sj∂si
=
∂2F
∂si∂sj
(24)
for every i and j. Writing ∂F(s)/∂si = A∧
i (s)F(s) using
(23), one can write the left hand side of (24) as
∂Ai
∂sj
∧
(s)F(s) + A∧
i (s)A∧
j (s)F(s)
(25)
and similarly for the right hand side by exchanging i with
j. Equating the two expressions and multiplying on the right
by the inverse of F(s), one obtains straightforwardly (20) by
recalling Jacobi identity A∧
i A∧
j −A∧
j A∧
i = (Ai × Aj)∧. This
proves the necessity of the condition. Sufficiency is provided
constructively, using
F(s) = ∆nJ(s1, s2, . . . , snJ)
(26)
where ∆nJ is given below and showing that F in (26) satisfies
(23) as long as (20) holds. In (26), ∆nJ and the functions
∆i : Ri →SE(3), i ∈{1, 2, . . . , nJ} are defined recursively
for a given value s = (s1, . . . , snJ) as the solution at σ = si
of the matrix differential equation
d
dσ ∆i = A∧
i (s1, . . . , si−1, σ, ..., 0) ∆i
(27)
with ∆i evaluated at (s1, . . . , si−1, σ) ∈Ri with initial
condition
∆i(s1, . . . , si−1, 0) = ∆i−1(s1, . . . , si−1)
(28)
and ∆1(0) = F(0) ∈SE(3) arbitrary (the desired orientation
of the centroidal frame for s = 0). Note that d∆i/dσ in (27)
equals ∂∆i(s1, . . . , si−1, σ)/∂si. In the following, we provide
the proof that (23) holds for nJ = 2. Proving (23) for nJ > 2
is a straightforward but tedious calculation that follows from
the technique used when nJ = 2. Given F(s) := ∆2(s1, s2), it
is immediate to see one must have ∂F/∂s2(s) = A∧
2 (s)F(s).
Showing ∂F/∂s1(s) = A∧
1 (s)F(s) is, instead, more involved
and requires (20). From (26), (27), and (28), for the case nJ =
2, one can write ∂F/∂s1(s) as
Z s2
0
∂
∂s1
(A∧
2 (s1, t)∆2(s1, t)) dt + ∂∆2
∂s1
(s1, 0),
(29)
that can be further expanded into
Z s2
0
∂A∧
2
∂s1
∆2 + A∧
2
∂∆2
∂s1
dt + A∧
1 (s1, 0)∆2(s1, 0),
(30)
where the terms inside the integral are evaluated at (s1, t).
Using (20), one gets that the integral above can be written as
Z s2
0
∂A∧
1
∂t
+ (A1 × A2)∧

∆2 + A∧
2
∂∆2
∂s1
dt
(31)
Using again the Jacobi identity and recalling that ∂∆2/∂t =
A∧
2 ∆2, the above integral can be rewritten as
Z s2
0
∂
∂t (A∧
1 ∆2) + A∧
2
∂∆2
∂s1
−A∧
1 ∆2

dt
(32)
where all terms are evaluated at (s1, t). By assuming
∂F/∂s1(s) = ∂∆2/∂s1(s) = A∧
1 (s)∆2(s), the second term
in the integral of (32) vanishes and allowing one to rewrite
(30) as
Z s2
0
∂
∂t (A∧
1 (s1, t)∆2(s1, t)) dt + A∧
1 (s1, 0)∆2(s1, 0) (33)
which equals A∧
1 (s)∆2(s) = A∧
1 (s)F(s), with no contradic-
tion on the assumption.
□
Remark. When the underlying Lie group is SE(3), the
expression given in (20) is equivalent to the curvature of a prin-
cipal connection (see [14, Chapter 2] and reference therein for
a concise and convenient summary of principal connections).
The curvature of a principal connection is typically written as
Bd
αβ(s) := ∂Ad
α
∂sβ −
∂Ad
β
∂sα + Cd
abAa
αAb
β
(34)
for d, α, β
∈{1, . . . , nJ} with Ai
j denoting the entry
(i, j) of A and Ca
bc the structure constants of the Lie group,
representing the Lie bracket operation that, for SE(3), is the
6D vector cross product appearing in (20). Therefore, the
right hand side of (20) is equivalent to (34). We find (20),
however, more accessible than (34) in particular to researchers
in multibody dynamics employing spatial vector notation [13],
not acquainted with differential geometry. An alternative proof
of Proposition 4.1 could be obtained by showing that when
(34) is identically zero (flatness of the connection) this implies
the existence of a function BF such that (23) holds. We
are not aware, though, of an accessible source where this is
clearly stated as in the proof of the proposition above and this
why we though worth presenting a proof that requires a basic
knowledge of the velocity kinematics of SE(3) and standard
results of calculus (Schwartz’s theorem) to be understood. The
closest we can get is Chapter II, Section 9, of the classical text
[30], which clearly requires a deep knowledge on differential
geometry to be fully understood. For sake of completeness,
we also mention that (24) could be replaced with the second
6
covariant derivative of F with respect to the (0) Cartan-
Schouten connection [31], which is known to be symmetric,
to obtain the following expression, similar to (25) but now
coordinate independent,
∂Ai
∂sj
(s) + 1
2 Ai(s) × Aj(s)
∧
F(s) ,
(35)
which leads then again to (20). Principal connections have
been also employed to study nonholonomic locomotion, as
the nonholomic constraint of a robot can be written in a form
equivalent to (22): see, e.g., [32]. We hope the presentation
given in this paper will help also accessing that literature.
C. The link between locked and average velocity
In this subsection, we elaborate further on the remark given
in the subsection IV-A showing that, when choosing AoC =
Apcom, the centroidal kinematic is simply given by
A ˙oC = A ˙pcom
(36)
A ˙RC = Aω∧
loc
ARC
(37)
with Aωloc given by the angular velocity component of Avloc.
We first show that, independently from where AoC is located,
Gvave and Avloc satisfy
A ˙pcom
Aωloc

= Gvave = GXA
Avloc.
(38)
This shows that the average angular velocity Gωave coin-
cides with the locked angular velocity Aωloc. Proving (38)
is obtained by employing the remark in the subsection IV-A
regarding the invariance of the coordinates of the CoM and
the following lemma.
Lemma. Given the differential equation
A ˙HC = Av∧AHC
(39)
assume there is a point p such that its coordinates Cp with
respect to C are constant. Define G = (p, [A]) so that G has
p as origin and the same orientation of A. Then, the velocity
Av written with respect to G equals
Gv = GXA
Av =
A ˙p
Aω

(40)
where Aω denotes the angular velocity component of Av. ■
Finally, to obtain (36)-(37), we employ (38) to express Avloc
in terms of Gvave and substitute it into (17), obtaining
A ˙oC = A ˙pcom + Aω∧
loc(AoC −Apcom)
(41)
A ˙RC = Aω∧
loc
ARC.
(42)
where we recall AHC = (ARC, AoC; 01×3, 1) ∈SE(3). As
we have selected AoC = Apcom, the result follows.
Fig. 1. The free-floating three link model. In this figure, s1 and s2 represent
the relative orientations of the two distal links with respect to the base. See
main text for a full description of the figure.
V. A NUMERICAL EXAMPLE
In this section, a simple example to illustrate the use of the
integrability condition (20) is given. We consider a mechanism
with two internal DOFs. This is the minimal number of DOFs
to observe the nonintegrability, because, for one DOF, (20) is
always trivially satisfied.
We numerically integrate (17), performing a motion that
starts and ends at the same internal joint configuration. The
base link will not, in general, return to the original pose. The
centroidal frame will always return to the original orientation
relative to the base link, if and only if (20) holds. In both
cases, as explained in the Remark of Section IV-A, its CoM
will return to its original position.
An illustration of the mechanism is given in Figure 1. The
mechanism is composed by three rigid bodies: a free-floating
base link (yellow) and two distal links (cyan and magenta).
The distal links are connected directly to the base via two
independently actuated revolute joints. For both links, the
offset between their center of mass and joint axis is identical
and denoted with d. To each body we firmly attach three
coordinate frames, indicated as B, 1 and 2 in the figure, each
centered at the corresponding body’s CoM. The base link mass
is 1 kg. Each distal link mass is also 1 kg. For the base link,
the rotational inertia (about the axis passing through the CoM
and orthogonal to the base link face) is 4 kg m2. For distal
links, the inertia is 1 kg m2. The rotational inertia with respect
to the other directions are non influential (we are considering
a planar mechanism) and can be assigned arbitrarily finding
the same result provided below.
We verify (20) for two different values of d: namely, for
d = 1 and d = 0. For d = 1, B12 = −B21 is equal, up to a
division by the factor (2 C1−2 + 6 S1 −6 S2 −28)2, to


2(C1+C2) (4 C1+4 C2−3 C1 S2+3 C2 S1)
2 C1 (4 S1+4 S2−3 S1 S2−3 S2S2)+2 C2 (4 S1+4 S2+3 S1 S2+3 S1S1)
0
0
0
−18 S1−2−24 C1−24 C2


where S1 := sin(s1), S2 := sin(s2), C1 := cos(s1), C2 :=
cos(s2), S1−2 := sin(s1 −s2), and C1−2 := cos(s1 −s2).
For d = 0, B12 = B21 ≡0. Details of the straightforward but
tedious computations are not provided for space limitations.
The conclusion is that, just for d = 0, the integration of
the average angular velocity will always produce a centroidal
frame whose orientation is only a function of the internal joint
7
displacements. This is confirmed by the animation snapshots
given in Figure 2 corresponding to two simulations for dif-
ferent values of d. The complete animation is available as a
multimedia attachment to this paper. For both cases, the joints
follow the sinusoidal trajectories given by
s1(t) = 3π
2

cos
2π
T t

−1

, s2(t) = π
2 sin
2π
T t

,
starting and ending in a mechanism configuration with the
distal links in a vertical position. These results are independent
of the particular initial pose and velocity of the base and
therefore, in Figure 2, only the relative pose of the centroidal
frame with respect the base link is shown.
VI. CONCLUSION AND DISCUSSION
In this paper we have established a clear link between
the concept of average angular velocity found in the robotics
and multibody dynamics literature and the concept of locked
velocity from the geometric mechanics literature. We have
provided an accessible definition and proof on the key al-
gebraic condition that can be used to establish when the
centroidal frame (in particular, its orientation) depends only on
the current robot configuration and not on its time evolution.
For systems with a small number of links, the condition could
be checked symbolically and, for more complex mechanism,
we expect efficient algorithms could be easily developed as
just the differentiation of the matrices L and A, typically
involved in the computation of the Coriolis’ matrix associated
to the dynamics, is required. Computationally, our experience
suggests to compute the locked velocity with respect the frame
whose origin is the CoM and orientation given by the base link
frame. The advantage of using this frame is that the locked
inertia matrix becomes diagonal, still depending only on the
shape variables.
The centroidal frame is always integrable when dealing with
a system possessing only one internal degree of freedom. Note
how this result would remain valid for a mechanism where
virtual constraints are employed to ensure that this happens,
making the internal degrees of freedom algebraically related
to a single variable that acts as the only internal degree of
freedom (see, e.g., the concept of gate variable in [33] and the
work that stemmed from it in employing virtual constraints in
the context of robot locomotion [34]). Our presentation might
therefore help in taking a different perspective to those results.
We have shown that mechanisms with two internal degrees
of freedom can also possess an integrable centroidal frame,
but this is not guaranteed as common experience and previous
investigations (e.g., the so called falling cat problem) have
already shown. It might be interesting to understand if there
are some rules in constructing a (nontrivial) mechanism such
that the integrability is satisfied. The centroidal frame for those
mechanism might provide an interesting natural output to use
in controlling the gross motion of the systems both in position
and in orientation. Even when dealing with a mechanism
where the centroidal frame is not integrable, one idea could
be to try to assign the external wrenches so as to guarantee
the integrability condition: one of the motivations to continue
investigating the centroidal dynamics even further.
APPENDIX
A. The momentum map and the total momentum
Noether’s theorem can be employed to conclude that, when
no gravity and external forces are applied, the momentum AJ
given by (9) is constant. In the context of geometric mechanics,
(9) is the momentum map J defined as
⟨J (q, ˙q), ξ⟩= FL(q, ˙q) · ξQ(q)
(43)
where FL(q, ˙q) · z
:= limt→0(L(q, ˙q + tz) −L(q, ˙q))/t
denotes the fiber derivative of the Lagrangian in the direction
z ∈TqQ and ξQ(q) the infinitesimal generator of the group
action formally defined as ξQ(q) = d/dt|t=0Φexp(tξ)(q). The
function Φg is the action of the symmetry group on the
configuration space: in the context of this paper, g ∈SE(3),
the configuration space Q = SE(3) × RnJ, and the group
action Φg : Q →Q is simply given by
Φg(H, s) = (gH, s),
(44)
corresponding to a rigid transformation of the entire robot ac-
cording to g that leaves invariant the shape s. The infinitesimal
generator associated to (44) is therefore
ξQ(q) = (ξ∧H, 0) ∈T(H,s)Q
(45)
and after straightforward computations one gets that the mo-
mentum map equals (9) as the right hand side of (43) is
FL(q, ˙q) · ξQ(q) =
v
˙s
T  L(s)
A(s)
AT (s)
S(s)
 BXA ξ
0

.
(46)
For the reader that is familiar with Lie group theory, note
that, in (46), BXA = AdH−1. For more details on momentum
maps and related concepts, we refer the interested reader to
[15, Chapter 11] and [14].
B. The center of mass is always a fixed point
In this appendix, we prove that the center of mass is always
a fixed with respect to AHC obtained by the time integration
of (17). Requiting that the CoM to be fixed with respect to
the frame C is equivalent to ask that
A ˙¯pcom=Av∧
loc
A¯pcom
(47)
where A¯pcom are the homogeneous coordinates of pcom with
respect to A obtained by appending 1 to the standard coor-
dinates Apcom, i.e., A¯pcom := (Apcom; 1) where ; denotes
row concatenation. The proof of this fact derives from a
straightforward manipulation of the expression of the time
derivative of the identity A¯pcom =
AHCC ¯pcom assuming
Cpcom to be a constant. The right hand side of (47) can be
expressed with respect to frame the frame G = (pcom, [A])
obtaining the equivalent condition
A ˙¯pcom=AHG
Gv∧
loc
G¯pcom
(48)
where G¯pcom = (0, 0, 0, 1)T . Equation (48) is then equivalent
to A ˙pcom = ARGGvloc and to A ˙pcom = Gvloc, since ARG =
I. This last condition is always true deriving directly from the
8
t = 0 s
t = 2 s
t = 4 s
t = 6 s
t = 8 s
t = 10 s
Fig. 2. Evolution of the centroidal frame. Nonintegrable case with d = 1 (first row). Integrable case with d = 0 (second row)
fact that the momentum map J expressed in the G frame is
given by
GJ (H, s, v, ˙s) = GXB
BLB
Bvloc
= GLG
Gvloc
where GLG is block diagonal with first block on the diagonal
equal to mI3×3 with m the total mass and that the linear
momentum component of GJ is necessarily mA ˙pcom.
REFERENCES
[1] P. M. Wensing and D. E. Orin, “Improved computation of the
humanoid centroidal dynamics and application for whole-body control,”
International Journal of Humanoid Robotics, vol. 13, no. 01, p.
1550039, 2016. [Online]. Available: http://www.worldscientific.com/
doi/abs/10.1142/S0219843615500395
[2] G. Nava, F. Romano, F. Nori, and D. Pucci, “Stability analysis and
design of momentum-based controllers for humanoid robots,” in 2016
IEEE/RSJ International Conference on Intelligent Robots and Systems,
Oct 2016.
[3] T. Koolen, S. Bertrand, G. Thomas, T. De Boer, T. Wu, J. Smith,
J. Englsberger, and J. Pratt, “Design of a momentum-based control
framework and application to the humanoid robot atlas,” International
Journal of Humanoid Robotics, vol. 13, 2016.
[4] G. Garofalo, B. Henze, J. Englsberger, and C. Ott, “On the inertially
decoupled structure of the floating base robot dynamics,” 8th Vienna
International Conference on Mathematical Modelling (MATHMOD), pp.
322–327, 2015.
[5] H. Dai, A. Valenzuela, and R. Tedrake, “Whole-body motion planning
with centroidal dynamics and full kinematics,” in 2014 IEEE-RAS
International Conference on Humanoid Robots, 2014, pp. 295–302.
[6] C. Ott, M. A. Roa, and G. Hirzinger, “Posture and balance control for
biped robots based on contact force optimization,” in Humanoid Robots
(Humanoids), 2011 11th IEEE-RAS International Conference on. IEEE,
2011, pp. 26–33.
[7] S.-H. Lee and A. Goswami, “A momentum-based balance controller for
humanoid robots on non-level and non-stationary ground,” Autonomous
Robots, vol. 33, no. 4, pp. 399–414, 2012.
[8] S. Kajita, F. Kanehiro, K. Kaneko, F. Fujiwara, K. Harada, K. Yokoi, and
H. Hirukawa, “Resolved momentum control: Humanoid motion planning
based on the linear and angular momentum,” in IEEE International
Conference on Intelligent Robots and Systems, 2003, pp. 1644–1650.
[9] D. E. Orin, A. Goswami, and S.-H. Lee, “Centroidal dynamics of a
humanoid robot,” Auton. Robots, vol. 35, no. 2-3, pp. 161–176, Jun.
2013.
[10] J. E. Marsden, Lectures on Mechanics.
Cambridge University Press,
1992.
[11] A. M. Bloch, P. Krishnaprasad, J. E. Marsden, and R. M. Murray, “Non-
holonomic mechanical systems with symmetry,” Archive for Rational
Mechanics and Analysis, vol. 136, no. 1, pp. 21–99, 1996.
[12] R. Featherstone, Rigid body dynamics algorithms.
Springer, 2008.
[13] R. Featherstone and D. E. Orin, “Dynamics,” in Springer Handbook of
Robotics, 2nd Ed, B. Siciliano and O. Khatib, Eds., 2016.
[14] A. Bloch, Nonholonomic Mechanics and Control, with the collaboration
of J. Ballieul, P. Crouch, and J.Marsden.
Springer-Verlag, 2003.
[15] J. E. Marsden and T. Ratiu, Introduction to mechanics and symmetry: a
basic exposition of classical mechanical systems.
Springer Science &
Business Media, 2013, vol. 17.
[16] R. M. Murray, Z. Li, S. S. Sastry, and S. S. Sastry, A mathematical
introduction to robotic manipulation.
CRC press, 1994.
[17] F. C. Park, J. Bobrow, and S. R. Ploen, “A Lie group formulation of
robot dynamics,” International Journal of Robotic Research, vol. 14,
no. 6, pp. 609–618, 1995.
[18] S.
Traversaro
and
A.
Saccon,
“Multibody
Dynamics
Notation,”
Technische
Universiteit
Eindhoven,
Tech.
Rep.,
2016.
[Online].
Available: http://repository.tue.nl/849895
[19] T. De Laet, S. Bellens, R. Smits, E. Aertbeli¨en, H. Bruyninckx, and
J. De Schutter, “Geometric relations between rigid bodies (part 1):
Semantics for standardization,” Robotics & Automation Magazine, IEEE,
vol. 20, no. 1, pp. 84–93, 2013.
[20] H. Bruyninckx and J. De Schutter, “Symbolic differentiation of the
velocity mapping for a serial kinematic chain,” Mechanism and machine
theory, vol. 31, no. 2, pp. 135–148, 1996.
[21] A. Jain, Robot and Multibody Dynamics: Analysis and Algorithms.
Springer, 2010.
[22] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: modelling,
planning and control.
Springer Science & Business Media, 2010.
[23] S. Chiaverini, G. Oriolo, and I. D. Walker, “Kinematically redundant
manipulators,” in Springer handbook of robotics.
Springer, 2008, pp.
245–268.
[24] P.-B. Wieber, R. Tedrake, and S. Kuindersma, “Modeling and control of
legged robots,” in Springer Handbook of Robotics, 2nd Ed, B. Siciliano
and O. Khatib, Eds., 2016.
[25] J. E. Marsden and J. Scheurle, “The reduced euler-lagrange equations,”
Fields Institute Comm, vol. 1, pp. 139–164, 1993.
[26] K. R. Ball, D. V. Zenkov, and A. M. Bloch, “Variational structures for
hamel’s equations and stabilization,” IFAC Workshop on Lagrangian and
Hamiltonian Methods for Non Linear Control, pp. 178–183, 2012.
[27] D. E. Orin and A. Goswami, “Centroidal momentum matrix of a hu-
manoid robot: Structure and properties,” in 2008 IEEE/RSJ International
Conference on Intelligent Robots and Systems.
IEEE, 2008.
[28] X. Ding and H. Chen, “Dynamic modeling and locomotion control
for quadruped robots based on center of inertia on SE(3),” Journal of
Dynamic Systems, Measurement, and Control, vol. 138, no. 1, 2016.
[29] P. Wieber, “Holonomy and nonholonomy in the dynamics of articulated
motion,” in Fast Motions in Biomechanics and Robotics. Optimization
and Feedback Control, M. Diehl and K. Mombaur, Eds. Springer, 2005.
[30] S. Kobayashi and K. Nomizu, Foundations of differential geometry (Vol.
I), 2nd ed.
Wiley, 1996.
[31] A. Saccon, J. Hauser, and A. P. Aguiar, “Optimal control on lie groups:
The projection operator approach,” IEEE Transactions on Automatic
Control, vol. 58, no. 9, pp. 2230–2245, 2013.
[32] J. Ostrowski and J. Burdick, “The geometric mechanics of undulatory
robotic locomotion,” The International Journal of Robotics Research,
vol. 17, pp. 683–701, 1998.
[33] J. W. Grizzle, G. Abba, and F. Plestan, “Asymptotically stable walking
for biped robots: analysis via systems with impulse effects,” IEEE
Transactions on Automatic Control, vol. 46, no. 1, pp. 51–64, 2001.
[34] J. W. Grizzle, C. Chevallereau, R. W. Sinnet, and A. D. Ames, “Models,
feedback control, and open problems of 3D bipedal robotic walking,”
Automatica, vol. 50, no. 8, pp. 1955 – 1988, 2014.