A Lie group formulation of Kinematics and

Dynamics of serial Manipulators

Course Project Report

16-741:Mechanics of Manipulation, Spring 2006

Anish K Mampetta
Robotics Institute
Carnegie Mellon University
Pittsburgh, PA 15213
[email protected]
1 Introduction

As the complexity of multibody system increases, the need for more elegant formulation
of the equations of motion becomes an issue of paramount importance. For example, it is
desirable to have an explicit representation of the equations of motion that can be manip-
ulated at a high-level and in which the kinematic and dynamic parameters of the system
appear in a transparent manner. Moreover, many application in robot control and planning
require a set of dynamic equations that can be defined explicitly with respect to parameter
of interest.

It is also desirable to have the equations of motion in a form which can be implemented
effectively using a computer for the purpose of simulation. In this report, the techniques
of Lie group is used to derive explicit equations of motion. These equations of motion
can be arranged in a recursive form for serial link manipulators. The resulting recursive
formulation is particularly suitable for computer implementation.

2 Background

2.1 Lie group and Lie algebra

Figure 1: Basic structure of a Lie group

A Lie Group G is a set of elements embodying simultaneously the properties of a group and
a smooth manifold. By the group property, (1) composition of any two elements g1, g2 G
is defined, yielding a composite element g that also belong to G; (2)identity, there exist a el-
ement e in G such that composition of e with any other element g yields the element g itself,
and (3) for any element g in G there exist an inverse g 1 such that the composition of g and
g 1 yields the identity e. By the manifold property, any two points in G can be connected
by a smooth trajectory, and at any point g one can define a differential dg that is tangent to
G. The differential at the neutral element (identity) is particularly important. This tangent
space at the identity is called the Lie algebra for that group. The Lie algebra along with a bi-
linear map called Lie bracket, forms a vector space. The liebracket [., .], satisfies (1) Skew-
symmetry: [x, y] = [x, y]. (2) Jacobi identity: [x, [y, z]] + [z, [x, y]] + [y, [z, x]] = 0 for
every x,y,z in the associated Lie algebra. The primary connection between a Lie group
and its Lie algebra is the exponential mapping. On matrix groups the exponential mapping
corresponds to the ordinary matrix exponential, i.e, if A is an element of the Lie algebra,
then exp(A) is an element of the Lie group.

2.2 Special Orthogonal and Euclidean groups, SO(3) and SE(3)

Transformations on orientation in Euclidian space are accomplished by the use of the ro-
tation matrix , which is an element of the special Orthogonal group, SO(3), which is a
Lie group (because it satisfies the properties of group and manifold mentioned above). The
rigid body transformation, cast in 4x4 homogeneous form, is also a Lie group referred as
the special Euclidian group or SE(3). Give the rotation SO(3) and translation b R3 ,
the SE(3) is give as

" #
b
(1)
0 1

The Lie algebra associated with the Lie group SO(3), denoted by so(3), can be determined
by evaluating the tangent vector to a smooth curve (t) on SO(3) where (0) = I , the
identity. Differentiating both sides of (t)(t)T = I with respect to t and evaluating at
t = 0 results in (0) + (0)T = 0 [put dots]. Therefore the Lie algebra of SO(3), denoted
by so(3), consists of a set of skew symmetric matrices on R3x3 of the form

0 z y

[] =
z 0 x
(2)
y x 0

where = (x , y , z ) R3 and [.] is an operation (cross operator)which change the

three dimensional vector into the associated skew symmetric matrix.

In a similar fashion, it can be shown that the Lie algebra associated with SE(3), denoted
se(3), consists of 4x4 matrix of the form

" #
[] v
(3)
0 1

where R3 and v R3 . We can also express g as a 6 dimensional vector, as

6
g = (w, v) R .

2.3 Spatial velocities and forces

