On Centroidal Dynamics and Integrability of Average Angular Velocity

TL;DR

This paper investigates when the average angular velocity in robotic systems defines an orientation frame solely based on current configuration, linking it to the flatness of the mechanical connection and providing accessible algebraic conditions.

Contribution

It offers a simple algebraic condition to determine when average angular velocity defines a configuration-dependent orientation frame, bridging geometric mechanics and multibody dynamics.

Findings

Provides an algebraic condition for the orientation frame dependence on configuration.

Reinterprets and proves the condition accessible to non-geometric mechanics readers.

Links the concept to the flatness of the mechanical connection in robotic systems.

Abstract

In the literature on robotics and multibody dynamics, 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 framethat 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 corresponds 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 dynamics 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.

Equations

$${}^D{J}_{A,C/B}$$

$${}^A{\mathrm{v}}_{A,B}$$

$${}^B{\mathrm{v}}_{A,B}$$

$${}_L{\mathbb{M}}_L^L{\dot{\mathrm{v}}}_{A,L}{+}^L{\mathrm{v}}_{A,L}{\bar{\times }}_L^{\ast }{\mathbb{M}}_L^L{\mathrm{v}}_{A,L}$$

$$M(q)\dot{\nu }+C(q,\nu )\nu +G(q)$$

$$l(H,\mathrm{s},\mathrm{v},\dot{\mathrm{s}})=\frac{1}{2}{\left[\begin{array}{c}\mathrm{v}\\ \dot{\mathrm{s}}\end{array}\right]}^T\left[\begin{array}{cc}\mathbb{L}(\mathrm{s})& \mathbb{A}(\mathrm{s})\\ {\mathbb{A}}^T(\mathrm{s})& \mathbb{S}(\mathrm{s})\end{array}\right]\left[\begin{array}{c}\mathrm{v}\\ \dot{\mathrm{s}}\end{array}\right]$$

$$\frac{d}{dt}\frac{\partial l}{\partial \mathrm{v}}+\mathrm{v}{\bar{\times }}^{\ast }\frac{\partial l}{\partial \mathrm{v}}$$

$$\frac{d}{dt}\frac{\partial l}{\partial \dot{\mathrm{s}}}-\frac{\partial l}{\partial \mathrm{s}}$$

$$\frac{d}{dt}\frac{\partial l}{\partial \mathrm{v}}+\mathrm{v}{\bar{\times }}^{\ast }\frac{\partial l}{\partial \mathrm{v}}$$

$$\frac{d}{dt}\frac{\partial l}{\partial \dot{\mathrm{s}}}-\frac{\partial l}{\partial \mathrm{s}}$$

$${}^i\mathrm{v}$$

$${}_A\mathcal{J}$$

$${\frac{d}{dt}}_A\mathcal{J}$$

$${}_G\mathcal{J}(H,\mathrm{s},\mathrm{v},\dot{\mathrm{v}})$$

$${}_B\mathcal{J}$$

$${}^B{\mathrm{v}}_{loc}:=\mathrm{v}+{\mathbb{L}}^{-1}(s)\mathbb{A}(s)\dot{\mathrm{s}}.$$

$${}^G{\mathrm{v}}_{ave}$$

$${}_A\mathcal{J}$$

$${}^A{\dot{H}}_C{=}^A{\mathrm{v}}_{loc}^{\wedge }{}^A{H}_C$$

$$\mathcal{A}(s):={\mathbb{L}}^{-1}(s)\mathbb{A}(s)$$

$${}^A{H}_C(t)=H(t){}^{B}F(s(t))$$

$${\mathcal{B}}_{ij}$$

$$HF(s)\in \text{SE}(3),$$

$${\left(\frac{d}{dt}(HF)(HF{)}^{-1}\right)}^{\vee }{=}^A{X}_B(\mathrm{v}+\mathcal{A}(s)\dot{\mathrm{s}}).$$

$${\mathcal{A}}_i(s)$$

$$\frac{{\partial }^{2}F}{\partial s_j\partial s_i}$$

$${\left(\frac{\partial {\mathcal{A}}_i}{\partial s_j}\right)}^{\wedge }(s)F(s)+{\mathcal{A}}_i^{\wedge }(s){\mathcal{A}}_j^{\wedge }(s)F(s)$$

$$F(s)$$

$$\frac{d}{d\sigma }{\Delta }_i$$

