Review Article

Geometric Algorithms for Robot Dynamics: A Tutorial Review OPEN ACCESS

[+] Author and Article Information
Frank C. Park

Department of Mechanical and
Aerospace Engineering,
Seoul National University,
Seoul 08826, Korea
e-mail: fcp@snu.ac.kr

Beobkyoon Kim, Cheongjae Jang, Jisoo Hong

Department of Mechanical and
Aerospace Engineering,
Seoul National University,
Seoul 08826, Korea

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

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.


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 [79]. 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 I3×3 be the 3 × 3 inertia matrix of the body, and let f3 and m3, 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 v3 and a3 are, respectively, the velocity and acceleration of the center of mass, and ω3 and α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 -y -z 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 and m. The velocities and accelerations of this new frame are now represented by primes: ω,v,α, and a, all of which are vectors expressed in the new frame. Finally, let p 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 formDisplay Formula


where V6 is a twist representing the linear and angular velocity of the rigid body and V˙6 its time derivative, F6 is a wrench representing the force and moment applied to the rigid body, G6×6 is a spatial inertia matrix, and adV6×6 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).

Matrix Lie Group Fundamentals.

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 [1114]. 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. (1)The identity matrix 1 is an element of G.
  2. (2)For every XG, 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 n2 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, [·,·]:V×VV, that satisfies for all scalars α, β and all elements A, B, C in V

  1. (1)[αA + βB, C] = α[A, C] + β[B, C] and [C, αA + βB] = α[C, A] + β[C, B];
  2. (2)[A, B] = –[B, A];
  3. (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 commutatorDisplay Formula

for 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 eA is defined by the seriesDisplay Formula

(3)eA= 1+A+12!A2+13!A3+

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

  1. (1)eA is always nonsingular with inverse given by eA. The exponential map thus maps A ∈ gl(n) into eA ∈ GL(n).
  2. (2)ePAP1=PeAP1 for any nonsingular Pn×n.
  3. (3)(d/dt)eAt=AeAt=eAtA and det(eA)=etr(A).
  4. (4)For any A, B ∈ gl(n), eAeB = eC, where C ∈ gl(n) is of the formDisplay Formula

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 preciselyDisplay Formula

with the Lie bracket taken to be the matrix Lie commutator. Equivalently, 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˙(0), the derivative of X(t) at t = 0, is a tangent vector to G at 1. The collection of all such vectors X˙(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:gG 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 Ag of the Lie algebra, the small adjoint map adA: gg is simply the Lie bracketDisplay Formula


Given some element XG of the Lie group, the large adjoint map AdX: gg is defined asDisplay Formula


If X ∈ GL(n) and A ∈ gl(n) satisfy eA = X, then A is said to be a matrix logarithm of X. The matrix logarithm, or matrix log for short, satisfies logeA=A and elogX=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 Display Formula

(8)logX=(X 1)12(X 1)2+13(X 1)3

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 NG. That is, let {Ω1,…, ΩN} be a basis for g. The map exp:NN that takesDisplay Formula

then defines a set of local coordinates 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 mapDisplay Formula

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 XG, the left translation map LX: GG is defined asDisplay Formula


Since LX1X=X1X= 1, the derivative of LX1 at X is a linear mapping from the tangent space TXG to T 1G=g (see Fig. 1). Given a differentiable curve X(t) on G with velocity X˙(t), the differential of LX1, denoted dLX1(X):TXGg and also denoted as the left differential at X, maps the tangent vector X˙ to an element of g as follows:Display Formula


The notion of the right-translation map and right differential can also be defined analogously: RX(Y) = Y X, andDisplay Formula


The left- and right-translation maps provide two natural ways of mapping any tangent vector X˙TXG to the Lie algebra g: via the differential of the left translation map as X1X˙ or the differential of the right-translation map as X˙X1. If A(t) ∈ g is differentiable, and X(t) = eA(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˙(t). Then both X1X˙ and X˙X1 are elements of g, which recall also is a vector space. By choosing an inner product ·,· 1 on g, an inner product ·,·X on the tangent space TXG can be constructed asDisplay Formula

(14)X˙,X˙X=X1X˙,X1X˙ 1

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 TG, to Y (t) = TX(t), thenDisplay Formula


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 X1X˙ by X˙X1, 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 minimizesDisplay Formula


subject to the matrix differential equationDisplay Formula


where ·,· 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˙=UX.) Choosing an inner product on g of the formDisplay Formula

for some symmetric positive-definite Qn×n, the optimal U(t) and X(t) must satisfy the differential equationsDisplay Formula
Display Formula
with boundary conditions X(0) and X(1) given, leading to a classical two-point boundary value problem. In the event that Q= 1, the second equation simplifies to U˙=[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 functionDisplay Formula

subject to the matrix differential equation (17) with boundary conditions X(0), X˙(0), X(1), and X˙(1) given. For the choice Q= 1 for the metric on g, it can be shown that the optimal U(t) and X(t) must then satisfy the following differential equations:Display Formula
Display Formula

The Rotation Group.

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

(24)SO(3)={R3×3|RTR= 1,det R=1}

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 matricesDisplay Formula


Since any element of so(3) has precisely three independent components, so(3) can be identified with 3. The following notation is used: given ω=(ω1,ω2,ω3)T3, its skew-symmetric representation will be denoted asDisplay Formula


For any [ω], [v] ∈ so(3) (or ω,v3), and R ∈ SO(3), the Lie bracket [⋅,⋅], small adjoint adω: so(3) → so(3), and large adjoint AdR: so(3) → so(3) areDisplay Formula

Display Formula
Display Formula

Equation (29) can also be written in the vector form AdR(ω) = .

As is well-known from classical kinematics, any orientation can be achieved by rotating a frame about some axis ω̂ 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 [ω̂]θso(3) such that θ and ω̂=1, the exponential map exp:so(3)SO(3) can be expressed in closed-form as follows:Display Formula

(30)e[ω̂]θ= 1+sinθ[ω̂]+(1cosθ)[ω̂]2

The above formula is also known as the Rodrigues rotation formula.

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

Display Formula
for any 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 [1518].

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 ωs3 be the column vector representation of w in {s} frame coordinates. ωs is then related to R as follows:Display Formula

which corresponds to the differential of the right-translation map. If ωb3 is the column vector representation of w in {b} frame coordinates, then from Eq. (29) it follows that Display Formula
which corresponds to the differential of the left translation map.

The Euclidean Group.

Let R ∈ SO(3) and p3 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 formDisplay Formula


where R ∈ SO(3) and p3 (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 formDisplay Formula


where [ω] ∈ so(3) and v3. S will also be expressed in six-dimensional column vector form as S=(ωT,vT)T, or more conveniently, S=(ω,v).

The Lie bracket of two elements S1=(ω1,v1),S2=(ω2,v2) can be evaluated as follows:Display Formula


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


or adS1(S2)=(ω1×ω2,ω1×v2ω2×v1).

Given T = (R, p) ∈ SE(3) and S=(ω,v)se(3), the large adjoint map AdT: se(3) → se(3) isDisplay Formula


In vector form, the above can also be written asDisplay Formula


or AdT(S)=(Rω,Rv+p×Rω). The notation [AdT] will also be used to denote the 6 × 6 matrixDisplay Formula


In what follows, [ω̂]so(3) denotes a vector of unit length, i.e., ω̂=1. Given Sθ=(ω̂θ,vθ)se(3), θ, the closed-form formula for the exponential map exp:se(3)SE(3) is given byDisplay Formula


where e[ω̂]θ is given by Eq. (30) andDisplay Formula

(43)G(θ)= 1θ+(1cosθ)[ω̂]+(θsinθ)[ω̂]2

The matrix log is uniquely defined over some neighborhood NSE(3) of the identityDisplay Formula