The spatial velocity of a rigid body, expressed in a moving frame (often referred to as
body-fixed reference frame) can be expressed as the pair (, v). If X(t) = ((t), b(t)) be
a curve in SE(3) describing the motion of the rigid body, the spatial velocity of the body,
expressed in the moving frame is an element of se(3). That is, (, v) se(3)
While spatial velocities are represented with elements of se(3), spatial forces, which con-
sists of a moment-force pair, are regarded as inhabiting the dual space of se(3), denoted as
se(3)*. This distinction can be traced for to the fact that forces (which behave as covec-
tors) transform differently under a change of coordinates than velocity (which behave as
tangent vectors). As a result, force and velocities can be thought of as belonging to the
dual space of velocities and angular velocities. A spatial force represented in screw form,
g = (M, F) se(3) , is often called wrench.

2.4 Adjoint operators

The Adjoint map is a coordinate transformation on se(3). This allows us to transform spatial
velocities from one reference frame to another via the SE(3) map between the two frames.
The Adjoint map, denoted by Ad, is given by

AdG (h) = GhG1 (4)

where, G SE(3) represents a transformation and h se(3) represents the spatial ve-
locity. For example, if V2 = (w2 , v2 ) denote the spatial velocity of a rigid body in frame
M2 and T1,2 SE(3) represent the coordinate map from frame M2 to frame M1 then
the spatial velocity of the rigid body in frame M1 , denoted by V1 = (w1 , v1 ), is given by
V1 = AdT1,2 (V2 ). Note that in equation (4), h is in the 4x4 matrix form. It can be shown
that if h is expressed as the 6 dimensional vector form, the expression for Adjoint of h is

" #" #
0 h
AdG (h) = (5)
[b] vh

The dual Adjoint operator, denoted as Ad* is a linear mapping on the dual space, se(3)*.
The mapping AdG (h ) : se(3) se(3) is given by

Adg (h) = G1 h G (6)

for G SE(3) and h se(3) . An equivalent form when h* is expressed in 6 dimen-

sional vector form is

" #" #
T T [b]T h
AdG (h ) = (7)
0 T vh

Notice that the matrix used for Ad* is the transpose of the one used for the Ad operator.
Physically, the dual Adjoint map is a coordinate transformation on se(3)*. This allows us
to transform spatial forces from one reference frame to another via the SE(3) map between
the two frames. For example, if F2 = (M2 , F2 ) se(3) denote the spatial force acting
on a rigid body with respect to frame M2 and T1,2 SE(3) represent the coordinate map
from frame M2 to frame M1 then the spatial force acting on rigid body expressed in frame
M1 , denoted by F1 = (M1 , F1 ), is given by F1 = AdT1,2 (F2 ).

The Lie algebra can also be used as a linear mapping on itself. This is the Lie bracket
operation discussed earlier. We will denote this operation by ad. It has the form:

adg (h) = [g, h] = gh hg, (8)

for g, h se(3) and g,h in the 4x4 matrix form of (3). The equivalent form for the 6-vector
representation of h is

" #" #
[g ] 0 h
adg (h) = (9)
[vg ] [g ] vh

Similarly, the dual operator, ad* is given by

ad g (h ) = [g, h ] = gh h g, (10)

or by the equivalent form:

" T T
#" #
[g ] [vg ] h
adg (h ) = T
(11)
0 [g ] vh

Physically, the mapping adg (h) and mapping adg (h) generalizes the standard cross product
operation to se(3) and se(3)*.

3 Kinematics: using product of matrix exponential

The kinematics of an open chain can be modeled as a sequence of homogeneous trans-

formations between consecutive joint frames. Let the transformation which describe the
motion between the frame i and the frame of link i-1 be Ti1,i SE(3). A series of se-
quential matrix transformations between adjacent joint frames can be combined as:

Ti,j = Ti,i+1 Ti+1,i+2 ...Tj1,j (12)

The homogeneous transformation Ti,i+1 can be written as

Ti,i+1 = Mi eSi qi (13)

where, Mi is the coordinate transformation between link i and link i-1, Si se(3) is the
joint screw written in the body-fixed frame for link i and qi se(3) is the current position
of join i relative to a specified zero position.