$${\Delta }_i(s_1,\dots ,s_{i-1},0)={\Delta }_{i-1}(s_1,\dots ,s_{i-1})$$

$${\int }_0^{s_2}\frac{\partial }{\partial s_1}\left({\mathcal{A}}_2^{\wedge }(s_1,t){\Delta }_2(s_1,t)\right)dt+\frac{\partial {\Delta }_2}{\partial s_1}(s_1,0),$$

Full text

In the language of geometric mechanics, this condition corresponds 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 dynamics 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 balance 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 notation 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 $\operatorname{Ad}$, $\times$ to $\operatorname{ad}$, and $~{}\bar{\times}^{*}$ to $-\operatorname{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 integration 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 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}\in\mathbb{R}^{3}$, $w^{\wedge}$ (read $w$ hat) is the $3\times 3$ skew-symmetric matrix associated with the cross product $\times$ in $\mathbb{R}^{3}$, so that $w^{\wedge}x=w\times x$. Given the skew-symmetric matrix $W=w^{\wedge}$, $W^{\vee}\in\mathbb{R}^{3}$ (read $W$ vee) denotes the inverse transformation. The set of rotational matrices is denoted $\text{SO}(3)$, the set of rigid transformations $\text{SE}(3)$. An element of $\text{SE}(3)$ has the structure $[R,~{}o~{};~{}0_{1\times 3},1]\in\mathbb{R}^{4\times 4}$, with $R\in\text{SO}(3)$, $o\in\mathbb{R}^{3}$, where ; denotes row concatenation.

II-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 $o_{F}$ its origin and with $[F]$ its orientation frame, writing $F=(o_{F},[F])$.

[TABLE]

II-B Coordinates vectors and transformation matrices notation

[TABLE]

[TABLE]

[TABLE]

In the expression for ${}_{B}\mathbb{M}_{B}$ (where $B$ is typically a body fixed frame), $m$ is the body mass, ${}^{B}c$ the CoM coordinates, and ${}_{B}\mathbb{I}_{B}$ the rotational inertia w.r.t. $B$. We use

[TABLE]

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 ${}^{D}{\rm v}_{A,C}=^{D}J_{A,C/B}(q)\,\nu$ with $\nu=(^{B}{\rm v}_{A,B},\dot{q}_{J})$.

II-C Frame velocity representation

The velocity of $B$ w.r.t. $A$ is given by ${}^{A}\dot{H}_{B}$, however, it is more common to express it as a one the following six-dimensional vectors,

[TABLE]

where ${}^{A}\omega_{A,B}:=(^{A}\dot{R}_{B}^{A}R_{B}^{\top})^{\vee}$. We refer to ${}^{A}{\rm v}_{A,B}$, ${}^{B}{\rm v}_{A,B}$, and ${}^{B[A]}{\rm v}_{A,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].

II-D Single Rigid Body Dynamics

Given a rigid body whose position in space is determined by ${}^{A}H_{L}$ with $L$ fixed to the body the classical Newton-Euler equations are written, in a combined form, as

[TABLE]

