We provide a tutorial and review of the state-of-the-art in robot dynamics algorithms that rely on methods from differential geometry, particularly the theory of Lie groups. After reviewing the underlying Lie group structure of the rigid-body motions and the geometric formulation of the equations of motion for a single rigid body, we show how classical screw-theoretic concepts can be expressed in a reference frame-invariant way using Lie-theoretic concepts and derive recursive algorithms for the forward and inverse dynamics and their differentiation. These algorithms are extended to robots subject to closed-loop and other constraints, joints driven by variable stiffness actuators, and also to the modeling of contact between rigid bodies. We conclude with a demonstration of how the geometric formulations and algorithms can be effectively used for robot motion optimization.

# Geometric Algorithms for Robot Dynamics: A Tutorial Review OPEN ACCESS

**Frank C. Park**

**Beobkyoon Kim**,

**Cheongjae Jang**,

**Jisoo Hong**

Manuscript received April 25, 2017; final manuscript received December 12, 2017; published online February 7, 2018. Editor: Harry Dankowicz.

*Appl. Mech. Rev*70(1), 010803 (Feb 07, 2018) (18 pages) Paper No: AMR-17-1029; doi: 10.1115/1.4039078 History: Received April 25, 2017; Revised December 12, 2017

Robotics is defined by motion—controlled programmable motion—and that mechanics, particularly multibody dynamics, playing a fundamental role in robotics should come as no surprise. A typical robot is constructed by connecting rigid bodies, called links, together with joints. Actuators such as electric motors then deliver forces and torques to the joints, enabling the robot to move. Dynamic models of the robot—by this we mean the equations of motion relating the applied forces and torques to the motion of the robot—are needed for a wide range of applications, from the design of model-based control laws and trajectory generation algorithms to the simulation, design optimization, and motion planning of robots.

Because rigid-body dynamics is a classical and well-understood subject, there is a tendency to regard robot dynamics as a solved problem. Although at some level this may be true, what makes the subject of robot dynamics challenging and still an open area of research is that robots increasingly have high-dimensional, very complex structures. A typical humanoid robot, for example, can possess up to 60 degrees-of-freedom (DOFs) and intermittently form closed chains and be subject to contact and other constraints during its motion. Some of the robot's links and joints may purposefully contain elastic (“soft”) elements like variable stiffness springs and dampers, which may either be passive or actively controlled. The actuators themselves may possess complicated dynamics involving gears and various transmission devices. Model-based control laws often require real-time calculation of the dynamics. The high-dimensional and nonlinear nature of the equations of motion, together with the need to account for contact and other physical constraints, all are factors that make the formulation and computation of the robot dynamics less than straightforward.

This paper attempts to provide a review of the state-of-the-art in robot dynamics algorithms. The literature on robot dynamics is vast, and a review of the earlier foundational works can be found in a number of sources, e.g., see Ref. [1]. Our focus in this paper will be limited to those formulations and algorithms that rely on methods from differential geometry, particularly the theory of Lie groups.

This focus on geometry is motivated by the observation that often the most salient physical features of a robot are best captured by a geometric description. The advantages of the geometric approach have been recognized for quite some time by practitioners of classical screw theory. Featherstone (see Ref. [2] and the earlier references cited therein) showed how the dynamics of rigid multibody systems can be formulated by using and extending constructs from classical screw theory. The articulated body inertia algorithm by Featherstone [3] is one of the remarkable achievements of this machinery.

What has made these tools largely inaccessible to practitioners is that they require an entirely new language of notations and constructs (screws, twists, wrenches, reciprocity, transversality, conjugacy, etc.) and their often obscure rules for manipulation and transformation. On the other hand, the mostly algebraic alternatives to screw theory often mean the practitioner ends up buried in the details of calculation, losing the simple and intuitive interpretation that lies at the heart of what they are calculating.