Expressing the link to link transformations in exponential form has an added advantage over
other representations like the Denavit-Hartenberg, in that the joint motion for prismatic,
revolute and screw joints are treated in a uniform way. Moreover, it is trivial to differentiate
eSi qi with respect to qi se(3) which makes if far less cumbersome to derive the sensitivity
equation with our approach.

Note1: Sensitivity equations are derivatives of motion equation with respect to the motion
parameters. They are essential for solving optimal control problems.

Note2: The exponential map is closely related to the screw representation of rigid body
motions. A screw consists of an axis S (i.e., directed line in space), a pitch h, and a
magnitude M. A screw motion is a rotation about S by M followed by a translation of
amount hM along S.

4 Dynamics

Using the vector form of spatial velocity, Newton-Euler equation can be expresses [4] as,

F = J V adV (JV ) (14)

where" ad* #is defined in (7), and V the spatial velocity is expressed as the pair " vector,
#
M
V = , whose origin is at the point of application of the applied load, F = ,
v F
and the spatial inertia J is

" #
I m[r]2 m[r]
J= (15)
m[r] m.1

In (14), the load can be applied at an arbitrary location on the body relative to its centre of
mass. The mass of the body is m and the inertia about the center of mass is I. The vector
from the point of application of the load to the center of mass is r.

The spatial form of the Newton-Euler equations can be applied to open kinematic chains
by summing the forces on each link of the chain and transforming them to the appropriate
coordinate system. The algorithm to solve for the dynamics[4] is given below. This algo-
rithm assumes that the motion (position, velocity and acceleration) for each joint is given
and solves for the unknown joint torques. It consists of two recursions - one outward from
the base to tip and one inward from the tip to base as follows:

Initialization
 Given : V0 = 0, V0 , Fn+1 (16)

Outward recursion: for i=1 to n do

Ti1,i = Mi eSi qi (17)