with ${}_{L}{\rm f}$ denoting the external wrench (combined force and torque vector) expressed w.r.t. $L$ and $~{}\bar{\times}^{*}$ denotes the dual 6D cross product (${}^{L}{\rm v}_{A,L}~{}\bar{\times}^{*}$ is equivalent to $-\operatorname{ad}^{*}_{{}^{L}{\rm v}_{A,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

III-A Floating systems with gravity and external contact forces

Consider a robotic system whose configuration is given by $q=(H,{\rm s})\in\text{SE}(3)\times\mathbb{R}^{n_{J}}$, where $H=^{A}H_{B}$ denotes the base link’s homogeneous transformation matrix and ${\rm s}$ represents the displacement of the $n_{J}$ internal joints. We will refer to $H$ as the pose and to ${\rm s}$ as the shape of the robot. The velocity of the mechanism is parameterized via $\nu=({\rm v},\dot{\rm s})\in\mathbb{R}^{6}\times\mathbb{R}^{n_{J}}$ with ${\rm v}=^{B}{\rm v}_{A,B}$ denoting the velocity of the base frame with respect the inertial frame expressed in the base frame (${}^{B}{\rm v}_{A,B}$ is a left trivialized velocity, cf. the notation section).

The dynamics of a floating articulated robotic system such as, e.g., a humanoid robot [24] is usually written as

[TABLE]

where $M$, $C$, and $G$ are, respectively, the mass matrix, Coriolis matrix, and potential force vector, $\tau$ is the internal joint torques, and ${}^{i}J$ and ${}_{i}{\rm f}$ are the $i$-th contact Jacobian and contact force, both expressed with respect to a contact frame $C_{i}$, 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,\dot{H},\dot{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

[TABLE]

where ${\rm v}=^{B}{\rm v}_{A,B}$ satisfies $\dot{H}=H{\rm v}^{\wedge}$ and $\mathbb{L}$, $\mathbb{A}$, and $\mathbb{S}$ are suitable partitions of the overall mass matrix $M(q)$ appearing in (2) in accordance with the dimension of ${\rm v}$ and $\dot{s}$. The matrix $\mathbb{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 $\mathbb{L}$ as ${}_{B}\mathbb{L}_{B}$ and $\mathbb{A}$ as ${}_{B}\mathbb{A}$ to indicate the output (and, for $\mathbb{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

[TABLE]

Hamel equations are a combination of standard Euler-Lagrange equations (5) and Newton-Euler equations (4), the latter also called Euler-Poincaré 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

[TABLE]

where $\tau$ and ${}_{i}{\rm f}$ are as in (2), $H^{-1}{\partial l}/{\partial H}$ the vector representation of the linear map $w\mapsto D_{1}l(H,{\rm s},{\rm v},\dot{\rm s})\cdot Hw^{\wedge}$, $w\in\mathbb{R}^{6}$, and ${}^{i}X$ and ${}^{i}S$ define, respectively, the pose and shape parts of the $i$-th contact Jacobian ${}^{i}J$. More precisely, the (mixed) velocity of the $i$-th contact point satisfies

[TABLE]

with ${}^{i}{\rm v}:=^{C_{i}[A]}{\rm v}_{A,C_{i}}$ and ${}^{i}J:=^{C_{i}[A]}J_{A,C_{i}/B}$ the $i$-th contact Jacobian (we refer to Section II for a clarification on the notation ${}^{D}J_{A,C/B}$). Note that, by definition, ${}^{i}X=^{C_{i}[A]}X_{B}$, implying ${}^{i}X^{T}=_{B}X^{C_{i}[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].

III-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

[TABLE]

is a constant of motion for the unforced system. In (9), $\mathbb{L}$ and $\mathbb{A}$ are as in (3) and ${}_{A}X^{B}$ 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 ${}_{A}\mathcal{J}$ 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, ${}_{A}\mathcal{J}$ evolves according to

[TABLE]

This result can be derived directly from a straightforward modification 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$.

III-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:=(p_{com},[A])$, that has as origin the combined CoM $p_{com}$ 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

[TABLE]

We refer to ${}_{G}\mathcal{J}$ 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 ${}_{A}\mathcal{J}$ given in (9) is constant and because $(_{G}{\dot{X}}^{A})_{A}\mathcal{J}$ 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 ${}^{A}p_{com}\times m\,^{A}{\dot{p}}_{com}$. The only difference between ${}_{G}\mathcal{J}$ and ${}_{A}\mathcal{J}$ is indeed that ${}_{A}\mathcal{J}$ contains within its angular part (i.e., its last three elements) also the angular momentum of the CoM. Then $(_{G}{\dot{X}}^{A})_{A}\mathcal{J}\equiv 0$ as ${}^{A}{\dot{p}}_{com}\times m\,^{A}{\dot{p}}_{com}\equiv 0$. This also implies that replacing $A$ with $G$ in (10) is all we need to obtain the evolution of ${}_{G}\mathcal{J}$.

III-D Locked and average velocities

In geometric mechanics, a special role is played by the left-trivialized locked velocity ${}^{B}{\rm v}_{loc}$ defined in such a way that

[TABLE]

or, equivalently,

[TABLE]

The locked velocity is, for each instant of time, the velocity at which the base link should move, while considering the internal 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 ${\rm v}$, ${\rm s}$, and $\dot{\rm s}$.

In the robotic literature, the average velocity is defined as ${}^{G}{\rm v}_{ave}:=^{G}X_{B}^{B}{\rm v}_{loc}$ [9]. Note that we can obtain ${}^{G}{\rm v}_{ave}$ from

[TABLE]

with ${{}_{G}\mathbb{L}_{G}}:=_{G}X^{B}{{}_{B}\mathbb{L}_{B}}^{B}X_{G}$ block diagonal. At this point is key to observe that ${}^{G}{\rm v}_{ave}$ depends also on $H$, other than ${\rm v}$, ${\rm s}$, and $\dot{\rm s}$ while ${}^{B}{\rm v}_{loc}$ 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é 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)$.

IV-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

[TABLE]

Posing ${}_{A}\mathbb{L}_{A}:=_{A}X^{B}_{B}\mathbb{L}_{B}^{B}X_{A}$ and ${}^{A}{\rm v}_{loc}:=^{A}X_{B}^{B}{\rm v}_{loc}$, the equation above simply becomes

[TABLE]

For a given initial configuration ${}^{A}H_{C}(t_{0})\in\text{SE}(3)$ at a given time $t=t_{0}$, one can then integrate the differential equation

[TABLE]

to get a frame that has, as right trivialized velocity, the locked velocity ${}^{A}{\rm v}_{loc}$. A key remark that keeps appearing in the robotic literature [27, 9, 28] is that the solution ${}^{A}H_{C}$ of (17) is not guaranteed to depend only on the configuration $(H,s)$. That is, when ${}^{A}H_{C}$ satisfies ${}^{A}H_{C}(t)=^{A}F(H(t),s(t))$ for a suitable function ${}^{A}F:\text{SE}(3)\times\mathbb{R}^{n_{J}}\rightarrow\text{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 $\mathbb{L}^{-1}\mathbb{A}$ are partial derivatives of a nonlinear function of the joint displacements.

Note that the initial condition for $C$ at $t=t_{0}$ is completely arbitrary and therefore the function ${}^{A}F(H,s)$, where it exists, is determined up to an arbitrary right multiplication by an element 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 ${}^{A}F$ exists, it must be of the form ${}^{A}F(H,s)=H\,^{B}F(s)$ with ${}^{B}F(s):\mathbb{R}^{n_{J}}\rightarrow\text{SE}(3)$. The dependence of the centroidal frame only on the configuration is therefore equivalent to the existence of this $\text{SE}(3)$-valued function of the internal joints. Tipically, the frame $C$ at $t=t_{0}$ 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 ${}^{A}H_{C}(t)=^{A}F(H(t),s(t))$, the CoM $p_{com}$ has always constant coordinates with respect to a frame $C$ that evolves according to (17). In formulas, ${}^{C}\dot{p}_{com}=d/dt\,^{C}p_{com}\equiv 0$. A proof of this fact is given in the Appendix -B. Therefore, in case indeed we can find a function ${}^{A}F$ such that ${}^{A}H_{C}(t)=^{A}F(H(t),s(t))$, it then seems natural to choose the frame $C$ such that $p_{com}$ is its origin. $\blacksquare$

IV-B The main result

Define

[TABLE]

with $\mathbb{L}(s)$ and $\mathbb{A}(s)$ expressed with respect to $B$ as in (3). Then, the following holds.

Proposition IV.1

The centroidal frame satisfying (17) is integrable, that is, there exist a (differentiable) function ${}^{B}F:\mathbb{R}^{n_{J}}\rightarrow\text{SE}(3)$ such that

[TABLE]

if and only if

[TABLE]

for every $i,j\in\{1,2,\dots,n_{J}\}$, where $\mathcal{A}_{i}$, $\mathcal{A}_{j}$ are the i-th and $j$-th columns of $\mathcal{A}$ and $\times$ the vector cross product in $\mathbb{R}^{6}$. $\square$

Proof:

The result is, roughly speaking, a generalization to $\text{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 function $F:\mathbb{R}^{n_{J}}\rightarrow\text{SE}(3)$ the right trivialized velocity associated to the homogeneous transformation matrix

[TABLE]

with $H=H(t)\in\text{SE}(3)$ and ${\rm s}={\rm s}(t)\in\mathbb{R}^{n_{J}}$ sufficiently smooth curves is equal to

[TABLE]

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 $\mathcal{A}(s)$ above satisfy, for $i\in\{1,2,\dots,n_{J}\}$,

[TABLE]

that is, $\mathcal{A}_{i}$ 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:\mathbb{R}^{n_{J}}\rightarrow\mathbb{R}^{4\times 4}$ one knows that its second derivative must satisfy

[TABLE]

for every $i$ and $j$. Writing $\partial F(s)/\partial s_{i}=\mathcal{A}^{\wedge}_{i}(s)F(s)$ using (23), one can write the left hand side of (24) as

[TABLE]

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 $\mathcal{A}^{\wedge}_{i}\mathcal{A}^{\wedge}_{j}-\mathcal{A}^{\wedge}_{j}\mathcal{A}^{\wedge}_{i}=(\mathcal{A}_{i}\times\mathcal{A}_{j})^{\wedge}$. This proves the necessity of the condition. Sufficiency is provided constructively, using

[TABLE]

where $\Delta_{n_{J}}$ is given below and showing that $F$ in (26) satisfies (23) as long as (20) holds. In (26), $\Delta_{n_{J}}$ and the functions $\Delta_{i}:\mathbb{R}^{i}\rightarrow\text{SE}(3)$, $i\in\{1,2,\dots,n_{J}\}$ are defined recursively for a given value $s=(s_{1},\dots,s_{n_{J}})$ as the solution at $\sigma=s_{i}$ of the matrix differential equation

[TABLE]

with $\Delta_{i}$ evaluated at $(s_{1},\dots,s_{i-1},\sigma)\in\mathbb{R}^{i}$ with initial condition

[TABLE]

and $\Delta_{1}(0)=F(0)\in\text{SE}(3)$ arbitrary (the desired orientation of the centroidal frame for $s=0$). Note that $d\Delta_{i}/d\sigma$ in (27) equals $\partial\Delta_{i}(s_{1},\dots,s_{i-1},\sigma)/\partial s_{i}$. In the following, we provide the proof that (23) holds for $n_{J}=2$. Proving (23) for $n_{J}>2$ is a straightforward but tedious calculation that follows from the technique used when $n_{J}=2$. Given $F(s):=\Delta_{2}(s_{1},s_{2})$, it is immediate to see one must have $\partial F/\partial s_{2}(s)=\mathcal{A}^{\wedge}_{2}(s)F(s)$. Showing $\partial F/\partial s_{1}(s)=\mathcal{A}^{\wedge}_{1}(s)F(s)$ is, instead, more involved and requires (20). From (26), (27), and (28), for the case $n_{J}=2$, one can write $\partial F/\partial s_{1}(s)$ as

[TABLE]

that can be further expanded into

[TABLE]

where the terms inside the integral are evaluated at $(s_{1},t)$. Using (20), one gets that the integral above can be written as

[TABLE]

Using again the Jacobi identity and recalling that ${\partial\Delta_{2}}/{\partial t}=\mathcal{A}_{2}^{\wedge}\Delta_{2}$, the above integral can be rewritten as

[TABLE]

where all terms are evaluated at $(s_{1},t)$. By assuming ${\partial F}/{\partial s_{1}}(s)={\partial\Delta_{2}}/{\partial s_{1}}(s)=\mathcal{A}_{1}^{\wedge}(s)\Delta_{2}(s)$, the second term in the integral of (32) vanishes and allowing one to rewrite (30) as

[TABLE]

which equals $\mathcal{A}_{1}^{\wedge}(s)\Delta_{2}(s)=\mathcal{A}_{1}^{\wedge}(s)F(s)$, with no contradiction on the assumption.

∎

Remark. When the underlying Lie group is $\text{SE}(3)$, the expression given in (20) is equivalent to the curvature of a principal 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

[TABLE]

for $d$, $\alpha$, $\beta\in\{1,\dots,n_{J}\}$ with $\mathcal{A}^{i}_{j}$ denoting the entry $(i,j)$ of $\mathcal{A}$ and $C^{a}_{bc}$ the structure constants of the Lie group, representing the Lie bracket operation that, for $\text{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 IV.1 could be obtained by showing that when (34) is identically zero (flatness of the connection) this implies the existence of a function ${}^{B}F$ 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 $\text{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 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,

[TABLE]

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.

IV-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 ${}^{A}o_{C}=^{A}p_{com}$, the centroidal kinematic is simply given by

[TABLE]

with ${}^{A}\omega_{loc}$ given by the angular velocity component of ${}^{A}{\rm v}_{loc}$. We first show that, independently from where ${}^{A}o_{C}$ is located, ${}^{G}{\rm v}_{ave}$ and ${}^{A}{\rm v}_{loc}$ satisfy

[TABLE]

This shows that the average angular velocity ${}^{G}\omega_{ave}$ coincides with the locked angular velocity ${}^{A}\omega_{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

[TABLE]

assume there is a point $p$ such that its coordinates ${}^{C}p$ 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 ${}^{A}{\rm v}$ written with respect to $G$ equals

[TABLE]

where ${}^{A}\omega$ denotes the angular velocity component of ${}^{A}{\rm v}$. $\blacksquare$

Finally, to obtain (36)-(37), we employ (38) to express ${}^{A}{\rm v}_{loc}$ in terms of ${}^{G}{\rm v}_{ave}$ and substitute it into (17), obtaining

[TABLE]

where we recall ${}^{A}H_{C}=(^{A}R_{C},^{A}o_{C};0_{1\times 3},1)\in\text{SE}(3)$. As we have selected ${}^{A}o_{C}=^{A}p_{com}$, the result follows.

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 ${\textrm{m}}^{2}$. For distal links, the inertia is $1$ kg ${\textrm{m}}^{2}$. 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$, $\mathcal{B}_{12}=-\mathcal{B}_{21}$ is equal, up to a division by the factor ${\left(2\,C_{1-2}+6\,S_{1}-6\,S_{2}-28\right)}^{2}$, to

[TABLE]

where $S_{1}:=\sin(s_{1})$, $S_{2}:=\sin(s_{2})$, $C_{1}:=\cos(s_{1})$, $C_{2}:=\cos(s_{2})$, $S_{1-2}:=\sin(s_{1}-s_{2})$, and $C_{1-2}:=\cos(s_{1}-s_{2})$. For $d=0$, $\mathcal{B}_{12}=\mathcal{B}_{21}\equiv 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 displacements. This is confirmed by the animation snapshots given in Figure 2 corresponding to two simulations for different 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

[TABLE]

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 algebraic 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 $\mathbb{L}$ and $\mathbb{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.

-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 ${}_{A}J$ given by (9) is constant. In the context of geometric mechanics, (9) is the momentum map $\mathcal{J}$ defined as

[TABLE]

where $\mathbb{F}L(q,\dot{q})\cdot z:=\lim_{t\rightarrow 0}(L(q,\dot{q}+tz)-L(q,\dot{q}))/t$ denotes the fiber derivative of the Lagrangian in the direction $z\in T_{q}Q$ and $\xi_{Q}(q)$ the infinitesimal generator of the group action formally defined as $\xi_{Q}(q)=d/dt|_{t=0}\Phi_{\exp(t\xi)}(q)$. The function $\Phi_{g}$ is the action of the symmetry group on the configuration space: in the context of this paper, $g\in\text{SE}(3)$, the configuration space $Q=SE(3)\times\mathbb{R}^{n_{J}}$, and the group action $\Phi_{g}:Q\rightarrow Q$ is simply given by

[TABLE]

corresponding to a rigid transformation of the entire robot according to $g$ that leaves invariant the shape ${\rm s}$. The infinitesimal generator associated to (44) is therefore

[TABLE]

and after straightforward computations one gets that the momentum map equals (9) as the right hand side of (43) is

[TABLE]

For the reader that is familiar with Lie group theory, note that, in (46), ${}^{B}X_{A}=\operatorname{Ad}_{H^{-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 ${}^{A}H_{C}$ 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

[TABLE]

where ${}^{A}{\bar{p}}_{com}$ are the homogeneous coordinates of $p_{com}$ with respect to $A$ obtained by appending $1$ to the standard coordinates ${}^{A}p_{com}$, i.e., ${}^{A}{\bar{p}}_{com}:=(^{A}p_{com};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}{\bar{p}}_{com}=^{A}H_{C}^{C}{\bar{p}}_{com}$ assuming ${}^{C}p_{com}$ to be a constant. The right hand side of (47) can be expressed with respect to frame the frame $G=(p_{com},[A])$ obtaining the equivalent condition

[TABLE]

where ${}^{G}\bar{p}_{com}=(0,0,0,1)^{T}$. Equation (48) is then equivalent to ${}^{A}{\dot{p}}_{com}=^{A}R_{G}^{G}v_{loc}$ and to ${}^{A}{\dot{p}}_{com}=^{G}v_{loc}$, since ${}^{A}R_{G}=I$. This last condition is always true deriving directly from the fact that the momentum map $\mathcal{J}$ expressed in the $G$ frame is given by

[TABLE]

where ${}_{G}\mathbb{L}_{G}$ is block diagonal with first block on the diagonal equal to $mI_{3\times 3}$ with $m$ the total mass and that the linear momentum component of ${}_{G}\mathcal{J}$ is necessarily $m^{A}{\dot{p}}_{com}$.