The geometric framework described in this paper has its roots in the Lie bracket-based formulation of the attitude dynamics equation for a rotating rigid body [4] and allows one to express screw-theoretic concepts as well as Featherstone's constructs using the language of Lie groups and Lie algebras. By exploiting the Lie group structure of the rigid-body motions, it becomes possible to express the kinematics of an open chain robot as a product of matrix exponentials—this remarkable result was first derived by Brockett [5]; some of its many powerful applications are described in Ref. [6]—allowing one to reinvent screw theory simply by appealing to basic notions from linear algebra and linear differential equations.

In fact, the geometric framework allows one to go much further. The resulting algorithms, together with the formulations upon which they are based, can be expressed in a way that is invariant with respect to the choice of link reference frames (coordinate invariance). Moreover, by drawing upon more general results from Lie theory, new algorithms, such as a recursive formulation for gradients and Hessians of the dynamics, which are essential to motion optimization, can be straightforwardly obtained.

Rather than attempting a comprehensive review of geometric dynamics algorithms for the entire spectrum of robot designs, our objective will be to describe in some detail the underlying geometric concepts for a single rigid body and then to derive the basic forward and inverse dynamics algorithms for open chains. Summary descriptions and references are then provided for the corresponding formulations and algorithms of more complex robots, including those subject to closed-loop and other constraints and robots containing soft elements. We then show how these geometric algorithms can be used effectively for robot motion optimization.

We will not explore those aspects of geometric mechanics that do not relate in an explicit way to the algorithmic and computational aspects of robot dynamics; we instead refer the reader to many excellent references on geometric mechanics and control such as Refs. [4] and [7–9]. Papers that explore in more detail the geometric aspects of the algorithmic constructs in this paper will be pointed out throughout.

This paper is organized as follows: After reviewing the Lie group structure of the rotation and Euclidean groups and their connections to velocities and forces, we then review in detail the geometric formulation of the equations of motion for a single rigid body; these equations form the basis for the ensuing formulations and algorithms for the forward and inverse dynamics of open chain structures and also for their derivatives. We also examine from the geometric perspective the dynamics of rigid bodies in contact with each other. The corresponding algorithms for robots subject to constraints, such as closed chains, and robots containing soft elements, are then reviewed. The remainder of this paper is then devoted to robot motion optimization, which employs in a fundamental way the geometric algorithms for the dynamics and also their differentiation. We conclude with a brief discussion of some of the open research issues in robot dynamics.

We begin with a motivating example that illustrates the advantages of the geometric approach to robot dynamics. Consider a single rigid body of mass $m$ with a body-fixed frame *x*-*y*-*z* attached to the body's center of mass; unless stated otherwise, all vector and matrix quantities mentioned later are expressed in terms of this body frame. Let $I\u2208\mathbb{R}3\xd73$ be the 3 × 3 inertia matrix of the body, and let $f\u2208\mathbb{R}3$ and $m\u2208\mathbb{R}3$, respectively, denote the external force and moment applied to the body (here *m* is applied about the center of mass). The equations of motion are then given by the familiar pair of equations

where $v\u2208\mathbb{R}3$ and $a\u2208\mathbb{R}3$ are, respectively, the velocity and acceleration of the center of mass, and $\omega \u2208\mathbb{R}3$ and $\alpha \u2208\mathbb{R}3$ are, respectively, the angular velocity and angular acceleration.

The previously mentioned dynamic equations are expressed in terms of the body frame attached to the center of mass. Let us now see how the equations would appear if a different body frame was chosen. Affix the $x\u2032\n-y\u2032\n-z\u2032$ frame to another point on the body; to simplify calculations, we will even assume that the orientation of this frame is identical to that of the *x*-*y*-*z* frame. The external force and moment expressed in the new frame are $f\u2032$ and $m\u2032$. The velocities and accelerations of this new frame are now represented by primes: $\omega \u2032,\u2009v\u2032,\u2009\alpha \u2032$, and $a\u2032$, all of which are vectors expressed in the new frame. Finally, let $p\u2032$ denote the vector from the new frame origin to the center of mass, also expressed in the new frame. The equations of motion now become