Vi = i)
AdT 1 i1,i (Vi1 ) + Si (q (18)
ai = adSi qi (AdT 1 i1,i (Vi1 ))
= adSi qi (Vi ) (19)
Vi = ) + S1 ddotqi + ai
AdT 1 i1,i (Vi1 (20)

Inward recursion: for i=n to 1 do

Fi = AdT 1 i1,i (Fi+1 ) + Ji Vi adVi (Ji Vi ) (21)

i = S T i Fi (22)

5 An example: Analysis of a 2R planar manipulator

The Dynamics of a two link planar robot using Lie group formulation is solved in [2].

Figure 2: 2R planar arm

The forward kinematic mapping is give by, Ti1,i = Mi eSi qi , For S = (, v) The expo-
nential mapping can be expressed as
" # ! " #
Sq
[] v exp([]q) b
e = exp q =
0 0 0 1

Where,
 exp([]q) = I + sin(q)[] + (1 cos(q))[]2 , and
b = (qI + (1 cos(q))[] + (q sin(q))[]2 )v

Since, we have only revolute joints, v = 0, hence b = 0. And since the arm is planar, we
have, Si = [0, 0, 1, 0, 0, 0]

For Link1, T0,1 = M1 eS1 q1 , S1 = [0, 0, 1, 0, 0, 0]

1 0 0 0

0 1 0 0
M1 =


0 0 1 0
0 0 0 1

cos(q1 ) sin(q1 ) 0 0

sin(q1 ) cos(q1 ) 0 0
T0,1 =



0 0 1 0
0 0 0 1

For Link2, T1,2 = M2 eS2 q2 , S2 = [0, 0, 1, 0, 0, 0]

1 0 0 L1

0 1 0 0
M2 =



0 0 1 0
0 0 0 1

cos(q2 ) sin(q2 ) 0 0

sin(q2 ) cos(q2 ) 0 0
T1,2 =



0 0 1 0
0 0 0 1

The mapping between the base and frame 2 is given by T0,2 = T0,1 T1,2

We can calculate the Adjoints for the Links from (5). As T = (, b), We have,
1 T T
T = ( , b)

cos(q1 ) sin(q1 ) 0 0 0 0

sin(q1 ) cos(q1 ) 0 0 0 0


0 0 1 0 0 0
AdT 1 =



0,1
0 0 0 cos(q1 ) sin(q1 ) 0

0 0 0 sin(q1 ) cos(q1 ) 0

0 0 0 0 0 1

and,

cos(q2 ) sin(q2 ) 0 0 0 0

sin(q2 ) cos(q2 ) 0 0
0 0


0 0 1 0 0 0
AdT 1 =



0,1
0 0 L1 sin(q2 ) cos(q2 ) sin(q2 ) 0

0 0 L1 cos(q2 ) sin(q2 ) cos(q2 ) 0

0 L1 0 0 0 1

We can calculate the velocities from (18)

V1 = [0, 0, q1 , 0, 0, 0]T ,
V2 = [0, 0, q1 + q1 , L1 sin(q2 )q1 , L1 cos(q2 )q1 , 0]T

Using (9)

0 q1 0 0 0 0

q1 0 0 0 0 0


0 0 0 0 0 0
adS1 q1 =



0 0 0 0 q1 0

0 0 0 q1 0 0

0 0 0 0 0 0

0 q2 0 0 0 0

q2 0 0 0 0 0


0 0 0 0 0 0
adS2 q2 =



0 0 0 0 q2 0

0 0 0 q2 0 0

0 0 0 0 0 0

Using(11)

0 q1 0 0 0 0

q1 0 0 0 0 0


0 0 0 0 0 0
adV1 =



0 0 0 0 q1 0

0 0 0 q1 0 0

0 0 0 0 0 0

and, adV2 =

0 (q1 + q2 ) 0 0 0 1)
L1 cos(q2 )(q

q2 + q2 0 0 0 0 1)
L1 sin(q2 )(q

1 ) L1 sin(q2 )(q
1)
0 0 0 L1 cos(q2 )(q 0


0 0 0 0 (q1 + q2 ) 0

0 0 0 (q1 + q2 ) 0 0

0 0 0 0 0 0
The J matrices for the system are

0 0 0 0 0 0

0 m1 a2 0 0 0 m1 a1
1

0 0 Izz1 + m1 a21 0 m1 a1 0
J1 =


0 0 0 m1 0 0

0 0 m1 a1 0 m1 0

0 m1 a1 0 0 0 m1

0 0 0 0 0 0

0 m2 a2 0 0 0 m2 a2
2

0 0 Izz2 + m2 a22 0 m2 a2 0
J2 =


0 0 0 m2 0 0

0 0 m2 a2 0 m2 0

0 m2 a2 0 0 0 m2

Where, m1 , m2 are the mass of Link1 and Link2 respectively and a1 , a2 are the distance
from the local reference to the centre of mass and Izz is the moment of inertia along the
z-axis.

The spatial forces can be calculated by using(21), and from (22) we get the joint torques.
Under gravity and in the absence of any external force, the joint torques are,

1 = (m1 a1 + m2 L2 )gcos(q1 ) + m2 ga2 cos(q1 + q2 )

2 = m2 ga2 cos(q1 + q2 )

6 Conclusion

An explicit representation of the equations of motion of a serial manipulator is presented

using the concepts of Lie group. The equations obtained can be manipulated at a high level
and can be expressed in a recursive form, which can be used to implement efficient algo-
rithms. The equations of motion for a 2R planar manipulator is derived as a demonstration
of the technique.

Reference
[1] Garett A. Sohl and James E. Bobrow, A Recursive Multibody Dynamics and Sensitivity Algo-
rithm for Branched Kinematic Chain, Journal of Dynamic Systems, Measurement, and Control, 2001,
ASME.
[2] Scott R. Ploen, Geometric Algorithms for the Dynamics and Control of Multibody Systems,
dissertation, Doctor of Philosophy, Mechanical and Aerospace, University of California, Irvine, 1997.
[3] Andres Kecskemethy, Lie-group first order operations in Rigid-body Kinematics, University of
Duisburg, Essen, Germany.
[4] F.C Park, J.E Bobrow, and S.R Ploen, A lie group formulation of robot dynamics, International
Journal of Robotics Research, 14 (1995), no. 6, 609-618