On Centroidal Dynamics and Integrability of Average Angular Velocity

0.46 MB·

by  Alessandro Saccon

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- different research communities with a different theoretical
namics, the concept of average angular velocity has received background and research focus.
considerable attention in recent years. We address the question
Paper’s contributions: (1) Equation of motions for a free-
of whether the average angular velocity defines an orientation
7 frame that depends only on the current robot configuration and floating robot written employing a robot-specific nota-
1 provideasimplealgebraicconditiontocheckwhetherthisholds. tion consistent with differential geometry notation: this
0 In the language of geometric mechanics, this condition cor- paper presents the dynamics of a simply supported articulated
2 responds to requiring the flatness of the mechanical connection rigid-body system subject to external forcing by employing
associatedtotheroboticsystem.Here,however,weprovideboth
n a notation which is inspired by the spatial vector algebra
areinterpretationandaproofofthisresultaccessibletoreaders
a notation [12], [13] while allowing for a one-to-one mapping
J with a background in rigid body kinematics and multibody dy-
namicsbutnotnecessarilyacquaintedwithdifferentialgeometry, withtheconceptsusedingeometricmechanicsanddifferential
0
stillprovidingpreciselinkstothegeometricmechanicsliterature. geometry related to the theory of differentiable manifolds and
1
Thisshouldhelpspreadingthealgebraicconditionbeyondthe Lie groups [14], [15] (e.g., X corresponds to Ad, × to ad,

] scopeofgeometricmechanics,contributingtoaproperutilization and ׯ∗ to −ad∗, see next section for details). While the
O and understanding of the concept of average angular velocity.
employment of Lie group formalism is robotics is certainly
R not new (see, e.g., the excellent publications [16] and [17]),
. I. INTRODUCTION we felt that an explicit parallelism between spatial vector
s
c Thetotalmomentumoffloatingarticulatedroboticsystems, algebra and Lie group notations was still missing. This holds
[ suchasaerialmanipulatorsandhumanoidrobots,hasreceived true, in particular, for the free-floating dynamics case treated

1 considerable attention in the robotic literature. There is a here, required for assessing the integrability of the average
v growing consensus that the dynamics of total momentum can angularvelocity;(2)differencebetweenthetotalmomentum
4 be used as a reduced but still exact model of the original Noether’s theorem and total momentum as commonly
1
systemthatcanease,e.g.,thedevelopmentofpostureandbal- encounter in robotics:wehighlighthowthetotalmomentum
5
ance controllers as well as planning algorithms for humanoid considered in the robotic literature [9] actually differs from
2
0 robots [1], [2], [3], [4], [5], [6], [7], [8]. The total momentum the total momentum (the momentum map) that derives from
. is defined as the sum of all linear and angular momenta the application of geometric mechanics version of Noether’s
1
of the (rigid) bodies composing the articulated system. The theorem.Asaconsequence,theaveragevelocityintherobotic
0
7 momentum is typically computed with respect to a frame literature and locked velocity in the geometric mechanics
1 which orientation is that of the inertial frame and origin is literature represent the same velocity, although expressed
: the total center of mass [9]. Its time evolution depends only with respect two different reference frames. This apparently
v
i on the external forces and torques acting on the system, such unessential detail plays however a key role (see discussion in
X
as gravity and contact forces. The total angular momentum Section III-D) in understanding the main result of this paper.
r can be split into a linear and an angular component. The In highlighting the difference between the two velocities, the
a
linearcomponent,whendividedbythetotalmass,capturesthe angular momentum of the center of mass, an extra component
average linear velocity of the mechanism, i.e, the velocity of oftheangularpartofthetotalmomentumonceexpressedwith
thecenterofmass(CoM).Althoughstilldebatingaboutwhich respect to the inertial frame, receives particular attention; (3)
value it should be regulated to, the angular component has Integrability condition for the average angular velocity:
beenusedtodefineaconceptof“averageangularvelocity”of We use the results and insights of the previous two points
theentiremechanism.Theconceptofaverageangularvelocity to show that the fundamental question “when does the inte-
isdiscussedin[9]anditcorresponds,roughlyspeaking,tothe gration of the average angular velocity define an orientation
angular velocity of entire mechanism, for the given pose but frame that depends only on the current internal joint position,
assuming the internal joints to be fixed, corresponding to the independentlyoftheirtimeevolution?”isequivalenttoasking
current value of the angular momentum. if a series of vector valued functions can be interpreted
The geometric mechanics community has been employing as the (right trivialized) partial derivatives of a nonlinear
a concept strictly related to the average velocity, the locked function of the internal joints. While pointing out that this
velocity,foratleasttwodecades[10,Section3.3][11,Section is equivalent to requiring, in terms of geometric mechanics,
5.3], but an explicit link between the two concepts appears to thattheassociatedmechanicalconnectionisflat(see,e.g.,the
bemissing.Weaimatprovidingthislinkinthispaper,hoping discussion on holonomy in [14, Section 3.14] and reference
that this will help the communication of key results among there in), we provide a reinterpretation of the result, and the
2
(cid:104) (cid:105)
acsosnonceicattieodn,atlhgaetbcraanicbecoenadsiitliyonfolelonwsuerdinbgytahereafldaetrneascsquoafintthede BMB = −mm13B×c3∧ mBBIBc∧ Gw.ern.te.rfarlaimzeedB inertia matrix
withmultibodydynamics,kinematicsofrigidtransformations, In the expression for M (where B is typically a body
B B
undergraduate calculus, and the understanding of Schwartz’s fixed frame), m is the body mass, Bc the CoM coordinates,
theorem (symmetry of second derivatives), but with no or and I the rotational inertia w.r.t. B. We use
B B
limited experience with differential geometry. Our hope is
to contribute to the diffusion and utilization of this algebraic DJA,C/B
condition beyond the scope of geometric mechanics.
to indicate the Jacobian relating the velocity of frame C with
Rigid body notation is reviewed in Section II. Section III
respect to A expressed in D with the velocity of the base
reviews the dynamics of floating mechanical systems and the
link expressed in B, so that Dv = DJ (q)ν with
A,C A,C/B
evolution of the total momentum. In Section IV, we review
ν =(Bv ,q˙ ).
A,B J
the concepts of average and locked velocities, centroidal
frame, and provide the algebraic condition to show when it
C. Frame velocity representation
dependsonlyofthecurrentposeandshapeofthemechanism.
Conclusions and a discussion are provided in Section VI. The velocity of B w.r.t. A is given by AH˙ , however, it
B
is more common to express it as a one the following six-
II. NOTATION dimensional vectors,
This section introduces basic notation used for dynamics (cid:20)Ao˙ −Aω∧ Ao (cid:21)
Av = B A,B B ,
computations. We refer to [18] for details. A,B Aω
A,B
Given a vector w = (x,y,z)T ∈ R3, w∧ (read w hat) is (cid:20) BR Ao˙ (cid:21) (cid:20) Ao˙ (cid:21)
the 3 × 3 skew-symmetric matrix associated with the cross BvA,B = BR AAω B , B[A]vA,B = Aω B ,
product × in R3, so that w∧x = w × x. Given the skew- A A,B A,B
symmetric matrix W =w∧, W∨ ∈R3 (read W vee) denotes where Aω := (AR˙ AR(cid:62))∨. We refer to Av , Bv ,
A,B B B A,B A,B
the inverse transformation. The set of rotational matrices is and B[A]v as, respectively, the right-trivialized, the left-
A,B
denoted SO(3), the set of rigid transformations SE(3). An trivialized, and the mixed velocity of B w.r.t. A. The mixed
element of SE(3) has the structure [R, o ; 0 ,1] ∈ R4×4, representation is also known as hybrid representation [20].
1×3
with R∈SO(3), o∈R3, where ; denotes row concatenation. Theleft-andright-trivializedrepresentationsarewidespread
in the literature of Lie group-based geometric mechanics
[16] (where they are called spatial and body velocities) and
A. Frames notation
recursive robot dynamics algorithms [12], [21] (where they
A frame is defined by a point, called origin, and an
arecalledspatialvelocities).Themixedvelocityiscommonly
orientation frame [19]. We use capital letters to indicate
used in multi-task control frameworks [22], [23], [2].
frames. Given a frame F, we denote with o its origin and
F
with [F] its orientation frame, writing F =(o ,[F]).
F
D. Single Rigid Body Dynamics
A, B, ... Reference frames
Given a rigid body whose position in space is determined
p An arbitrary point by AH with L fixed to the body the classical Newton-Euler
L
B[A] Frame with origin o and orientation [A]
B equations are written, in a combined form, as
M Lv˙ +Lv ׯ∗ M Lv = f (1)
B. Coordinates vectors and transformation matrices notation L L A,L A,L L L A,L L
Ap∈R3 Coordinates of p w.r.t. A with f denoting the external wrench (combined force and
L
AoB ∈R3 Coordinates of oB w.r.t. to A torque vector) expressed w.r.t. L and ׯ∗ denotes the dual
ARB ∈R3×3 Rotation matrix from [B] to 6Dcrossproduct(LvA,L ׯ∗ isequivalentto−ad∗LvA,L inthe
[A] languageofLiegroups).WeusetheletterLsincearigidbody
(cid:104) (cid:105)
AHB = A01R×B3 Ao1B Rigid transf. from B to A on an articulated mechanism is usually referred to as a link.
ACXvAB,B==(cid:104)(cid:104)A03CCR×ωvB3AA,A,BBo∧B(cid:105)AA∈RRRBB6(cid:105) VVeelloocciittyy troafnsfB. frowm.r.Bt. ttooAA A. Floating systIeImI.sRwIitGhIDg-rBavOiDtyYaDnYdNeAxtMerInCaSl contact forces
expressed in C
Consider a robotic system whose configuration is given by
(cid:20) (cid:21)
BvA,B×= B0ω3A∧×,3B BBωvA∧A∧,,BB Vector cross product in R6 qlin=k’(sHho,ms)o∈geSnEeo(3u)s×trRannsJfo,rwmhaetrieonHm=atrAixHaBnddsenreoptersestehnetsbathsee
AXB =(cid:104)Ao∧BAARRBB A03R×B3(cid:105) (WrXenBch=trAaXns−f.Tf)rom B to A dthisepplaocseemaenndt otof tsheasntJheinsthearnpaelojofinthtse.rWobeowt.ilTlhreefveerltoociHty oasf
Bf=(cid:104)CCfτAA,,BB(cid:105)∈R6 CwA.oro.tr.dBinates oBf the wrench f twhiethmvec=haBnvisAm,Bisdepnaoratimngettehreizevdelovciaityνo=f th(ve,bs˙a)s∈e fRra6m×e wRintJh
(cid:20) (cid:21) respect the inertial frame expressed in the base frame (Bv
BvA,Bׯ∗ = BBωv∧A∧,B B0ω3∧×3 Dual cross product in R6 is a left trivialized velocity, cf. the notation section). A,B
A,B A,B
3
The dynamics of a floating articulated robotic system such withiv:=Ci[A]vA,Ci andiJ :=Ci[A]JA,Ci/B thei-thcontact
as, e.g., a humanoid robot [24] is usually written as Jacobian (we refer to Section II for a clarification on the
M(q)ν˙ +C(q,ν)ν+G(q)=[0;τ]+(cid:88)(iJ)T if (2) nimotpaltyioinngDiXJAT,C=/BB).XNCoit[eA]t,haat,wbryendcehfintriatinosnf,orimXat=ionC.i[EAx]XceBpt,
i forthenotation,(6)-(7)areequivalenttothemorecommon(2)
whereM,C,andGare,respectively,themassmatrix,Coriolis but they provide extra structure that helps to understand the
matrix, and potential force vector, τ is the internal joint definition and time evolution of the total momentum directly
torques,andiJ and f arethei-thcontactJacobianandcontact from the equations of motion. From a computational point of
i
force, both expressed with respect to a contact frame C , view, forward and inverse dynamics for (2) can be obtained
i
fixed with respect to its corresponding link. To derive (2), using,e.g.,thefloating-baserecursiveNewton-Euleralgorithm
one can take a Newton-Euler approach summing up all the and composite-rigid-body algorithm presented in [12].
contributions of the internal and external forces for each body
using(1)orsettingupaLagrangianL(H,s,H˙,s˙)andemploy
B. The total momentum expressed in the inertial frame
the Euler-Lagrange equations. However, as H is not a vector
The Lagrangian (3) is not a function of H meaning it is
quantity, either one use a local vector parametrization for
invariant with respect to a rigid transformation. As shown in
H (based, e.g., on Euler angles) or employ the tools from
Appendix A, standard results of geometric mechanics imply
geometric mechanics and form the left trivialized Lagrangian
that the quantity
1(cid:20)v(cid:21)T (cid:20) L(s) A(s)(cid:21)(cid:20)v(cid:21)
l(H,s,v,s˙)= (3) J = XB(Lv+As˙) (9)
2 s˙ AT(s) S(s) s˙ A A
is a constant of motion for the unforced system. In (9), L and
where v = BvA,B satisfies H˙ = Hv∧ and L, A, and S are A are as in (3) and XB as in Section II with A denoting the
A
suitable partitions of the overall mass matrix M(q) appearing
inertial frame and B the base link frame.
in (2) in accordance with the dimension of v and s˙. The
Recalling that (3) is obtained by summing up all kinetic
matrix L is typically referred to as the locked inertia tensor
energies of each link dynamics (1) employing link Jacobian
as is corresponds to the (generalized) inertia of the entire
withastructuresimilarto(8),itisstraightforwardtorecognize
mechanism computed with respect to B assuming no motions
in J the total momentum given by the sum of the all the
of its internal joints. To be more precise, one should write L A
linearandangularmomentaofeachrigidbodyexpressedwith
as L and A as A to indicate the output (and, for L, also
B B B respect to the origin of A. When gravity and external forcing
input) frame that these transformations accept. This should
are present, J evolves according to
A
help, e.g., to better interpret the expression for the combined
(cid:32) (cid:33)
linear and angular momentum given by (9), later in the text. d ∂l (cid:88)
J = XB H−1 + (iX)T f . (10)
The dynamics of the articulated mechanism can be then dtA A ∂H i
i
derived using Hamel equations (see, e.g., [25], [15, Section
13.6], [26]), namely Thisresultcanbederiveddirectlyfromastraightforwardmod-
ification of Noether’s theorem (for a proof of the geometric
d ∂l ∂l
+v ׯ∗ =0 (4) versionofNoether’stheorem,see[14,Chapter3]).Onecareful
dt∂v ∂v inspection of the above formula shows, however, that (10) is
d ∂l ∂l
− =0. (5) actuallyequivalentto(6),onlywrittenintheinertialframeA.
dt∂s˙ ∂s
Hamel equations are a combination of standard Euler-
C. The total momentum expressed at the center of mass
Lagrange equations (5) and Newton-Euler equations (4), the
The momentum of the system can be expressed also
latter also called Euler-Poincare´ equations for a generic Lie
with respect to other frames. In particular, the frame G :=
group [15, Section 13.5]. In the presence of internal and
(p ,[A]),thathasasoriginthecombinedCoMp andthe
external forces and the presence of potential energy due to, com com
orientation of the inertial frame A, is commonly found in the
e.g., the effect of gravity, (4)-(5) become
robotic literature [27], [9]. With respect to G, the momentum
d ∂l +v ׯ∗∂l =H−1 ∂l +(cid:88)(iX)T f (6) is given by
dt∂v ∂v ∂H i
i J(H,s,v,v˙):= XA(H,s) J(H,s,v,v˙). (11)
G G A
d ∂l ∂l (cid:88)
− =τ + (iS)T f, (7)
dt∂s˙ ∂s i We refer to GJ as the centroidal momentum (in accordance
i with, e.g., [9]). Remarkably, when no external forces and
where τ and f are as in (2), H−1∂l/∂H the vector represen- potential are present, this quantity is also constant as J
i A
tation of the linear map w (cid:55)→D l(H,s,v,s˙)·Hw∧, w ∈R6, given in (9) is constant and because ( X˙A) J is always a
1 G A
and iX and iS define, respectively, the pose and shape parts zero. This last fact is related to the angular momentum of the
of the i-th contact Jacobian iJ. More precisely, the (mixed) CoM.Denotingwithmthetotalmass,theangularmomentum
velocity of the i-th contact point satisfies of the CoM is simply given by Ap ×mAp˙ . The only
com com
difference between J and J is indeed that J contains
iv=iJν =iXv+iSs˙ (8) G A A
within its angular part (i.e., its last three elements) also the
4
angular momentum of the CoM. Then ( X˙A) J ≡ 0 as in the robotic literature [27], [9], [28] is that the solution
G A
Ap˙ ×mAp˙ ≡ 0. This also implies that replacing A AH of (17) is not guaranteed to depend only on the con-
com com C
with G in (10) is all we need to obtain the evolution of J. figuration (H,s). That is, when AH satisfies AH (t) =
G C C
AF(H(t),s(t)) for a suitable function AF :SE(3)×RnJ →
D. Locked and average velocities SE(3). It is well known that this does not always happen
as in the simulation results presented in [29] and references
Ingeometricmechanics,aspecialroleisplayedbytheleft-
therein.Whatappeartobelessknownisthatthereisasimple
trivialized locked velocity Bv defined in such a way that
loc condition to check when this happens and that this is related
J =L(s)v+A(s)s˙ =:L(s)Bv (12) to asking if the columns of L−1A are partial derivatives of a
B loc
nonlinear function of the joint displacements.
or, equivalently,
Note that the initial condition for C at t=t is completely
0
Bv :=v+L−1(s)A(s)s˙. (13) arbitraryandthereforethefunctionAF(H,s),whereitexists,
loc
isdetermineduptoanarbitraryrightmultiplicationbyanele-
Thelockedvelocityis,foreachinstantoftime,thevelocityat
mentofSE(3).Furthermore,giventhefactthatthekinematics
which the base link should move, while considering the inter-
(17) is actually invariant to an arbitrary pose transformation,
nal joints as locked, to get the same value of the momentum onegetstheextraconditionthatifAF exists,itmustbeofthe
corresponding to the current velocity of the mechanism. Note form AF(H,s)=HBF(s) with BF(s):RnJ →SE(3). The
how the locked velocity is expressed with respect to the base
dependence of the centroidal frame only on the configuration
frame, so it is only a function of v, s, and s˙.
is therefore equivalent to the existence of this SE(3)-valued
In the robotic literature, the average velocity is defined as
functionoftheinternaljoints.Tipically,theframeC att=t
0
Gv :=GX Bv [9].NotethatwecanobtainGv from
ave B loc ave is taken to have its origin coinciding with the total center of
Gv = L−1 J (14) mass because it can be shown (see remark below) that (17)
ave G G G
will maintain the equivalence of the two points: this justifies
with GLG :=GXBBLBBXG block diagonal. At this point is the use of centroidal frame as name for C.
key to observe that Gvave depends also on H, other than v, s, Remark.Independentlyoftheexistenceofaconfiguration-
and s˙ while Bvloc does not. We will show in the next section, dependent-only frame satisfying AH (t) = AF(H(t),s(t)),
C
that the latter is then preferable in answering the integrability the CoM p has always constant coordinates with respect
com
question for the average (angular) velocity. to a frame C that evolves according to (17). In formulas,
As a side note, it is worth recalling at this point that both Cp˙ =d/dtCp ≡0. A proof of this fact is given in the
com com
the locked velocity and average velocity can be used to block
Appendix B. Therefore, in case indeed we can find a function
diagonalize the mass matrix. However, as the kinetic energy AF suchthatAH (t)=AF(H(t),s(t)),itthenseemsnatural
C
is now written with respect to another velocity one must be to choose the frame C such that p is its origin. (cid:4)
com
careful in deriving the equations of motion from it. When
using the locked velocity, Lagrange-Poincare´ equations (see,
e.g., [15, Chapter 13]) can be use to retrive a set of equations B. The main result
equivalent to (4)-(5).
Define
IV. THECENTROIDALFRAMEANDTHEMAINRESULT A(s):=L−1(s)A(s) (18)
In this section, we recall the concept of centroidal frame
with L(s) and A(s) expressed with respect to B as in (3).
and provide the algebraic condition ensuring that it depends
Then, the following holds.
only on the configuration (H,s).
Proposition 4.1: The centroidal frame satisfying (17) is
integrable, that is, there exist a (differentiable) function BF :
A. The centroidal frame and the main question RnJ →SE(3) such that
The definition of the locked velocity given by (13) allows
AH (t)=H(t)BF(s(t)) (19)
one to write the momentum with respect to A simply as C
J = XB L Bv . (15) if and only if
A A B B loc
Posing ALA :=AXBBLBBXA and Avloc :=AXBBvloc, the B := ∂Ai − ∂Aj +A ×A ≡0 (20)
equation above simply becomes ij ∂s ∂s i j
j i
AJ =ALAAvloc. (16) 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. (cid:3)
For a given initial configuration AH (t )∈SE(3) at a given
C 0
time t=t0, one can then integrate the differential equation Proof. The result is, roughly speaking, a generalization to
AH˙ =Av∧ AH (17) SE(3)ofSchwartz’stheorem(symmetryofsecondderivatives)
C loc C and the related theorem stating that closed differential forms
to get a frame that has, as right trivialized velocity, the arelocallyexact(existenceofapotentialfunction).Westartby
locked velocity Av . A key remark that keeps appearing theobservationthatifwetakeanarbitrarysufficientlysmooth
loc
5
function F : RnJ → SE(3) the right trivialized velocity andrequires(20).From(26),(27),and(28),forthecasenJ =
associated to the homogeneous transformation matrix 2, one can write ∂F/∂s (s) as
1
HF(s)∈SE(3), (21) (cid:90) s2 ∂ (A∧(s ,t)∆ (s ,t))dt+ ∂∆2(s ,0), (29)
∂s 2 1 2 1 ∂s 1
with H = H(t) ∈ SE(3) and s = s(t) ∈ RnJ sufficiently 0 1 1
that can be further expanded into
smooth curves is equal to
(cid:18)d (cid:19)∨ (cid:90) s2 ∂A∧2 ∆ +A∧∂∆2dt+A∧(s ,0)∆ (s ,0), (30)
(HF)(HF)−1 =AX (v+A(s)s˙). (22) ∂s 2 2 ∂s 1 1 2 1
dt B 0 1 1
where the terms inside the integral are evaluated at (s ,t).
The above formula has exactly the same structure of the 1
Using (20), one gets that the integral above can be written as
locked velocity given in (13) with the extra interpretation
that the columns of the matrix A(s) above satisfy, for i ∈ (cid:90) s2(cid:18)∂A∧ (cid:19) ∂∆
1 +(A ×A )∧ ∆ +A∧ 2dt (31)
{1,2,...,nJ}, 0 ∂t 1 2 2 2 ∂s1
(cid:18)∂F (cid:19)∨ Using again the Jacobi identity and recalling that ∂∆ /∂t =
Ai(s)= ∂si(s)F(s)−1 ∈R6, (23) A∧2∆2, the above integral can be rewritten as 2
that is, Ai can be interpreted as the right trivialized partial (cid:90) s2 ∂ (A∧∆ )+A∧(cid:18)∂∆2 −A∧∆ (cid:19)dt (32)
derivatives of the nonlinear function F depending on the ∂t 1 2 2 ∂s 1 2
0 1
internal joints. Thinking F as a function F : RnJ → R4×4
where all terms are evaluated at (s ,t). By assuming
one knows that its second derivative must satisfy 1
∂F/∂s (s) = ∂∆ /∂s (s) = A∧(s)∆ (s), the second term
1 2 1 1 2
∂2F ∂2F in the integral of (32) vanishes and allowing one to rewrite
= (24)
∂s ∂s ∂s ∂s (30) as
j i i j
f(o2r3)e,voenreyciananwdrijte. Wtheritlienfgt h∂aFnd(ss)i/d∂esoif=(24A)∧ia(ss)F(s) using (cid:90)0s2 ∂∂t(A∧1(s1,t)∆2(s1,t))dt+A∧1(s1,0)∆2(s1,0) (33)
(cid:18)∂A (cid:19)∧ which equals A∧1(s)∆2(s) = A∧1(s)F(s), with no contradic-
i (s)F(s)+A∧(s)A∧(s)F(s) (25) tion on the assumption.
∂s i j
j (cid:3)
and similarly for the right hand side by exchanging i with
Remark. When the underlying Lie group is SE(3), the
j. Equating the two expressions and multiplying on the right
expressiongivenin(20)isequivalenttothecurvatureofaprin-
by the inverse of F(s), one obtains straightforwardly (20) by
cipalconnection(see[14,Chapter2]andreferencethereinfor
recalling Jacobi identity A∧A∧−A∧A∧ =(A ×A )∧. This
i j j i i j a concise and convenient summary of principal connections).
proves the necessity of the condition. Sufficiency is provided
The curvature of a principal connection is typically written as
constructively, using
∂Ad ∂Ad
F(s)=∆nJ(s1,s2,...,snJ) (26) Bαdβ(s):= ∂sβα − ∂sαβ +CadbAaαAbβ (34)
where∆nJ isgivenbelowandshowingthatF in(26)satisfies for d, α, β ∈ {1,...,nJ} with Aij denoting the entry
(∆23): aRsil→ongSEa(s3)(2,0i)∈ho{l1d,s2.,I.n..(,2n6),}∆arneJdaenfidnetdherefcuunrcsitvioenlys (i,j) of A and Cbac the structure constants of the Lie group,
i J representing the Lie bracket operation that, for SE(3), is the
for a given value s=(s ,...,s ) as the solution at σ =s
1 nJ i 6D vector cross product appearing in (20). Therefore, the
of the matrix differential equation
right hand side of (20) is equivalent to (34). We find (20),
d however,moreaccessiblethan(34)inparticulartoresearchers
∆ =A∧(s ,...,s ,σ,...,0)∆ (27)
dσ i i 1 i−1 i inmultibodydynamicsemployingspatialvectornotation[13],
notacquaintedwithdifferentialgeometry.Analternativeproof
with ∆ evaluated at (s ,...,s ,σ) ∈ Ri with initial
i 1 i−1 of Proposition 4.1 could be obtained by showing that when
condition
(34)isidenticallyzero(flatnessoftheconnection)thisimplies
the existence of a function BF such that (23) holds. We
∆ (s ,...,s ,0)=∆ (s ,...,s ) (28)
i 1 i−1 i−1 1 i−1
are not aware, though, of an accessible source where this is
and ∆ (0) = F(0) ∈ SE(3) arbitrary (the desired orientation clearlystatedasintheproofofthepropositionaboveandthis
1
of the centroidal frame for s=0). Note that d∆ /dσ in (27) why we though worth presenting a proof that requires a basic
i
equals∂∆ (s ,...,s ,σ)/∂s .Inthefollowing,weprovide knowledge of the velocity kinematics of SE(3) and standard
i 1 i−1 i
the proof that (23) holds for n =2. Proving (23) for n >2 resultsofcalculus(Schwartz’stheorem)tobeunderstood.The
J J
is a straightforward but tedious calculation that follows from closestwecangetisChapterII,Section9,oftheclassicaltext
thetechniqueusedwhenn =2.GivenF(s):=∆ (s ,s ),it [30], which clearly requires a deep knowledge on differential
J 2 1 2
is immediate to see one must have ∂F/∂s (s)=A∧(s)F(s). geometry to be fully understood. For sake of completeness,
2 2
Showing ∂F/∂s (s)=A∧(s)F(s) is, instead, more involved we also mention that (24) could be replaced with the second
1 1
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,
(cid:18)∂A 1 (cid:19)∧
i(s)+ A (s)×A (s) F(s), (35)
∂s 2 i j
j
which leads then again to (20). Principal connections have Fig.1. Thefree-floatingthreelinkmodel.Inthisfigure,s1ands2represent
therelativeorientationsofthetwodistallinkswithrespecttothebase.See
been also employed to study nonholonomic locomotion, as maintextforafulldescriptionofthefigure.
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. V. ANUMERICALEXAMPLE
In this section, a simple example to illustrate the use of the
integrabilitycondition(20)isgiven.Weconsideramechanism
C. The link between locked and average velocity withtwointernalDOFs.ThisistheminimalnumberofDOFs
to observe the nonintegrability, because, for one DOF, (20) is
Inthissubsection,weelaboratefurtherontheremarkgiven
always trivially satisfied.
in the subsection IV-A showing that, when choosing Ao =
C We numerically integrate (17), performing a motion that
Ap , the centroidal kinematic is simply given by
com starts and ends at the same internal joint configuration. The
base link will not, in general, return to the original pose. The
Ao˙ =Ap˙ (36)
C com centroidal frame will always return to the original orientation
AR˙C =Aωl∧ocARC (37) 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
with Aωloc given bythe angularvelocity componentof Avloc. will return to its original position.
We first show that, independently from where AoC is located, An illustration of the mechanism is given in Figure 1. The
Gvave and Avloc satisfy mechanism is composed by three rigid bodies: a free-floating
base link (yellow) and two distal links (cyan and magenta).
(cid:20)Ap˙ (cid:21)
com =Gv =GX Av . (38) The distal links are connected directly to the base via two
Aω ave A loc
loc independently actuated revolute joints. For both links, the
offset between their center of mass and joint axis is identical
This shows that the average angular velocity Gω coin-
ave and denoted with d. To each body we firmly attach three
cides with the locked angular velocity Aω . Proving (38)
loc coordinate frames, indicated as B, 1 and 2 in the figure, each
is obtained by employing the remark in the subsection IV-A
centeredatthecorrespondingbody’sCoM.Thebaselinkmass
regarding the invariance of the coordinates of the CoM and
is 1 kg. Each distal link mass is also 1 kg. For the base link,
the following lemma.
the rotational inertia (about the axis passing through the CoM
Lemma. Given the differential equation and orthogonal to the base link face) is 4 kg m2. For distal
links,theinertiais1kgm2.Therotationalinertiawithrespect
AH˙C =Av∧AHC (39) to the other directions are non influential (we are considering
a planar mechanism) and can be assigned arbitrarily finding
assume there is a point p such that its coordinates Cp with the same result provided below.
respect to C are constant. Define G =(p,[A]) so that G has We verify (20) for two different values of d: namely, for
p as origin and the same orientation of A. Then, the velocity d = 1 and d = 0. For d = 1, B = −B is equal, up to a
12 21
Av written with respect to G equals division by the factor (2C +6S −6S −28)2, to
1−2 1 2
Gv=GX Av=(cid:20)Ap˙(cid:21) (40)  2(C1+C2)(4C1+4C2−3C1S2+3C2S1) 
A Aω 2C1(4S1+4S2−3S1S2−3S2S2)+2C2(4S1+4S2+3S1S2+3S1S1)
 0 
where Aω denotes the angular velocity component of Av. (cid:4)  0 
 
 0 
Finally,toobtain(36)-(37),weemploy(38)toexpressAvloc −18S1−2−24C1−24C2
in terms of Gvave and substitute it into (17), obtaining where S1 := sin(s1), S2 := sin(s2), C1 := cos(s1), C2 :=
cos(s ), S := sin(s −s ), and C := cos(s −s ).
2 1−2 1 2 1−2 1 2
Ao˙C =Ap˙com+Aωl∧oc(AoC −Apcom) (41) For d=0, B12 =B21 ≡0. Details of the straightforward but
AR˙ =Aω∧ AR . (42) tedious computations are not provided for space limitations.
C loc C
The conclusion is that, just for d = 0, the integration of
where we recall AH = (AR ,Ao ;0 ,1) ∈ SE(3). As the average angular velocity will always produce a centroidal
C C C 1×3
we have selected Ao =Ap , the result follows. framewhoseorientationisonlyafunctionoftheinternaljoint
C com
7
displacements. This is confirmed by the animation snapshots APPENDIX
given in Figure 2 corresponding to two simulations for dif-
A. The momentum map and the total momentum
ferent values of d. The complete animation is available as a
multimediaattachmenttothispaper.Forbothcases,thejoints Noether’s theorem can be employed to conclude that, when
follow the sinusoidal trajectories given by no gravity and external forces are applied, the momentum AJ
(cid:18) (cid:18) (cid:19) (cid:19) (cid:18) (cid:19) givenby(9)isconstant.Inthecontextofgeometricmechanics,
3π 2π π 2π
s (t)= cos t −1 , s (t)= sin t , (9) is the momentum map J defined as
1 2 T 2 2 T
(cid:104)J(q,q˙),ξ(cid:105)=FL(q,q˙)·ξ (q) (43)
starting and ending in a mechanism configuration with the Q
distallinksinaverticalposition.Theseresultsareindependent where FL(q,q˙) · z := lim (L(q,q˙ + tz) − L(q,q˙))/t
t→0
of the particular initial pose and velocity of the base and
denotes the fiber derivative of the Lagrangian in the direction
therefore, in Figure 2, only the relative pose of the centroidal
z ∈ T Q and ξ (q) the infinitesimal generator of the group
q Q
frame with respect the base link is shown.
action formally defined as ξ (q) = d/dt| Φ (q). The
Q t=0 exp(tξ)
function Φ is the action of the symmetry group on the
g
VI. CONCLUSIONANDDISCUSSION
configuration space: in the context of this paper, g ∈ SE(3),
In this paper we have established a clear link between the configuration space Q = SE(3) × RnJ, and the group
the concept of average angular velocity found in the robotics action Φ :Q→Q is simply given by
g
and multibody dynamics literature and the concept of locked
velocity from the geometric mechanics literature. We have Φg(H,s)=(gH,s), (44)
provided an accessible definition and proof on the key al-
corresponding to a rigid transformation of the entire robot ac-
gebraic condition that can be used to establish when the
cordingtog thatleavesinvarianttheshapes.Theinfinitesimal
centroidalframe(inparticular,itsorientation)dependsonlyon
generator associated to (44) is therefore
the current robot configuration and not on its time evolution.
Forsystemswithasmallnumberoflinks,theconditioncould ξ (q)=(ξ∧H,0)∈T Q (45)
Q (H,s)
be checked symbolically and, for more complex mechanism,
we expect efficient algorithms could be easily developed as and after straightforward computations one gets that the mo-
just the differentiation of the matrices L and A, typically mentum map equals (9) as the right hand side of (43) is
involved in the computation of the Coriolis’ matrix associated
(cid:20)v(cid:21)T (cid:20) L(s) A(s)(cid:21)(cid:20)BX ξ(cid:21)
to the dynamics, is required. Computationally, our experience FL(q,q˙)·ξ (q)= A . (46)
Q s˙ AT(s) S(s) 0
suggeststocomputethelockedvelocitywithrespecttheframe
whoseoriginistheCoMandorientationgivenbythebaselink
For the reader that is familiar with Lie group theory, note
frame. The advantage of using this frame is that the locked that, in(46), BX =Ad . Formore detailson momentum
A H−1
inertia matrix becomes diagonal, still depending only on the
maps and related concepts, we refer the interested reader to
shape variables.
[15, Chapter 11] and [14].
Thecentroidalframeisalwaysintegrablewhendealingwith
asystempossessingonlyoneinternaldegreeoffreedom.Note
how this result would remain valid for a mechanism where B. The center of mass is always a fixed point
virtual constraints are employed to ensure that this happens, Inthisappendix,weprovethatthecenterofmassisalways
making the internal degrees of freedom algebraically related a fixed with respect to AH obtained by the time integration
C
to a single variable that acts as the only internal degree of of (17). Requiting that the CoM to be fixed with respect to
freedom(see,e.g.,theconceptofgatevariablein[33]andthe the frame C is equivalent to ask that
work that stemmed from it in employing virtual constraints in
the context of robot locomotion [34]). Our presentation might Ap¯˙com=Avl∧ocAp¯com (47)
thereforehelpintakingadifferentperspectivetothoseresults.
where Ap¯ are the homogeneous coordinates of p with
We have shown that mechanisms with two internal degrees com com
respect to A obtained by appending 1 to the standard coor-
of freedom can also possess an integrable centroidal frame,
dinates Ap , i.e., Ap¯ := (Ap ;1) where ; denotes
butthisisnotguaranteedascommonexperienceandprevious com com com
row concatenation. The proof of this fact derives from a
investigations (e.g., the so called falling cat problem) have
straightforward manipulation of the expression of the time
already shown. It might be interesting to understand if there
derivative of the identity Ap¯ = AH Cp¯ assuming
are some rules in constructing a (nontrivial) mechanism such com C com
Cp to be a constant. The right hand side of (47) can be
thattheintegrabilityissatisfied.Thecentroidalframeforthose com
expressed with respect to frame the frame G = (p ,[A])
mechanism might provide an interesting natural output to use com
obtaining the equivalent condition
incontrollingthegrossmotionofthesystemsbothinposition
and in orientation. Even when dealing with a mechanism Ap¯˙ =AH Gv∧ Gp¯ (48)
com G loc com
where the centroidal frame is not integrable, one idea could
be to try to assign the external wrenches so as to guarantee where Gp¯ =(0,0,0,1)T. Equation (48) is then equivalent
com
the integrability condition: one of the motivations to continue to Ap˙ =AR Gv and to Ap˙ =Gv , since AR =
com G loc com loc G
investigating the centroidal dynamics even further. I.Thislastconditionisalwaystruederivingdirectlyfromthe
8
t=0 s t=2 s t=4 s t=6 s t=8 s t=10 s
Fig.2. Evolutionofthecentroidalframe.Nonintegrablecasewithd=1(firstrow).Integrablecasewithd=0(secondrow)
fact that the momentum map J expressed in the G frame is [15] J.E.MarsdenandT.Ratiu,Introductiontomechanicsandsymmetry:a
given by basicexpositionofclassicalmechanicalsystems. SpringerScience&
BusinessMedia,2013,vol.17.
J(H,s,v,s˙)= XB L Bv [16] R. M. Murray, Z. Li, S. S. Sastry, and S. S. Sastry, A mathematical
G G B B loc introductiontoroboticmanipulation. CRCpress,1994.
= L Gv [17] F. C. Park, J. Bobrow, and S. R. Ploen, “A Lie group formulation of
G G loc
robot dynamics,” International Journal of Robotic Research, vol. 14,
where L is block diagonal with first block on the diagonal no.6,pp.609–618,1995.
G G
[18] S. Traversaro and A. Saccon, “Multibody Dynamics Notation,”
equal to mI with m the total mass and that the linear
3×3 Technische Universiteit Eindhoven, Tech. Rep., 2016. [Online].
momentum component of GJ is necessarily mAp˙com. Available:http://repository.tue.nl/849895
[19] T. De Laet, S. Bellens, R. Smits, E. Aertbelie¨n, H. Bruyninckx, and
J. De Schutter, “Geometric relations between rigid bodies (part 1):
REFERENCES Semanticsforstandardization,”Robotics&AutomationMagazine,IEEE,
vol.20,no.1,pp.84–93,2013.
[1] P. M. Wensing and D. E. Orin, “Improved computation of the [20] H. Bruyninckx and J. De Schutter, “Symbolic differentiation of the
humanoidcentroidaldynamicsandapplicationforwhole-bodycontrol,” velocitymappingforaserialkinematicchain,”Mechanismandmachine
International Journal of Humanoid Robotics, vol. 13, no. 01, p. theory,vol.31,no.2,pp.135–148,1996.
1550039, 2016. [Online]. Available: http://www.worldscientific.com/ [21] A. Jain, Robot and Multibody Dynamics: Analysis and Algorithms.
doi/abs/10.1142/S0219843615500395 Springer,2010.
[2] G. Nava, F. Romano, F. Nori, and D. Pucci, “Stability analysis and [22] B.Siciliano,L.Sciavicco,L.Villani,andG.Oriolo,Robotics:modelling,
design of momentum-based controllers for humanoid robots,” in 2016 planningandcontrol. SpringerScience&BusinessMedia,2010.
IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems, [23] S. Chiaverini, G. Oriolo, and I. D. Walker, “Kinematically redundant
Oct2016. manipulators,” in Springer handbook of robotics. Springer, 2008, pp.
[3] T. Koolen, S. Bertrand, G. Thomas, T. De Boer, T. Wu, J. Smith, 245–268.
J. Englsberger, and J. Pratt, “Design of a momentum-based control [24] P.-B.Wieber,R.Tedrake,andS.Kuindersma,“Modelingandcontrolof
framework and application to the humanoid robot atlas,” International leggedrobots,”inSpringerHandbookofRobotics,2ndEd,B.Siciliano
JournalofHumanoidRobotics,vol.13,2016. andO.Khatib,Eds.,2016.
[4] G. Garofalo, B. Henze, J. Englsberger, and C. Ott, “On the inertially [25] J.E.MarsdenandJ.Scheurle,“Thereducedeuler-lagrangeequations,”
decoupled structure of the floating base robot dynamics,” 8th Vienna FieldsInstituteComm,vol.1,pp.139–164,1993.
InternationalConferenceonMathematicalModelling(MATHMOD),pp. [26] K.R.Ball,D.V.Zenkov,andA.M.Bloch,“Variationalstructuresfor
322–327,2015. hamel’sequationsandstabilization,”IFACWorkshoponLagrangianand
[5] H.Dai,A.Valenzuela,andR.Tedrake,“Whole-bodymotionplanning HamiltonianMethodsforNonLinearControl,pp.178–183,2012.
with centroidal dynamics and full kinematics,” in 2014 IEEE-RAS [27] D. E. Orin and A. Goswami, “Centroidal momentum matrix of a hu-
InternationalConferenceonHumanoidRobots,2014,pp.295–302. manoidrobot:Structureandproperties,”in2008IEEE/RSJInternational
[6] C.Ott,M.A.Roa,andG.Hirzinger,“Postureandbalancecontrolfor ConferenceonIntelligentRobotsandSystems. IEEE,2008.
bipedrobotsbasedoncontactforceoptimization,”inHumanoidRobots [28] X. Ding and H. Chen, “Dynamic modeling and locomotion control
(Humanoids),201111thIEEE-RASInternationalConferenceon. IEEE, for quadruped robots based on center of inertia on SE(3),” Journal of
2011,pp.26–33. DynamicSystems,Measurement,andControl,vol.138,no.1,2016.
[7] S.-H.LeeandA.Goswami,“Amomentum-basedbalancecontrollerfor [29] P.Wieber,“Holonomyandnonholonomyinthedynamicsofarticulated
humanoidrobotsonnon-levelandnon-stationaryground,”Autonomous motion,” in Fast Motions in Biomechanics and Robotics. Optimization
Robots,vol.33,no.4,pp.399–414,2012. andFeedbackControl,M.DiehlandK.Mombaur,Eds. Springer,2005.
[8] S.Kajita,F.Kanehiro,K.Kaneko,F.Fujiwara,K.Harada,K.Yokoi,and [30] S.KobayashiandK.Nomizu,Foundationsofdifferentialgeometry(Vol.
H.Hirukawa,“Resolvedmomentumcontrol:Humanoidmotionplanning I),2nded. Wiley,1996.
based on the linear and angular momentum,” in IEEE International [31] A.Saccon,J.Hauser,andA.P.Aguiar,“Optimalcontrolonliegroups:
ConferenceonIntelligentRobotsandSystems,2003,pp.1644–1650. The projection operator approach,” IEEE Transactions on Automatic
[9] D. E. Orin, A. Goswami, and S.-H. Lee, “Centroidal dynamics of a Control,vol.58,no.9,pp.2230–2245,2013.
humanoid robot,” Auton. Robots, vol. 35, no. 2-3, pp. 161–176, Jun. [32] J. Ostrowski and J. Burdick, “The geometric mechanics of undulatory
2013. robotic locomotion,” The International Journal of Robotics Research,
[10] J. E. Marsden, Lectures on Mechanics. Cambridge University Press, vol.17,pp.683–701,1998.
1992. [33] J.W.Grizzle,G.Abba,andF.Plestan,“Asymptoticallystablewalking
[11] A.M.Bloch,P.Krishnaprasad,J.E.Marsden,andR.M.Murray,“Non- for biped robots: analysis via systems with impulse effects,” IEEE
holonomic mechanical systems with symmetry,” Archive for Rational TransactionsonAutomaticControl,vol.46,no.1,pp.51–64,2001.
MechanicsandAnalysis,vol.136,no.1,pp.21–99,1996. [34] J.W.Grizzle,C.Chevallereau,R.W.Sinnet,andA.D.Ames,“Models,
[12] R.Featherstone,Rigidbodydynamicsalgorithms. Springer,2008. feedback control, and open problems of 3D bipedal robotic walking,”
[13] R.FeatherstoneandD.E.Orin,“Dynamics,”inSpringerHandbookof Automatica,vol.50,no.8,pp.1955–1988,2014.
Robotics,2ndEd,B.SicilianoandO.Khatib,Eds.,2016.
[14] A.Bloch,NonholonomicMechanicsandControl,withthecollaboration
ofJ.Ballieul,P.Crouch,andJ.Marsden. Springer-Verlag,2003.