The derivation of these equations is quite standard and can be found in standard dynamics texts, e.g., see Ref. [10].

What is immediately apparent is that the equations become considerably more complex with respect to this new reference frame. This feature is precisely what is behind the complexity of multibody robot dynamics: the forces and moments acting on a specific link are typically expressed in different reference frames, and these must be expressed in terms of a common frame before they can be summed.

What we show later in this section is that by applying the basic geometric Lie group constructs to the special Euclidean group of rigid-body motions, the dynamics for a single rigid body can, regardless of the choice of reference frame, always be expressed in the form

where $V\u2208\mathbb{R}6$ is a *twist* representing the linear and angular velocity of the rigid body and $V\u02d9\u2208\mathbb{R}6$ its time derivative, $F\u2208\mathbb{R}6$ is a *wrench* representing the force and moment applied to the rigid body, $G\u2208\mathbb{R}6\xd76$ is a *spatial inertia matrix*, and $adV\u2208\mathbb{R}6\xd76$ is a linear twist transformation matrix and $adVT$ its transpose. There exist well-defined transformation rules for twists, wrenches, and spatial inertias under a change of reference frame. Equation (1) forms the basis for the geometrical formulations and algorithms that follow. We now review the fundamental concepts and constructs for deriving Eq. (1).

This section covers the fundamentals of matrix Lie groups at a level targeted to the practitioner; for a more rigorous discussion and review of Lie groups and their properties, see, e.g., Refs. [6] and [11–14]. Let GL(*n*), referred to as the *general linear group*, denote the set of real *n* × *n* nonsingular matrices. A subset **G** of GL(*n*) is a *matrix Lie group* if it has the structure of both a differentiable manifold (imagine a smooth multidimensional surface embedded in a higher-dimensional Euclidean space) and an algebraic group under matrix multiplication

^{(1)}The identity matrix $\n1$ is an element of**G**.^{(2)}For every*X*∈**G**, its inverse*X*^{−1}exists and is also an element of**G**.

GL(*n*) itself is a matrix Lie group: it is a differentiable manifold of dimension *n*^{2} and also satisfies the properties of an algebraic group under matrix multiplication. The set of rotation matrices and also the set of rigid-body motions (or homogeneous transformations) are examples of matrix Lie groups that we will treat in detail later. The *special linear group* SL(*n*), consisting of real *n* × *n* matrices of unit determinant, is also a matrix Lie group.

A *Lie algebra* is a vector space $V$ along with a mapping called the *Lie bracket*, $[\xb7,\xb7]:V\xd7V\u2192V$, that satisfies for all scalars *α*, *β* and all elements *A*, *B*, *C* in $V$

^{(1)}[*αA + βB, C*] =*α*[*A*,*C*] +*β*[*B*,*C*] and [*C*,*αA*+*βB*] =*α*[*C*,*A*] +*β*[*C*,*B*];^{(2)}[*A*,*B*] = –[*B*,*A*];^{(3)}[*A*, [*B, C*]] + [*C*, [*A*,*B*]] + [*B*, [*C*,*A*]] = 0.

As an example of a Lie algebra, the set of real *n* × *n* matrices, denoted gl(*n*), is a matrix Lie algebra under the Lie bracket defined by the *matrix commutator*

*A*,

*B*∈ gl(

*n*).

Any matrix Lie group gives rise to a matrix Lie algebra, and conversely, to any matrix Lie algebra, there is a unique corresponding matrix Lie group. The *matrix exponential* makes explicit this connection between matrix Lie groups and Lie algebras. For any real *n* × *n* matrix *A* ∈ gl(*n*), its matrix exponential *e ^{A}* is defined by the series

The series is always guaranteed to converge for finite *A*. The matrix exponential satisfies the following properties:

^{(1)}*e*is always nonsingular with inverse given by^{A}*e*^{−}. The exponential map thus maps^{A}*A*∈ gl(*n*) into*e*∈ GL(^{A}*n*).^{(2)}$ePAP\u22121=PeAP\u22121$ for any nonsingular $P\u2208\mathbb{R}n\xd7n$.^{(3)}$(d/dt)eAt=AeAt=eAtA$ and $det(eA)=etr(A)$.^{(4)}For any*A*,*B*∈ gl(*n*),*e*=^{A}e^{B}*e*, where^{C}*C*∈ gl(*n*) is of the formDisplay Formula (4)$C=A+B+12[A,B]+112{[[B,A],A]+[[B,A],B]}+\cdots $

If **G** is a matrix Lie group, its corresponding matrix Lie algebra, denoted **g**, is defined to be the set of all *n* × *n* real matrices whose exponential is an element of **G**. More precisely

**g**is the

*tangent space*to

**G**at the identity element. That is, let

*X*(

*t*) be a differentiable curve on

**G**such that

*X*(0) = $I$. Then $X\u02d9(0)$, the derivative of

*X*(

*t*) at

*t*= 0, is a tangent vector to

**G**at $\n1$. The collection of all such vectors $X\u02d9(0)$, obtained as the derivative of all possible differentiable curves

*X*(

*t*) on

*G*such that

*X*(0) = $I$, then constitutes the Lie algebra

**g**.

**g**is a vector space—a vector subspace of gl(

*n*), to be precise—and the exponential map $exp\u2009:g\u2192G$ is defined from the Lie algebra to the Lie group. As an example, the Lie algebra of GL(

*n*) is gl(

*n*). The Lie algebra of the special linear group SL(

*n*) is the set of

*n*×

*n*real matrices with trace zero, denoted sl(

*n*).

Having defined the Lie algebra of a Lie group, we now define a pair of mappings that play a central role in describing how various physical quantities that arise in multibody dynamics transform under a change of reference frames. Given an element *A* ∈ **g** of the Lie algebra, the *small adjoint map* ad* _{A}*:

**g**→

**g**is simply the Lie bracket

Given some element *X* ∈ **G** of the Lie group, the *large adjoint map* Ad* _{X}*:

**g**→

**g**is defined as

If *X* ∈ GL(*n*) and *A* ∈ gl(*n*) satisfy *e ^{A}* =

*X*, then

*A*is said to be a

*matrix logarithm*of

*X*. The matrix logarithm, or matrix log for short, satisfies $log\u2009eA=A$ and $e\u2009log\u2009X=X$. The matrix log need not be unique, but if

*X*is restricted to some sufficiently small neighborhood of the identity, then the matrix log is indeed unique and defined by the convergent series

If *X* is now restricted to be an element of some matrix Lie group **G**, then over some neighborhood $N$ of the identity element of **G**, the matrix log is uniquely defined and acts as a set of local coordinates for $N\u2286G$. That is, let {Ω_{1},…, Ω* _{N}*} be a basis for

**g**. The map $exp\u2009:\mathbb{R}N\u2192N$ that takes

*q*for $N$, with the matrix log as its inverse. Local coordinates constructed in this fashion are referred to as

*canonical coordinates of the first kind*[14]. Another way to construct local coordinates for $N$ is via the map

Such coordinates are called *canonical coordinates of the second kind*. It is not always necessary for the {Ω_{1},…, Ω* _{N}*} to form a basis in the case of canonical coordinates of the second kind.

For any group element *X* ∈ **G**, the *left translation* map *L _{X}*:

**G**→

**G**is defined as

Since $LX\u22121X=X\u22121X=\n1$, the derivative of $LX\u22121$ at *X* is a linear mapping from the tangent space *T _{X}*

**G**to $T\n1G=g$ (see Fig. 1). Given a differentiable curve

*X*(

*t*) on

**G**with velocity $X\u02d9(t)$, the

*differential*of $LX\u22121$, denoted $dLX\u22121(X):TXG\u2192g$ and also denoted as the

*left differential*at

*X*, maps the tangent vector $X\u02d9$ to an element of

**g**as follows:

The notion of the *right-translation* map and *right differential* can also be defined analogously: *R _{X}*(

*Y*) =

*Y X*, and

The left- and right-translation maps provide two natural ways of mapping any tangent vector $X\u02d9\u2208TXG$ to the Lie algebra **g**: via the differential of the left translation map as $X\u22121X\u02d9$ or the differential of the right-translation map as $X\u02d9X\u22121$. If *A*(*t*) ∈ **g** is differentiable, and *X*(*t*) = *e ^{A}*

^{(}

^{t}^{)}∈

**G**, then it can be shown that

The left and right differential maps offer a natural way of constructing a *Riemannian metric* on **G**—a Riemannian metric on a manifold $M$ is a smoothly varying family of inner products defined at each tangent space to $M$—simply by constructing an inner product on the Lie algebra **g**. Let *X*(*t*) denote a smooth curve on **G** with velocity $X\u02d9(t)$. Then both $X\u22121X\u02d9$ and $X\u02d9X\u22121$ are elements of **g**, which recall also is a vector space. By choosing an inner product $\u2329\xb7,\xb7\u232a\n1$ on **g**, an inner product $\u2329\xb7,\xb7\u232aX$ on the tangent space *T _{X}*

**G**can be constructed as

The above is a *
left-invariant Riemannian metric*: it is left-invariant in the sense that if the original curve *X*(*t*) is now multiplied from the left by some constant *T* ∈ **G**, to *Y* (*t*) = *TX*(*t*), then

That is, the resulting inner product is invariant with respect to left translations by *T*. The *
right-invariant Riemannian metric* is, following an analogous construction but this time replacing $X\u22121X\u02d9$ by $X\u02d9X\u22121$, invariant with respect to right translations by *T*.

For the left- and right-invariant Riemannian metrics defined earlier, the *minimal geodesics*, i.e., minimum-length paths between two arbitrary endpoints on **G**, can be determined by solving an appropriate optimal control problem. To obtain minimal geodesics with respect to the left-invariant metric, given endpoints *X*(0) and *X*(1) on **G**, we seek the trajectory *U*(*t*) ∈ **g**, *t* ∈ [0, 1] that minimizes

where $\u2329\xb7,\xb7\u232a$ denotes an inner product on **g**. (If minimal geodesics with respect to the right-invariant Riemannian metric are desired, the corresponding differential equation would be expressed as $X\u02d9=UX$.) Choosing an inner product on **g** of the form

*U*(

*t*) and

*X*(

*t*) must satisfy the differential equations

*X*(0) and

*X*(1) given, leading to a classical two-point boundary value problem. In the event that $Q=\n1$, the second equation simplifies to $U\u02d9=[UT,U]$.

The solution *X*(*t*) to Eqs. (19) and (20) represents a minimum velocity curve on SE(3). Minimum acceleration curves can be found by minimizing the objective function

*X*(0), $X\u02d9(0)$,

*X*(1), and $X\u02d9(1)$ given. For the choice $Q=\n1$ for the metric on

**g**, it can be shown that the optimal

*U*(

*t*) and

*X*(

*t*) must then satisfy the following differential equations:

The set of 3 × 3 rotation matrices forms the *special orthogonal group* SO(3)

SO(3) is a three-dimensional Lie group under matrix multiplication and describes all the possible orientations of a rigid body in physical space. The Lie algebra of SO(3), denoted so(3), consists of the real 3 × 3 skew-symmetric matrices

Since any element of so(3) has precisely three independent components, so(3) can be identified with $\mathbb{R}3$. The following notation is used: given $\omega =(\omega 1,\omega 2,\omega 3)T\u2208\mathbb{R}3$, its skew-symmetric representation will be denoted as

For any [*ω*], [*v*] ∈ so(3) (or $\omega ,v\u2208\mathbb{R}3$), and *R* ∈ SO(3), the Lie bracket [⋅,⋅], small adjoint ad* _{ω}*: so(3) → so(3), and large adjoint Ad

*: so(3) → so(3) are*

_{R}As is well-known from classical kinematics, any orientation can be achieved by rotating a frame about some axis $\omega \u0302$ by some specified angle *θ*. The axis-angle formula is an explicit representation of this property that can be expressed through the matrix exponential operator. Given $[\omega \u0302]\theta \u2208so(3)$ such that $\theta \u2208\mathbb{R}$ and $\Vert \omega \u0302\Vert =1$, the exponential map $exp\u2009:so(3)\u2192SO(3)$ can be expressed in closed-form as follows:

A closed-form formula for the matrix log on SO(3) is given by

*R*such that tr(

*R*) ≠ −1. The formula in the general case is covered in, e.g., Ref. [15]. Other mathematical representation for rotations, e.g., Euler angles (which correspond to canonical coordinates of the second kind on SO(3)), Cayley–Rodrigues parameters, unit quaternions, etc., are discussed in Refs. [6] and [15–18].

We now examine the angular velocity of a rigid body from the Lie group perspective. Suppose a rigid body is rotating in space with angular velocity **w**. Let {s} denote the fixed (or space) frame, and {b} denote the moving (or body) frame attached to the rotating rigid body. The orientation of {b} relative to {s} is then described by the rotation *R* ∈ SO(3). Let $\omega s\u2208\mathbb{R}3$ be the column vector representation of **w** in {s} frame coordinates. *ω _{s}* is then related to

*R*as follows:

**w**in {b} frame coordinates, then from Eq. (29) it follows that

Let *R* ∈ SO(3) and $p\u2208\mathbb{R}3$ be the orientation and position of the rigid body relative to the fixed frame. The *special Euclidean group* SE(3), which we shall refer to more simply as the Euclidean group, consists of 4 × 4 real matrices of the form

where *R* ∈ SO(3) and $p\u2208\mathbb{R}3$ (the 0 in the last row denotes a three-dimensional row vector of zeros). For convenience, we shall at times also write (*R*, *p*) ∈ SE(3). SE(3) is a six-dimensional Lie group under matrix multiplication and describes the set of all possible relative displacements between two reference frames situated in physical space.

The Lie algebra of SE(3), denoted se(3), consists of 4 × 4 matrices of the form

where [*ω*] ∈ so(3) and $v\u2208\mathbb{R}3$. $S$ will also be expressed in six-dimensional column vector form as $S=(\omega T,vT)T$, or more conveniently, $S=(\omega ,v)$.

The Lie bracket of two elements $S1=(\omega 1,v1),\u2009S2=(\omega 2,v2)$ can be evaluated as follows:

The above can be written more compactly using the small adjoint map $adS1(S2)$

Given *T* = (*R*, *p*) ∈ SE(3) and $S=(\omega ,v)\u2208se(3)$, the large adjoint map Ad* _{T}*: se(3) → se(3) is

or $AdT(S)=(R\omega ,Rv+p\xd7R\omega )$. The notation [Ad* _{T}*] will also be used to denote the 6 × 6 matrix

In what follows, $[\omega \u0302]\u2208so(3)$ denotes a vector of unit length, i.e., $\Vert \omega \u0302\Vert =1$. Given $S\theta =(\omega \u0302\theta ,v\theta )\u2208se(3)$, $\theta \u2208\mathbb{R}$, the closed-form formula for the exponential map $exp\u2009:se(3)\u2192SE(3)$ is given by

where $e[\omega \u0302]\theta $ is given by Eq. (30) and

The matrix log is uniquely defined over some neighborhood $N\u2282SE(3)$ of the identity